64 #include <Eigen/Dense> 72 #include <QtConcurrent/QtConcurrent> 78 using namespace Eigen;
94 : m_sensors(sensorSet),
104 if(m_sensors != sensorSet) {
105 m_sensors = sensorSet;
112 const MatrixXd& matProjectors,
114 const MatrixXd& matCoilsHead,
117 fit(matProjectedData,matProjectors,hpiModelParameters,matCoilsHead,
false,hpiFitResult);
123 const MatrixXd& matProjectors,
125 const MatrixXd& matCoilsHead,
126 const bool bOrderFrequencies,
129 if(matProjectedData.rows() != matProjectors.rows()) {
130 std::cout<<
"HPIFit::fit - Projector and data dimensions do not match. Returning."<<std::endl;
132 }
else if(hpiModelParameters.iNHpiCoils()!= matCoilsHead.rows()) {
133 std::cout<<
"HPIFit::fit - Number of coils and hpi digitizers do not match. Returning."<<std::endl;
135 }
else if(matProjectedData.rows()==0 || matProjectors.rows()==0) {
136 std::cout<<
"HPIFit::fit - No data or Projectors passed. Returning."<<std::endl;
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;
143 const MatrixXd matAmplitudes = computeAmplitudes(matProjectedData,
146 const MatrixXd matCoilsSeed = computeSeedPoints(matAmplitudes,
147 hpiFitResult.devHeadTrans,
148 hpiFitResult.errorDistances,
151 CoilParam fittedCoilParams = dipfit(matCoilsSeed,
154 hpiModelParameters.iNHpiCoils(),
159 if(bOrderFrequencies) {
160 const std::vector<int> vecOrder = findCoilOrder(fittedCoilParams.pos,
163 fittedCoilParams.pos = order(vecOrder,fittedCoilParams.pos);
164 hpiFitResult.hpiFreqs = order(vecOrder,hpiModelParameters.
vecHpiFreqs());
167 hpiFitResult.GoF = computeGoF(fittedCoilParams.dpfiterror);
169 hpiFitResult.fittedCoils = getFittedPointSet(fittedCoilParams.pos);
171 hpiFitResult.devHeadTrans = computeDeviceHeadTransformation(fittedCoilParams.pos,
174 hpiFitResult.errorDistances = computeEstimationError(fittedCoilParams.pos,
176 hpiFitResult.devHeadTrans);
181 Eigen::MatrixXd HPIFit::computeAmplitudes(
const Eigen::MatrixXd& matProjectedData,
185 MatrixXd matTopo = m_signalModel.
fitData(hpiModelParameters,matProjectedData);
186 matTopo.transposeInPlace();
189 const int iNumCoils = hpiModelParameters.iNHpiCoils();
191 MatrixXd matAmpSine(matProjectedData.cols(), iNumCoils);
192 MatrixXd matAmpCosine(matProjectedData.cols(), iNumCoils);
194 matAmpSine = matTopo.leftCols(iNumCoils);
195 matAmpCosine = matTopo.middleCols(iNumCoils,iNumCoils);
198 for(
int j = 0; j < iNumCoils; ++j) {
201 fNS = matAmpSine.col(j).array().square().sum();
202 fNC = matAmpCosine.col(j).array().square().sum();
204 matAmpSine.col(j) = matAmpCosine.col(j);
213 Eigen::MatrixXd HPIFit::computeSeedPoints(
const Eigen::MatrixXd& matAmplitudes,
215 const QVector<double>& vecError,
216 const Eigen::MatrixXd& matCoilsHead)
218 const int iNumCoils = matCoilsHead.rows();
219 MatrixXd matCoilsSeed = MatrixXd::Zero(iNumCoils,3);
221 const double dError = std::accumulate(vecError.begin(), vecError.end(), .0) / vecError.size();
223 if(transDevHead.
trans != MatrixXd::Identity(4,4).cast<
float>() && dError < 0.010) {
225 matCoilsSeed = transDevHead.
apply_inverse_trans(matCoilsHead.cast<
float>()).cast<
double>();
228 VectorXi vecChIdcs(iNumCoils);
230 for (
int j = 0; j < iNumCoils; j++) {
232 VectorXd::Index indMax;
233 matAmplitudes.col(j).maxCoeff(&indMax);
234 if(indMax < m_sensors.ncoils()) {
237 vecChIdcs(j) = iChIdx;
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);
253 CoilParam HPIFit::dipfit(
const MatrixXd matCoilsSeed,
255 const MatrixXd& matData,
257 const MatrixXd& matProjectors,
258 const int iMaxIterations,
259 const float fAbortError)
263 QList<HPIFitData> lCoilData;
265 for(qint32 i = 0; i < iNumCoils; ++i) {
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;
274 lCoilData.append(coilData);
279 if(!lCoilData.isEmpty()) {
286 QFuture<void> future = QtConcurrent::map(lCoilData,
288 future.waitForFinished();
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;
306 std::vector<int> HPIFit::findCoilOrder(
const MatrixXd& matCoilsDev,
307 const MatrixXd& matCoilsHead)
310 MatrixXd matCoilTemp = matCoilsDev;
311 const int iNumCoils = matCoilsDev.rows();
313 std::vector<int> vecOrder(iNumCoils);
314 std::iota(vecOrder.begin(), vecOrder.end(), 0);;
317 const double dErrorMin = 0.010;
318 double dErrorActual = 0.0;
319 double dErrorBest = dErrorMin;
321 MatrixXd matTrans(4,4);
322 std::vector<int> vecOrderBest = vecOrder;
324 bool bSuccess =
false;
327 for(
int i = 0; i < iNumCoils; i++) {
328 matCoilTemp.row(i) = matCoilsDev.row(vecOrder[i]);
330 matTrans = computeTransformation(matCoilsHead,matCoilTemp);
331 dErrorActual = objectTrans(matCoilsHead,matCoilTemp,matTrans);
332 if(dErrorActual < dErrorMin && dErrorActual < dErrorBest) {
334 dErrorBest = dErrorActual;
335 vecOrderBest = vecOrder;
338 }
while (std::next_permutation(vecOrder.begin(), vecOrder.end()));
344 double HPIFit::objectTrans(
const MatrixXd& matHeadCoil,
345 const MatrixXd& matCoil,
346 const MatrixXd& matTrans)
349 const int iNumCoils = matHeadCoil.rows();
350 MatrixXd matTemp = matCoil;
353 matTemp.conservativeResize(matCoil.rows(),matCoil.cols()+1);
354 matTemp.block(0,3,iNumCoils,1).setOnes();
355 matTemp.transposeInPlace();
358 MatrixXd matTestPos = matTrans * matTemp;
361 MatrixXd matDiff = matTestPos.block(0,0,3,iNumCoils) - matHeadCoil.transpose();
362 VectorXd vecError = matDiff.colwise().norm();
365 double dError = matDiff.colwise().norm().mean();;
372 Eigen::MatrixXd HPIFit::order(
const std::vector<int>& vecOrder,
373 const Eigen::MatrixXd& matToOrder)
375 const int iNumCoils = vecOrder.size();
376 MatrixXd matToOrderTemp = matToOrder;
378 for(
int i = 0; i < iNumCoils; i++) {
379 matToOrderTemp.row(i) = matToOrder.row(vecOrder[i]);
381 return matToOrderTemp;
386 QVector<int> HPIFit::order(
const std::vector<int>& vecOrder,
387 const QVector<int>& vecToOrder)
389 const int iNumCoils = vecOrder.size();
390 QVector<int> vecToOrderTemp = vecToOrder;
392 for(
int i = 0; i < iNumCoils; i++) {
393 vecToOrderTemp[i] = vecToOrder[vecOrder[i]];
395 return vecToOrderTemp;
400 Eigen::VectorXd HPIFit::computeGoF(
const Eigen::VectorXd& vecDipFitError)
402 VectorXd vecGoF(vecDipFitError.size());
403 for(
int i = 0; i < vecDipFitError.size(); ++i) {
404 vecGoF(i) = 1 - vecDipFitError(i);
412 const Eigen::MatrixXd& matCoilsHead)
414 const MatrixXd matTrans = computeTransformation(matCoilsHead,matCoilsDev);
415 return FiffCoordTrans::make(1,4,matTrans.cast<
float>(),
true);
420 Eigen::Matrix4d HPIFit::computeTransformation(Eigen::MatrixXd matNH, MatrixXd matBT)
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;
428 for(
int i = 0; i < 15; ++i) {
430 matXdiff = matNH.col(0) - matBT.col(0);
431 matYdiff = matNH.col(1) - matBT.col(1);
432 matZdiff = matNH.col(2) - matBT.col(2);
434 dMeanX = matXdiff.mean();
435 dMeanY = matYdiff.mean();
436 dMeanZ = matZdiff.mean();
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;
446 matC = matBT.transpose() * matNH;
448 JacobiSVD< MatrixXd > svd(matC ,Eigen::ComputeThinU | ComputeThinV);
450 matQ = svd.matrixU() * svd.matrixV().transpose();
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;
460 matBT = matBT * matQ;
463 dNormf = (matNH.transpose()-matBT.transpose()).norm();
467 for(
int j = 0; j < 3; ++j) {
468 for(
int k = 0; k < 3; ++k) {
469 matRot(j,k) = matQ(k,j);
474 matTrans(0,3) = dMeanX;
475 matTrans(1,3) = dMeanY;
476 matTrans(2,3) = dMeanZ;
481 matTransFinal = matRot * matTrans * matTransFinal;
483 return matTransFinal;
488 QVector<double> HPIFit::computeEstimationError(
const Eigen::MatrixXd& matCoilsDev,
489 const Eigen::MatrixXd& matCoilsHead,
493 MatrixXd matTemp = matCoilsDev;
494 MatrixXd matTestPos = transDevHead.
apply_trans(matTemp.cast<
float>()).cast<
double>();
495 MatrixXd matDiffPos = matTestPos - matCoilsHead;
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();
511 const int iNumCoils = matCoilsDev.rows();
513 for(
int i = 0; i < iNumCoils; ++i) {
515 digPoint.
kind = FIFFV_POINT_EEG;
517 digPoint.
r[0] = matCoilsDev(i,0);
518 digPoint.
r[1] = matCoilsDev(i,1);
519 digPoint.
r[2] = matCoilsDev(i,2);
521 fittedPointSet << digPoint;
523 return fittedPointSet;
529 const Eigen::MatrixXf& transDevHead,
530 Eigen::MatrixXd& matPosition,
531 const Eigen::VectorXd& vecGoF,
532 const QVector<double>& vecError)
535 Matrix3f matRot = transDevHead.block(0,0,3,3);
537 Eigen::Quaternionf quatHPI(matRot);
538 double dError = std::accumulate(vecError.begin(), vecError.end(), .0) / vecError.size();
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;
Eigen::MatrixX3f apply_trans(const Eigen::MatrixX3f &rr, bool do_move=true) const
Digitization point description.
Coordinate transformation description.
void doDipfitConcurrent()
IOUtils class declaration.
static void storeHeadPosition(float fTime, const Eigen::MatrixXf &matTransDevHead, Eigen::MatrixXd &matPosition, const Eigen::VectorXd &vecGoF, const QVector< double > &vecError)
MNEMath class declaration.
FwdCoilSet class declaration.
Eigen::MatrixXd fitData(const HpiModelParameters &hpiModelParameters, const Eigen::MatrixXd &matData)
Holds a set of digitizer points.
QVector< int > vecHpiFreqs() const
SensorSet class declaration.
Eigen::Matrix< float, 4, 4, Eigen::DontAlign > trans
HPIFitData class declaration.
void checkForUpdate(const SensorSet &sensorSet)
SignalModel class declaration.
FiffDigPointSet class declaration.
HpiModelParameters class declaration.
HPIFit class declaration.
Brief description of this class.
Brief description of this class.
Eigen::MatrixX3f apply_inverse_trans(const Eigen::MatrixX3f &rr, bool do_move=true) const
HpiDataUpdater class declaration.
HPI Fit algorithm data structure.
void fit(const Eigen::MatrixXd &matProjectedData, const Eigen::MatrixXd &matProjectors, const HpiModelParameters &hpiModelParameters, const Eigen::MatrixXd &matCoilsHead, HpiFitResult &hpiFitResult)
FiffCov class declaration.