MNE-CPP  0.1.9
A Framework for Electrophysiology
icp.cpp
Go to the documentation of this file.
1 //=============================================================================================================
35 //=============================================================================================================
36 // INCLUDES
37 //=============================================================================================================
38 
39 #include "icp.h"
40 #include <iostream>
41 
42 #include "fiff/fiff_coord_trans.h"
44 
45 //=============================================================================================================
46 // QT INCLUDES
47 //=============================================================================================================
48 
49 #include <QSharedPointer>
50 #include <QDebug>
51 
52 //=============================================================================================================
53 // EIGEN INCLUDES
54 //=============================================================================================================
55 
56 #include <Eigen/Core>
57 #include <Eigen/Dense>
58 #include <Eigen/Eigenvalues>
59 #include <Eigen/Geometry>
60 
61 //=============================================================================================================
62 // USED NAMESPACES
63 //=============================================================================================================
64 
65 using namespace MNELIB;
66 using namespace RTPROCESSINGLIB;
67 using namespace Eigen;
68 using namespace FIFFLIB;
69 
70 //=============================================================================================================
71 // DEFINE GLOBAL METHODS
72 //=============================================================================================================
73 
74 bool RTPROCESSINGLIB::performIcp(const MNEProjectToSurface::SPtr mneSurfacePoints,
75  const Eigen::MatrixXf& matPointCloud,
76  FiffCoordTrans& transFromTo,
77  float& fRMSE,
78  bool bScale,
79  int iMaxIter,
80  float fTol,
81  const VectorXf& vecWeitgths)
87 {
88  if(matPointCloud.rows() == 0){
89  qWarning() << "[RTPROCESSINGLIB::icp] Passed point cloud is empty.";
90  return false;
91  }
92 
93  // Initialization
94  int iNP = matPointCloud.rows(); // The number of points
95  float fMSEPrev = 0.0f;
96  float fMSE = 0.0f; // The mean square error
97  float fScale = 1.0f;
98  MatrixXf matP0 = matPointCloud; // Initial Set of points
99  MatrixXf matPk = matP0; // Transformed Set of points
100  MatrixXf matYk(matPk.rows(),matPk.cols()); // Iterative closest points on the surface
101  MatrixXf matDiff = matYk;
102  VectorXf vecSE(matDiff.rows());
103  Matrix4f matTrans; // the transformation matrix
104  VectorXi vecNearest; // Triangle of the new point
105  VectorXf vecDist; // The Distance between matX and matP
106 
107  // Initial transformation - From point cloud To surface
108  FiffCoordTrans transICP = transFromTo;
109  matPk = transICP.apply_trans(matPk);
110 
111  // Icp algorithm:
112  for(int iIter = 0; iIter < iMaxIter; ++iIter) {
113 
114  // Step a: compute the closest point on the surface; eq 29
115  if(!mneSurfacePoints->mne_find_closest_on_surface(matPk, iNP, matYk, vecNearest, vecDist)) {
116  qWarning() << "[RTPROCESSINGLIB::icp] mne_find_closest_on_surface was not sucessfull.";
117  return false;
118  }
119 
120  // Step b: compute the registration; eq 30
121  if(!fitMatchedPoints(matP0, matYk, matTrans, fScale, bScale, vecWeitgths)) {
122  qWarning() << "[RTPROCESSINGLIB::icp] point cloud registration not succesfull";
123  }
124 
125  // Step c: apply registration
126  transICP.trans = matTrans;
127  matPk = transICP.apply_trans(matP0);
128 
129  // step d: compute mean-square-error and terminate if below fTol
130  vecDist = vecDist.cwiseProduct(vecDist);
131  fMSE = vecDist.sum() / iNP;
132  fRMSE = std::sqrt(fMSE);
133 
134  if(std::sqrt(std::fabs(fMSE - fMSEPrev)) < fTol) {
135  transFromTo = transICP;
136  qInfo() << "[RTPROCESSINGLIB::icp] ICP was succesfull and exceeded after " << iIter +1 << " Iterations with RMSE dist: " << fRMSE * 1000 << " mm.";
137  return true;
138  }
139  fMSEPrev = fMSE;
140  qInfo() << "[RTPROCESSINGLIB::icp] ICP iteration " << iIter + 1 << " with RMSE: " << fRMSE * 1000 << " mm.";
141  }
142  transFromTo = transICP;
143 
144  qWarning() << "[RTPROCESSINGLIB::icp] Maximum number of " << iMaxIter << " Iterations exceeded with RMSE: " << fRMSE * 1000 << " mm.";
145  return true;
146 }
147 
148 //=============================================================================================================
149 
150 bool RTPROCESSINGLIB::fitMatchedPoints(const MatrixXf& matSrcPoint,
151  const MatrixXf& matDstPoint,
152  Eigen::Matrix4f& matTrans,
153  float fScale,
154  bool bScale,
155  const VectorXf& vecWeitgths)
163 {
164  // init values
165  MatrixXf matP = matSrcPoint;
166  MatrixXf matX = matDstPoint;
167  VectorXf vecW = vecWeitgths;
168  VectorXf vecMuP; // column wise mean - center of mass
169  VectorXf vecMuX; // column wise mean - center of mass
170  MatrixXf matDot;
171  MatrixXf matSigmaPX; // cross-covariance
172  MatrixXf matAij; // Anti-Symmetric matrix
173  Vector3f vecDelta; // column vector, elements of matAij
174  Matrix4f matQ = Matrix4f::Identity(4,4);
175  Matrix3f matScale = Matrix3f::Identity(3,3); // scaling matrix
176  Matrix3f matRot = Matrix3f::Identity(3,3);
177  Vector3f vecTrans;
178  float fTrace = 0.0;
179  fScale = 1.0;
180 
181  // test size of point clouds
182  if(matSrcPoint.size() != matDstPoint.size()) {
183  qWarning() << "[RTPROCESSINGLIB::fitMatched] Point clouds do not match.";
184  return false;
185  }
186 
187  // get center of mass
188  if(vecWeitgths.isZero()) {
189  vecMuP = matP.colwise().mean(); // eq 23
190  vecMuX = matX.colwise().mean();
191  matDot = matP.transpose() * matX;
192  matDot = matDot / matP.rows();
193  } else {
194  vecW = vecWeitgths / vecWeitgths.sum();
195  vecMuP = vecW.transpose() * matP;
196  vecMuX = vecW.transpose() * matX;
197 
198  MatrixXf matXWeighted = matX;
199  for(int i = 0; i < (vecW.size()); ++i) {
200  matXWeighted.row(i) = matXWeighted.row(i) * vecW(i);
201  }
202  matDot = matP.transpose() * (matXWeighted);
203  }
204 
205  // get cross-covariance
206  matSigmaPX = matDot - (vecMuP * vecMuX.transpose()); // eq 24
207  matAij = matSigmaPX - matSigmaPX.transpose();
208  vecDelta(0) = matAij(1,2); vecDelta(1) = matAij(2,0); vecDelta(2) = matAij(0,1);
209  fTrace = matSigmaPX.trace();
210  matQ(0,0) = fTrace; // eq 25
211  matQ.block(0,1,1,3) = vecDelta.transpose();
212  matQ.block(1,0,3,1) = vecDelta;
213  matQ.block(1,1,3,3) = matSigmaPX + matSigmaPX.transpose() - fTrace * MatrixXf::Identity(3,3);
214 
215  // unit eigenvector coresponding to maximum eigenvalue of matQ is selected as optimal rotation quaterions q0,q1,q2,q3
216  SelfAdjointEigenSolver<MatrixXf> es(matQ);
217  Vector4f vecEigVec = es.eigenvectors().col(matQ.cols()-1); // only take last Eigen-Vector since this corresponds to the maximum Eigenvalue
218 
219  // quatRot(w,x,y,z)
220  Quaternionf quatRot(vecEigVec(0),vecEigVec(1),vecEigVec(2),vecEigVec(3));
221  quatRot.normalize();
222  matRot = quatRot.matrix();
223 
224  // get scaling factor and matrix
225  if(bScale) {
226  MatrixXf matDevX = matX.rowwise() - vecMuX.transpose();
227  MatrixXf matDevP = matP.rowwise() - vecMuP.transpose();
228  matDevX = matDevX.cwiseProduct(matDevX);
229  matDevP = matDevP.cwiseProduct(matDevP);
230 
231  if(!vecWeitgths.isZero()) {
232  for(int i = 0; i < (vecW.size()); ++i) {
233  matDevX.row(i) = matDevX.row(i) * vecW(i);
234  matDevP.row(i) = matDevP.row(i) * vecW(i);
235  }
236  }
237  // get scaling factor and set scaling matrix
238  fScale = std::sqrt(matDevX.sum() / matDevP.sum());
239  matScale *= fScale;
240  }
241 
242  // get translation and Rotation
243  vecTrans = vecMuX - fScale * matRot * vecMuP;
244  matRot *= matScale;
245 
246  matTrans.block<3,3>(0,0) = matRot;
247  matTrans.block<3,1>(0,3) = vecTrans;
248  matTrans(3,3) = 1.0f;
249  matTrans.block<1,3>(3,0) = MatrixXf::Zero(1,3);
250  return true;
251 }
252 
253 //=========================================================================================================
254 
255 bool RTPROCESSINGLIB::discard3DPointOutliers(const QSharedPointer<MNELIB::MNEProjectToSurface> mneSurfacePoints,
256  const MatrixXf& matPointCloud,
257  const FiffCoordTrans& transFromTo,
258  VectorXi& vecTake,
259  MatrixXf& matTakePoint,
260  float fMaxDist)
261 {
262  // Initialization
263  int iNP = matPointCloud.rows(); // The number of points
264  MatrixXf matP = matPointCloud; // Initial Set of points
265  MatrixXf matYk(matPointCloud.rows(),matPointCloud.cols()); // Iterative losest points on the surface
266  VectorXi vecNearest; // Triangle of the new point
267  VectorXf vecDist; // The Distance between matX and matP
268 
269  // Initial transformation - From point cloud To surface
270  matP = transFromTo.apply_trans(matP);
271 
272  int iDiscarded = 0;
273 
274  // discard outliers if necessary
275  if(fMaxDist > 0.0) {
276  if(!mneSurfacePoints->mne_find_closest_on_surface(matP, iNP, matYk, vecNearest, vecDist)) {
277  qWarning() << "[RTPROCESSINGLIB::icp] mne_find_closest_on_surface was not sucessfull.";
278  return false;
279  }
280 
281  for(int i = 0; i < vecDist.size(); ++i) {
282  if(std::fabs(vecDist(i)) < fMaxDist) {
283  vecTake.conservativeResize(vecTake.size()+1);
284  vecTake(vecTake.size()-1) = i;
285  matTakePoint.conservativeResize(matTakePoint.rows()+1,3);
286  matTakePoint.row(matTakePoint.rows()-1) = matPointCloud.row(i);
287  } else {
288  iDiscarded++;
289  }
290  }
291  }
292  qInfo() << "[RTPROCESSINGLIB::discardOutliers] " << iDiscarded << "digitizers discarded.";
293  return true;
294 }
295 
296 //=============================================================================================================
297 // DEFINE MEMBER METHODS
298 //=============================================================================================================
RTPROCESINGSHARED_EXPORT bool performIcp(const QSharedPointer< MNELIB::MNEProjectToSurface > mneSurfacePoints, const Eigen::MatrixXf &matPointCloud, FIFFLIB::FiffCoordTrans &transFromTo, float &fRMSE, bool bScale=false, int iMaxIter=20, float fTol=0.001, const Eigen::VectorXf &vecWeitgths=vecDefaultWeigths)
ICP declarations.
FiffCoordTrans class declaration.
Coordinate transformation description.
MNEProjectToSurface class declaration.
RTPROCESINGSHARED_EXPORT bool fitMatchedPoints(const Eigen::MatrixXf &matSrcPoint, const Eigen::MatrixXf &matDstPoint, Eigen::Matrix4f &matTrans, float fScale=1.0, bool bScale=false, const Eigen::VectorXf &vecWeitgths=vecDefaultWeigths)
Eigen::Matrix< float, 4, 4, Eigen::DontAlign > trans
RTPROCESINGSHARED_EXPORT bool discard3DPointOutliers(const QSharedPointer< MNELIB::MNEProjectToSurface > mneSurfacePoints, const Eigen::MatrixXf &matPointCloud, const FIFFLIB::FiffCoordTrans &transFromTo, Eigen::VectorXi &vecTake, Eigen::MatrixXf &matTakePoint, float fMaxDist=0.0)
Eigen::MatrixX3f apply_trans(const Eigen::MatrixX3f &rr, bool do_move=true) const
QSharedPointer< MNEProjectToSurface > SPtr