MNE-CPP  0.1.9
A Framework for Electrophysiology
hpifit.cpp
Go to the documentation of this file.
1 //=============================================================================================================
37 //=============================================================================================================
38 // INCLUDES
39 //=============================================================================================================
40 
41 #include "hpifit.h"
42 #include "hpifitdata.h"
43 #include "sensorset.h"
44 #include "hpidataupdater.h"
45 #include "signalmodel.h"
46 #include "hpimodelparameters.h"
47 
48 #include <utils/ioutils.h>
49 #include <utils/mnemath.h>
50 
51 #include <iostream>
52 #include <vector>
53 #include <numeric>
54 #include <fiff/fiff_cov.h>
56 #include <fstream>
57 
58 #include <fwd/fwd_coil_set.h>
59 
60 //=============================================================================================================
61 // EIGEN INCLUDES
62 //=============================================================================================================
63 
64 #include <Eigen/Dense>
65 
66 //=============================================================================================================
67 // QT INCLUDES
68 //=============================================================================================================
69 
70 #include <QVector>
71 #include <QFuture>
72 #include <QtConcurrent/QtConcurrent>
73 
74 //=============================================================================================================
75 // USED NAMESPACES
76 //=============================================================================================================
77 
78 using namespace Eigen;
79 using namespace INVERSELIB;
80 using namespace FIFFLIB;
81 using namespace FWDLIB;
82 
83 //=============================================================================================================
84 // DEFINE GLOBAL METHODS
85 //=============================================================================================================
86 
87 //=============================================================================================================
88 // DEFINE MEMBER METHODS
89 //=============================================================================================================
90 
91 //=============================================================================================================
92 
93 HPIFit::HPIFit(const SensorSet& sensorSet)
94  : m_sensors(sensorSet),
95  m_signalModel(SignalModel())
96 {
97 
98 }
99 
100 //=============================================================================================================
101 
102 void HPIFit::checkForUpdate(const SensorSet &sensorSet)
103 {
104  if(m_sensors != sensorSet) {
105  m_sensors = sensorSet;
106  }
107 }
108 
109 //=============================================================================================================
110 
111 void HPIFit::fit(const MatrixXd& matProjectedData,
112  const MatrixXd& matProjectors,
113  const HpiModelParameters& hpiModelParameters,
114  const MatrixXd& matCoilsHead,
115  HpiFitResult& hpiFitResult)
116 {
117  fit(matProjectedData,matProjectors,hpiModelParameters,matCoilsHead,false,hpiFitResult);
118 }
119 
120 //=============================================================================================================
121 
122 void HPIFit::fit(const MatrixXd& matProjectedData,
123  const MatrixXd& matProjectors,
124  const HpiModelParameters& hpiModelParameters,
125  const MatrixXd& matCoilsHead,
126  const bool bOrderFrequencies,
127  HpiFitResult& hpiFitResult)
128 {
129  if(matProjectedData.rows() != matProjectors.rows()) {
130  std::cout<< "HPIFit::fit - Projector and data dimensions do not match. Returning."<<std::endl;
131  return;
132  } else if(hpiModelParameters.iNHpiCoils()!= matCoilsHead.rows()) {
133  std::cout<< "HPIFit::fit - Number of coils and hpi digitizers do not match. Returning."<<std::endl;
134  return;
135  } else if(matProjectedData.rows()==0 || matProjectors.rows()==0) {
136  std::cout<< "HPIFit::fit - No data or Projectors passed. Returning."<<std::endl;
137  return;
138  } else if(m_sensors.ncoils() != matProjectedData.rows()) {
139  std::cout<< "HPIFit::fit - Number of channels in sensors and data do not match. Returning."<<std::endl;
140  return;
141  }
142 
143  const MatrixXd matAmplitudes = computeAmplitudes(matProjectedData,
144  hpiModelParameters);
145 
146  const MatrixXd matCoilsSeed = computeSeedPoints(matAmplitudes,
147  hpiFitResult.devHeadTrans,
148  hpiFitResult.errorDistances,
149  matCoilsHead);
150 
151  CoilParam fittedCoilParams = dipfit(matCoilsSeed,
152  m_sensors,
153  matAmplitudes,
154  hpiModelParameters.iNHpiCoils(),
155  matProjectors,
156  500,
157  1e-9f);
158 
159  if(bOrderFrequencies) {
160  const std::vector<int> vecOrder = findCoilOrder(fittedCoilParams.pos,
161  matCoilsHead);
162 
163  fittedCoilParams.pos = order(vecOrder,fittedCoilParams.pos);
164  hpiFitResult.hpiFreqs = order(vecOrder,hpiModelParameters.vecHpiFreqs());
165  }
166 
167  hpiFitResult.GoF = computeGoF(fittedCoilParams.dpfiterror);
168 
169  hpiFitResult.fittedCoils = getFittedPointSet(fittedCoilParams.pos);
170 
171  hpiFitResult.devHeadTrans = computeDeviceHeadTransformation(fittedCoilParams.pos,
172  matCoilsHead);
173 
174  hpiFitResult.errorDistances = computeEstimationError(fittedCoilParams.pos,
175  matCoilsHead,
176  hpiFitResult.devHeadTrans);
177 }
178 
179 //=============================================================================================================
180 
181 Eigen::MatrixXd HPIFit::computeAmplitudes(const Eigen::MatrixXd& matProjectedData,
182  const HpiModelParameters& hpiModelParameters)
183 {
184  // fit model
185  MatrixXd matTopo = m_signalModel.fitData(hpiModelParameters,matProjectedData);
186  matTopo.transposeInPlace();
187 
188  // split into sine and cosine amplitudes
189  const int iNumCoils = hpiModelParameters.iNHpiCoils();
190 
191  MatrixXd matAmpSine(matProjectedData.cols(), iNumCoils);
192  MatrixXd matAmpCosine(matProjectedData.cols(), iNumCoils);
193 
194  matAmpSine = matTopo.leftCols(iNumCoils);
195  matAmpCosine = matTopo.middleCols(iNumCoils,iNumCoils);
196 
197  // Select sine or cosine component depending on their contributions to the amplitudes
198  for(int j = 0; j < iNumCoils; ++j) {
199  float fNS = 0.0;
200  float fNC = 0.0;
201  fNS = matAmpSine.col(j).array().square().sum();
202  fNC = matAmpCosine.col(j).array().square().sum();
203  if(fNC > fNS) {
204  matAmpSine.col(j) = matAmpCosine.col(j);
205  }
206  }
207 
208  return matAmpSine;
209 }
210 
211 //=============================================================================================================
212 
213 Eigen::MatrixXd HPIFit::computeSeedPoints(const Eigen::MatrixXd& matAmplitudes,
214  const FIFFLIB::FiffCoordTrans& transDevHead,
215  const QVector<double>& vecError,
216  const Eigen::MatrixXd& matCoilsHead)
217 {
218  const int iNumCoils = matCoilsHead.rows();
219  MatrixXd matCoilsSeed = MatrixXd::Zero(iNumCoils,3);
220 
221  const double dError = std::accumulate(vecError.begin(), vecError.end(), .0) / vecError.size();
222 
223  if(transDevHead.trans != MatrixXd::Identity(4,4).cast<float>() && dError < 0.010) {
224  // if good last fit, use old trafo
225  matCoilsSeed = transDevHead.apply_inverse_trans(matCoilsHead.cast<float>()).cast<double>();
226  } else {
227  // if not, find max amplitudes in channels
228  VectorXi vecChIdcs(iNumCoils);
229 
230  for (int j = 0; j < iNumCoils; j++) {
231  int iChIdx = 0;
232  VectorXd::Index indMax;
233  matAmplitudes.col(j).maxCoeff(&indMax);
234  if(indMax < m_sensors.ncoils()) {
235  iChIdx = indMax;
236  }
237  vecChIdcs(j) = iChIdx;
238  }
239  // and go 3 cm inwards from max channels
240  for (int j = 0; j < vecChIdcs.rows(); ++j) {
241  if(vecChIdcs(j) < m_sensors.ncoils()) {
242  Vector3d r0 = m_sensors.r0(vecChIdcs(j));
243  Vector3d ez = m_sensors.ez(vecChIdcs(j));
244  matCoilsSeed.row(j) = (-1 * ez * 0.03 + r0);
245  }
246  }
247  }
248  return matCoilsSeed;
249 }
250 
251 //=============================================================================================================
252 
253 CoilParam HPIFit::dipfit(const MatrixXd matCoilsSeed,
254  const SensorSet& sensors,
255  const MatrixXd& matData,
256  const int iNumCoils,
257  const MatrixXd& matProjectors,
258  const int iMaxIterations,
259  const float fAbortError)
260 {
261  //Do this in conncurrent mode
262  //Generate QList structure which can be handled by the QConcurrent framework
263  QList<HPIFitData> lCoilData;
264 
265  for(qint32 i = 0; i < iNumCoils; ++i) {
266  HPIFitData coilData;
267  coilData.m_coilPos = matCoilsSeed.row(i);
268  coilData.m_sensorData = matData.col(i);
269  coilData.m_sensors = sensors;
270  coilData.m_matProjector = matProjectors;
271  coilData.m_iMaxIterations = iMaxIterations;
272  coilData.m_fAbortError = fAbortError;
273 
274  lCoilData.append(coilData);
275  }
276  //Do the concurrent filtering
277  CoilParam coil(iNumCoils);
278 
279  if(!lCoilData.isEmpty()) {
280  // //Do sequential
281  // for(int l = 0; l < lCoilData.size(); ++l) {
282  // doDipfitConcurrent(lCoilData[l]);
283  // }
284 
285  //Do concurrent
286  QFuture<void> future = QtConcurrent::map(lCoilData,
288  future.waitForFinished();
289 
290 
291  //Transform results to final coil information
292  for(qint32 i = 0; i < lCoilData.size(); ++i) {
293  coil.pos.row(i) = lCoilData.at(i).m_coilPos;
294  coil.mom = lCoilData.at(i).m_errorInfo.moment.transpose();
295  coil.dpfiterror(i) = lCoilData.at(i).m_errorInfo.error;
296  coil.dpfitnumitr(i) = lCoilData.at(i).m_errorInfo.numIterations;
297 
298  //std::cout<<std::endl<< "HPIFit::dipfit - Itr steps for coil " << i << " =" <<coil.dpfitnumitr(i);
299  }
300  }
301  return coil;
302 }
303 
304 //=============================================================================================================
305 
306 std::vector<int> HPIFit::findCoilOrder(const MatrixXd& matCoilsDev,
307  const MatrixXd& matCoilsHead)
308 {
309  // extract digitized and fitted coils
310  MatrixXd matCoilTemp = matCoilsDev;
311  const int iNumCoils = matCoilsDev.rows();
312 
313  std::vector<int> vecOrder(iNumCoils);
314  std::iota(vecOrder.begin(), vecOrder.end(), 0);;
315 
316  // maximum 10 mm mean error
317  const double dErrorMin = 0.010;
318  double dErrorActual = 0.0;
319  double dErrorBest = dErrorMin;
320 
321  MatrixXd matTrans(4,4);
322  std::vector<int> vecOrderBest = vecOrder;
323 
324  bool bSuccess = false;
325  // permutation
326  do {
327  for(int i = 0; i < iNumCoils; i++) {
328  matCoilTemp.row(i) = matCoilsDev.row(vecOrder[i]);
329  }
330  matTrans = computeTransformation(matCoilsHead,matCoilTemp);
331  dErrorActual = objectTrans(matCoilsHead,matCoilTemp,matTrans);
332  if(dErrorActual < dErrorMin && dErrorActual < dErrorBest) {
333  // exit
334  dErrorBest = dErrorActual;
335  vecOrderBest = vecOrder;
336  bSuccess = true;
337  }
338  } while (std::next_permutation(vecOrder.begin(), vecOrder.end()));
339  return vecOrderBest;
340 }
341 
342 //=============================================================================================================
343 
344 double HPIFit::objectTrans(const MatrixXd& matHeadCoil,
345  const MatrixXd& matCoil,
346  const MatrixXd& matTrans)
347 {
348  // Compute the fiducial registration error - the lower, the better.
349  const int iNumCoils = matHeadCoil.rows();
350  MatrixXd matTemp = matCoil;
351 
352  // homogeneous coordinates
353  matTemp.conservativeResize(matCoil.rows(),matCoil.cols()+1);
354  matTemp.block(0,3,iNumCoils,1).setOnes();
355  matTemp.transposeInPlace();
356 
357  // apply transformation
358  MatrixXd matTestPos = matTrans * matTemp;
359 
360  // remove
361  MatrixXd matDiff = matTestPos.block(0,0,3,iNumCoils) - matHeadCoil.transpose();
362  VectorXd vecError = matDiff.colwise().norm();
363 
364  // compute error
365  double dError = matDiff.colwise().norm().mean();;
366 
367  return dError;
368 }
369 
370 //=============================================================================================================
371 
372 Eigen::MatrixXd HPIFit::order(const std::vector<int>& vecOrder,
373  const Eigen::MatrixXd& matToOrder)
374 {
375  const int iNumCoils = vecOrder.size();
376  MatrixXd matToOrderTemp = matToOrder;
377 
378  for(int i = 0; i < iNumCoils; i++) {
379  matToOrderTemp.row(i) = matToOrder.row(vecOrder[i]);
380  }
381  return matToOrderTemp;
382 }
383 
384 //=============================================================================================================
385 
386 QVector<int> HPIFit::order(const std::vector<int>& vecOrder,
387  const QVector<int>& vecToOrder)
388 {
389  const int iNumCoils = vecOrder.size();
390  QVector<int> vecToOrderTemp = vecToOrder;
391 
392  for(int i = 0; i < iNumCoils; i++) {
393  vecToOrderTemp[i] = vecToOrder[vecOrder[i]];
394  }
395  return vecToOrderTemp;
396 }
397 
398 //=============================================================================================================
399 
400 Eigen::VectorXd HPIFit::computeGoF(const Eigen::VectorXd& vecDipFitError)
401 {
402  VectorXd vecGoF(vecDipFitError.size());
403  for(int i = 0; i < vecDipFitError.size(); ++i) {
404  vecGoF(i) = 1 - vecDipFitError(i);
405  }
406  return vecGoF;
407 }
408 
409 //=============================================================================================================
410 
411 FIFFLIB::FiffCoordTrans HPIFit::computeDeviceHeadTransformation(const Eigen::MatrixXd& matCoilsDev,
412  const Eigen::MatrixXd& matCoilsHead)
413 {
414  const MatrixXd matTrans = computeTransformation(matCoilsHead,matCoilsDev);
415  return FiffCoordTrans::make(1,4,matTrans.cast<float>(),true);
416 }
417 
418 //=============================================================================================================
419 
420 Eigen::Matrix4d HPIFit::computeTransformation(Eigen::MatrixXd matNH, MatrixXd matBT)
421 {
422  MatrixXd matXdiff, matYdiff, matZdiff, matC, matQ;
423  Matrix4d matTransFinal = Matrix4d::Identity(4,4);
424  Matrix4d matRot = Matrix4d::Zero(4,4);
425  Matrix4d matTrans = Matrix4d::Identity(4,4);
426  double dMeanX,dMeanY,dMeanZ,dNormf;
427 
428  for(int i = 0; i < 15; ++i) {
429  // Calculate mean translation for all points -> centroid of both data sets
430  matXdiff = matNH.col(0) - matBT.col(0);
431  matYdiff = matNH.col(1) - matBT.col(1);
432  matZdiff = matNH.col(2) - matBT.col(2);
433 
434  dMeanX = matXdiff.mean();
435  dMeanY = matYdiff.mean();
436  dMeanZ = matZdiff.mean();
437 
438  // Apply translation -> bring both data sets to the same center location
439  for (int j = 0; j < matBT.rows(); ++j) {
440  matBT(j,0) = matBT(j,0) + dMeanX;
441  matBT(j,1) = matBT(j,1) + dMeanY;
442  matBT(j,2) = matBT(j,2) + dMeanZ;
443  }
444 
445  // Estimate rotation component
446  matC = matBT.transpose() * matNH;
447 
448  JacobiSVD< MatrixXd > svd(matC ,Eigen::ComputeThinU | ComputeThinV);
449 
450  matQ = svd.matrixU() * svd.matrixV().transpose();
451 
452  //Handle special reflection case
453  if(matQ.determinant() < 0) {
454  matQ(0,2) = matQ(0,2) * -1;
455  matQ(1,2) = matQ(1,2) * -1;
456  matQ(2,2) = matQ(2,2) * -1;
457  }
458 
459  // Apply rotation on translated points
460  matBT = matBT * matQ;
461 
462  // Calculate GOF
463  dNormf = (matNH.transpose()-matBT.transpose()).norm();
464 
465  // Store rotation part to transformation matrix
466  matRot(3,3) = 1;
467  for(int j = 0; j < 3; ++j) {
468  for(int k = 0; k < 3; ++k) {
469  matRot(j,k) = matQ(k,j);
470  }
471  }
472 
473  // Store translation part to transformation matrix
474  matTrans(0,3) = dMeanX;
475  matTrans(1,3) = dMeanY;
476  matTrans(2,3) = dMeanZ;
477 
478  // Safe rotation and translation to final matrix for next iteration step
479  // This step is safe to do since we change one of the input point sets (matBT)
480  // ToDo: Replace this for loop with a least square solution process
481  matTransFinal = matRot * matTrans * matTransFinal;
482  }
483  return matTransFinal;
484 }
485 
486 //=============================================================================================================
487 
488 QVector<double> HPIFit::computeEstimationError(const Eigen::MatrixXd& matCoilsDev,
489  const Eigen::MatrixXd& matCoilsHead,
490  const FIFFLIB::FiffCoordTrans& transDevHead)
491 {
492  //Calculate Error
493  MatrixXd matTemp = matCoilsDev;
494  MatrixXd matTestPos = transDevHead.apply_trans(matTemp.cast<float>()).cast<double>();
495  MatrixXd matDiffPos = matTestPos - matCoilsHead;
496 
497  // compute error
498  int iNumCoils = matCoilsDev.rows();
499  QVector<double> vecError(iNumCoils);
500  for(int i = 0; i < matDiffPos.rows(); ++i) {
501  vecError[i] = matDiffPos.row(i).norm();
502  }
503  return vecError;
504 }
505 
506 //=============================================================================================================
507 
508 FIFFLIB::FiffDigPointSet HPIFit::getFittedPointSet(const Eigen::MatrixXd& matCoilsDev)
509 {
510  FiffDigPointSet fittedPointSet;
511  const int iNumCoils = matCoilsDev.rows();
512 
513  for(int i = 0; i < iNumCoils; ++i) {
514  FiffDigPoint digPoint;
515  digPoint.kind = FIFFV_POINT_EEG; //Store as EEG so they have a different color
516  digPoint.ident = i;
517  digPoint.r[0] = matCoilsDev(i,0);
518  digPoint.r[1] = matCoilsDev(i,1);
519  digPoint.r[2] = matCoilsDev(i,2);
520 
521  fittedPointSet << digPoint;
522  }
523  return fittedPointSet;
524 }
525 
526 //=============================================================================================================
527 
528 void HPIFit::storeHeadPosition(float fTime,
529  const Eigen::MatrixXf& transDevHead,
530  Eigen::MatrixXd& matPosition,
531  const Eigen::VectorXd& vecGoF,
532  const QVector<double>& vecError)
533 
534 {
535  Matrix3f matRot = transDevHead.block(0,0,3,3);
536 
537  Eigen::Quaternionf quatHPI(matRot);
538  double dError = std::accumulate(vecError.begin(), vecError.end(), .0) / vecError.size(); // HPI estimation Error
539 
540  matPosition.conservativeResize(matPosition.rows()+1, 10);
541  matPosition(matPosition.rows()-1,0) = fTime;
542  matPosition(matPosition.rows()-1,1) = quatHPI.x();
543  matPosition(matPosition.rows()-1,2) = quatHPI.y();
544  matPosition(matPosition.rows()-1,3) = quatHPI.z();
545  matPosition(matPosition.rows()-1,4) = transDevHead(0,3);
546  matPosition(matPosition.rows()-1,5) = transDevHead(1,3);
547  matPosition(matPosition.rows()-1,6) = transDevHead(2,3);
548  matPosition(matPosition.rows()-1,7) = vecGoF.mean();
549  matPosition(matPosition.rows()-1,8) = dError;
550  matPosition(matPosition.rows()-1,9) = 0;
551 }
552 
static void storeHeadPosition(float fTime, const Eigen::MatrixXf &matTransDevHead, Eigen::MatrixXd &matPosition, const Eigen::VectorXd &vecGoF, const QVector< double > &vecError)
Definition: hpifit.cpp:528
IOUtils class declaration.
QVector< int > vecHpiFreqs() const
void checkForUpdate(const SensorSet &sensorSet)
Definition: hpifit.cpp:102
Digitization point description.
Coordinate transformation description.
FiffCov class declaration.
HPI Fit algorithm data structure.
Definition: hpifitdata.h:108
Holds a set of digitizer points.
void fit(const Eigen::MatrixXd &matProjectedData, const Eigen::MatrixXd &matProjectors, const HpiModelParameters &hpiModelParameters, const Eigen::MatrixXd &matCoilsHead, HpiFitResult &hpiFitResult)
SignalModel class declaration.
FiffDigPointSet class declaration.
MNEMath class declaration.
HpiModelParameters class declaration.
Eigen::Matrix< float, 4, 4, Eigen::DontAlign > trans
HPIFit class declaration.
Eigen::MatrixX3f apply_trans(const Eigen::MatrixX3f &rr, bool do_move=true) const
FwdCoilSet class declaration.
Brief description of this class.
Definition: signalmodel.h:74
Brief description of this class.
Eigen::MatrixX3f apply_inverse_trans(const Eigen::MatrixX3f &rr, bool do_move=true) const
SensorSet class declaration.
HPIFitData class declaration.
fiff_float_t r[3]
Eigen::MatrixXd fitData(const HpiModelParameters &hpiModelParameters, const Eigen::MatrixXd &matData)
Definition: signalmodel.cpp:71
HpiDataUpdater class declaration.