WIP [Maint] Refactoring Hpi Fitting

applications/mne_scan/plugins/hpi/hpi.cpp

@@ -47,6 +47,7 @@
 #include <scMeas/realtimemultisamplearray.h>
 #include <scMeas/realtimehpiresult.h>
 #include <inverse/hpiFit/hpifit.h>
+#include <inverse/hpiFit/hpidataupdater.h>
 
 #include <fiff/fiff_info.h>
 #include <fiff/c/fiff_digitizer_data.h>
@@ -547,14 +548,21 @@ void Hpi::run()
     }
 
     // init hpi fit
+    HpiModelParameters hpiModelParameters(m_vCoilFreqs,
+                                          m_pFiffInfo->sfreq,
+                                          m_pFiffInfo->linefreq,
+                                          false);
     HpiFitResult fitResult;
+    fitResult.hpiFreqs = m_vCoilFreqs;
+    fitResult.errorDistances = QVector<double>(m_vCoilFreqs.size());
     fitResult.devHeadTrans = m_pFiffInfo->dev_head_t;
     fitResult.devHeadTrans.from = 1;
     fitResult.devHeadTrans.to = 4;
 
     FiffCoordTrans transDevHeadRef = m_pFiffInfo->dev_head_t;
 
-    HPIFit HPI = HPIFit(m_pFiffInfo, true);
+    HpiDataUpdater hpiDataUpdater = HpiDataUpdater(m_pFiffInfo);
+    HPIFit HPI = HPIFit(hpiDataUpdater.getSensors());
 
     double dErrorMax = 0.0;
     double dMeanErrorDist = 0.0;
@@ -571,6 +579,7 @@ void Hpi::run()
     m_mutex.unlock();
 
     MatrixXd matDataMerged(m_pFiffInfo->chs.size(), fittingWindowSize);
+    bool bOrder = false;
 
     while(!isInterruptionRequested()) {
         m_mutex.lock();
@@ -591,41 +600,40 @@ void Hpi::run()
                 m_mutex.lock();
                 if(m_bDoSingleHpi) {
                     m_bDoSingleHpi = false;
-                    fitResult.devHeadTrans.clear();
+                    fitResult = HpiFitResult();
                 }
                 fitResult.sFilePathDigitzers = m_sFilePathDigitzers;
                 m_mutex.unlock();
 
                 matDataMerged.block(0, iDataIndexCounter, matData.rows(), matDataMerged.cols()-iDataIndexCounter) =
                         matData.block(0, 0, matData.rows(), matDataMerged.cols()-iDataIndexCounter);
 
-                // Perform HPI fit
                 m_mutex.lock();
-                if(m_bDoFreqOrder) {
-                    // find correct frequencie order if requested
-                    HPI.findOrder(matDataMerged,
-                                  m_matCompProjectors,
-                                  fitResult.devHeadTrans,
-                                  m_vCoilFreqs,
-                                  fitResult.errorDistances,
-                                  fitResult.GoF,
-                                  fitResult.fittedCoils,
-                                  m_pFiffInfo);
-                    m_bDoFreqOrder = false;
-                }
+                hpiModelParameters = HpiModelParameters(m_vCoilFreqs,
+                                                        m_pFiffInfo->sfreq,
+                                                        m_pFiffInfo->linefreq,
+                                                        false);
+                hpiDataUpdater.checkForUpdate(m_pFiffInfo);
+                HPI.checkForUpdate(hpiDataUpdater.getSensors());
+
+                hpiDataUpdater.prepareDataAndProjectors(matDataMerged,m_matCompProjectors);
+                bOrder = m_bDoFreqOrder;
+                m_bDoFreqOrder = false;
                 m_mutex.unlock();
 
                 // Perform actual fitting
-                m_mutex.lock();
-                HPI.fitHPI(matDataMerged,
-                           m_matCompProjectors,
-                           fitResult.devHeadTrans,
-                           m_vCoilFreqs,
-                           fitResult.errorDistances,
-                           fitResult.GoF,
-                           fitResult.fittedCoils,
-                           m_pFiffInfo);
-                m_mutex.unlock();
+                HPI.fit(hpiDataUpdater.getProjectedData(),
+                        hpiDataUpdater.getProjectors(),
+                        hpiModelParameters,
+                        hpiDataUpdater.getHpiDigitizer(),
+                        bOrder,
+                        fitResult);
+
+                if(bOrder) {
+                    m_mutex.lock();
+                    m_vCoilFreqs = fitResult.hpiFreqs;
+                    m_mutex.unlock();
+                }
 
                 //Check if the error meets distance requirement
                 if(fitResult.errorDistances.size() > 0) {

examples/ex_hpiFit/ex_hpiFit.pro

@@ -1,7 +1,7 @@
 #==============================================================================================================
 #
 # @file     ex_hpiFit.pro
-# @author   Ruben Dörfel <[email protected]>;
+# @author   Ruben Dörfel <[email protected]>;
 # @since    0.1.0
 # @date     January, 2020
 #

examples/ex_hpiFit/main.cpp

@@ -1,7 +1,7 @@
 //=============================================================================================================
 /**
  * @file     main.cpp
- * @author   Ruben Dörfel <[email protected]>;
+ * @author   Ruben Dörfel <[email protected]>;
  * @since    0.1.0
  * @date     January, 2020
  *
@@ -44,6 +44,7 @@
 #include <fiff/fiff_dig_point_set.h>
 
 #include <inverse/hpiFit/hpifit.h>
+#include <inverse/hpiFit/hpidataupdater.h>
 
 #include <utils/ioutils.h>
 #include <utils/generics/applicationlogger.h>
@@ -105,8 +106,7 @@ int main(int argc, char *argv[])
     QCommandLineOption inStep("step", "The step size in ms.", "in","10");
     QCommandLineOption inFreqs("freqs", "The frequencies used.", "in","154,158,161,166");
     QCommandLineOption inSave("save", "Store the fitting results [0,1].", "in","0");
-    QCommandLineOption inVerbose("verbose", "Print to command line [0,1].", "in","0");
-    QCommandLineOption inDebug("debug", "Save debug info during HPI fit [0,1].", "in","0");
+    QCommandLineOption inVerbose("verbose", "Print to command line [0,1].", "in","1");
     QCommandLineOption inFast("fast", "Do fast fits [0,1].", "in","0");
     QCommandLineOption outName("fileOut", "The output file name for movement data.", "out","position.txt");
 
@@ -116,7 +116,6 @@ int main(int argc, char *argv[])
     parser.addOption(inFreqs);
     parser.addOption(inSave);
     parser.addOption(inVerbose);
-    parser.addOption(inDebug);
     parser.addOption(inFast);
     parser.addOption(outName);
 
@@ -138,7 +137,6 @@ int main(int argc, char *argv[])
     QFile t_fileIn(parser.value(inFile));
     bool bSave = parser.value(inSave).toInt();
     bool bVerbose = parser.value(inVerbose).toInt();
-    bool bDoDebug = parser.value(inDebug).toInt();
     bool bFast = parser.value(inFast).toInt();
     QString sNameOut(parser.value(outName));
 
@@ -175,7 +173,7 @@ int main(int argc, char *argv[])
         vecFreqs[i] = lFreqs[i].toInt();
     }
 
-    QVector<double> vecError;
+    QVector<double> vecError(lFreqs.size());
     VectorXd vecGoF;
     FiffDigPointSet fittedPointSet;
 
@@ -202,7 +200,11 @@ int main(int argc, char *argv[])
     // if debugging files are necessary set bDoDebug = true;
     QString sHPIResourceDir = QCoreApplication::applicationDirPath() + "/HPIFittingDebug";
 
-    HPIFit HPI = HPIFit(pFiffInfo,bFast);
+    HpiDataUpdater hpiDataUpdater = HpiDataUpdater(pFiffInfo);
+    HPIFit HPI = HPIFit(hpiDataUpdater.getSensors());
+
+    MatrixXd matAmplitudes;
+    MatrixXd matCoilLoc(4,3);
 
     // ordering of frequencies
     from = first + vecTime(0);
@@ -214,14 +216,21 @@ int main(int argc, char *argv[])
 
     // order frequencies
     timer.start();
-    HPI.findOrder(matData,
-                  matProjectors,
-                  pFiffInfo->dev_head_t,
-                  vecFreqs,
-                  vecError,
-                  vecGoF,
-                  fittedPointSet,
-                  pFiffInfo);
+    hpiDataUpdater.prepareDataAndProjectors(matData,matProjectors);
+    const auto& matProjectedData = hpiDataUpdater.getProjectedData();
+    const auto& matPreparedProjectors = hpiDataUpdater.getProjectors();
+    const auto& matCoilsHead = hpiDataUpdater.getHpiDigitizer();
+    HpiModelParameters hpiModelParameters(vecFreqs,
+                                          pFiffInfo->sfreq,
+                                          pFiffInfo->linefreq,
+                                          bFast);
+
+    HpiFitResult hpiFitResult;
+    HPI.fit(matProjectedData,matPreparedProjectors,hpiModelParameters,matCoilsHead,true,hpiFitResult);
+    hpiModelParameters = HpiModelParameters(hpiFitResult.hpiFreqs,
+                                            pFiffInfo->sfreq,
+                                            pFiffInfo->linefreq,
+                                            bFast);
 
     float fTimer = 0.0;
 
@@ -240,30 +249,28 @@ int main(int argc, char *argv[])
         }
 
         timer.start();
-        HPI.fitHPI(matData,
-                   matProjectors,
-                   pFiffInfo->dev_head_t,
-                   vecFreqs,
-                   vecError,
-                   vecGoF,
-                   fittedPointSet,
-                   pFiffInfo,
-                   bDoDebug,
-                   sHPIResourceDir);
+        hpiDataUpdater.prepareDataAndProjectors(matData,matProjectors);
+        const auto& matProjectedData = hpiDataUpdater.getProjectedData();
+        const auto& matPreparedProjectors = hpiDataUpdater.getProjectors();
+        HPI.fit(matProjectedData,
+                matPreparedProjectors,
+                hpiModelParameters,
+                matCoilsHead,
+                hpiFitResult);
         fTimer = timer.elapsed();
 
-        HPIFit::storeHeadPosition(vecTime(i), pFiffInfo->dev_head_t.trans, matPosition, vecGoF, vecError);
+        HPIFit::storeHeadPosition(vecTime(i), hpiFitResult.devHeadTrans.trans, matPosition, hpiFitResult.GoF, hpiFitResult.errorDistances);
         matPosition(i,9) = fTimer;
         // if big head displacement occures, update debHeadTrans
-        if(MNEMath::compareTransformation(transDevHead.trans, pFiffInfo->dev_head_t.trans, fThreshRot, fThreshTrans)) {
-            transDevHead = pFiffInfo->dev_head_t;
+        if(MNEMath::compareTransformation(transDevHead.trans, hpiFitResult.devHeadTrans.trans, fThreshRot, fThreshTrans)) {
+            transDevHead = hpiFitResult.devHeadTrans;
             qInfo() << "dev_head_t has been updated.";
         }
         if(bVerbose) {
             qInfo() << "Iteration" << i << "Of" << vecTime.size()
                     << " Duration " << fTimer << "ms"
-                    << " Error" << vecError[0]*1000 << vecError[1]*1000 << vecError[2]*1000 << vecError[3]*1000
-                    << " GoF" << vecGoF[0] << vecGoF[1] << vecGoF[2] << vecGoF[3];
+                    << " Error" << hpiFitResult.errorDistances[0]*1000 << hpiFitResult.errorDistances[1]*1000 << hpiFitResult.errorDistances[2]*1000 << hpiFitResult.errorDistances[3]*1000
+                    << " GoF" << hpiFitResult.GoF[0] << hpiFitResult.GoF[1] << hpiFitResult.GoF[2] << hpiFitResult.GoF[3];
         }
     }
     qInfo() << "Iterations:" << vecTime.size()