inv_hpi_fit.cpp

Go to the documentation of this file.

//=============================================================================================================
 
//=============================================================================================================
// INCLUDES
//=============================================================================================================
 
#include "inv_hpi_fit.h"
#include "inv_hpi_fit_data.h"
#include "inv_sensor_set.h"
#include "inv_hpi_data_updater.h"
#include "inv_signal_model.h"
#include "inv_hpi_model_parameters.h"
 
#include <utils/ioutils.h>
 
#define _USE_MATH_DEFINES
#include <cmath>
#include <iostream>
#include <vector>
#include <numeric>
#include <fiff/fiff_cov.h>
#include <fiff/fiff_dig_point_set.h>
#include <fstream>
 
#include <fwd/fwd_coil_set.h>
 
//=============================================================================================================
// EIGEN INCLUDES
//=============================================================================================================
 
#include <Eigen/Dense>
 
//=============================================================================================================
// QT INCLUDES
//=============================================================================================================
 
#include <QVector>
#include <QFuture>
#include <QtConcurrent/QtConcurrent>
 
//=============================================================================================================
// USED NAMESPACES
//=============================================================================================================
 
using namespace Eigen;
using namespace INVLIB;
using namespace FIFFLIB;
using namespace FWDLIB;
 
//=============================================================================================================
// DEFINE GLOBAL METHODS
//=============================================================================================================
 
//=============================================================================================================
// DEFINE MEMBER METHODS
//=============================================================================================================
 
//=============================================================================================================
 
InvHpiFit::InvHpiFit(const InvSensorSet& sensorSet)
    : m_sensors(sensorSet),
      m_signalModel(InvSignalModel())
{
 
}
 
//=============================================================================================================
 
void InvHpiFit::checkForUpdate(const InvSensorSet &sensorSet)
{
    if(m_sensors != sensorSet) {
        m_sensors = sensorSet;
    }
}
 
//=============================================================================================================
 
void InvHpiFit::fit(const MatrixXd& matProjectedData,
                 const MatrixXd& matProjectors,
                 const InvHpiModelParameters& hpiModelParameters,
                 const MatrixXd& matCoilsHead,
                 HpiFitResult& hpiFitResult)
{
    fit(matProjectedData,matProjectors,hpiModelParameters,matCoilsHead,false,hpiFitResult);
}
 
//=============================================================================================================
 
void InvHpiFit::fit(const MatrixXd& matProjectedData,
                 const MatrixXd& matProjectors,
                 const InvHpiModelParameters& hpiModelParameters,
                 const MatrixXd& matCoilsHead,
                 const bool bOrderFrequencies,
                 HpiFitResult& hpiFitResult)
{
    if(matProjectedData.rows() != matProjectors.rows()) {
        std::cout<< "InvHpiFit::fit - Projector and data dimensions do not match. Returning."<<std::endl;
        return;
    } else if(hpiModelParameters.iNHpiCoils()!= matCoilsHead.rows()) {
        std::cout<< "InvHpiFit::fit - Number of coils and hpi digitizers do not match. Returning."<<std::endl;
        return;
    } else if(matProjectedData.rows()==0 || matProjectors.rows()==0) {
        std::cout<< "InvHpiFit::fit - No data or Projectors passed. Returning."<<std::endl;
        return;
    } else if(m_sensors.ncoils() != matProjectedData.rows()) {
        std::cout<< "InvHpiFit::fit - Number of channels in sensors and data do not match. Returning."<<std::endl;
        return;
    }
 
    const MatrixXd matAmplitudes = computeAmplitudes(matProjectedData,
                                                     hpiModelParameters);
 
    const MatrixXd matCoilsSeed = computeSeedPoints(matAmplitudes,
                                                    hpiFitResult.devHeadTrans,
                                                    hpiFitResult.errorDistances,
                                                    matCoilsHead);
 
    CoilParam fittedCoilParams = dipfit(matCoilsSeed,
                                        m_sensors,
                                        matAmplitudes,
                                        hpiModelParameters.iNHpiCoils(),
                                        matProjectors,
                                        500,
                                        1e-9f);
 
    if(bOrderFrequencies) {
        const std::vector<int> vecOrder = findCoilOrder(fittedCoilParams.pos,
                                                        matCoilsHead);
 
        fittedCoilParams.pos = order(vecOrder,fittedCoilParams.pos);
        hpiFitResult.hpiFreqs = order(vecOrder,hpiModelParameters.vecHpiFreqs());
    }
 
    hpiFitResult.GoF = computeGoF(fittedCoilParams.dpfiterror);
 
    hpiFitResult.fittedCoils = getFittedPointSet(fittedCoilParams.pos);
 
    hpiFitResult.devHeadTrans = computeDeviceHeadTransformation(fittedCoilParams.pos,
                                                                matCoilsHead);
 
    hpiFitResult.errorDistances = computeEstimationError(fittedCoilParams.pos,
                                                         matCoilsHead,
                                                         hpiFitResult.devHeadTrans);
}
 
//=============================================================================================================
 
Eigen::MatrixXd InvHpiFit::computeAmplitudes(const Eigen::MatrixXd& matProjectedData,
                                          const InvHpiModelParameters& hpiModelParameters)
{
    // fit model
    MatrixXd matTopo = m_signalModel.fitData(hpiModelParameters,matProjectedData);
    matTopo.transposeInPlace();
 
    // split into sine and cosine amplitudes
    const int iNumCoils = hpiModelParameters.iNHpiCoils();
 
    MatrixXd matAmpSine(matProjectedData.cols(), iNumCoils);
    MatrixXd matAmpCosine(matProjectedData.cols(), iNumCoils);
 
    matAmpSine = matTopo.leftCols(iNumCoils);
    matAmpCosine = matTopo.middleCols(iNumCoils,iNumCoils);
 
    // Select sine or cosine component depending on their contributions to the amplitudes
    for(int j = 0; j < iNumCoils; ++j) {
        float fNS = 0.0;
        float fNC = 0.0;
        fNS = matAmpSine.col(j).array().square().sum();
        fNC = matAmpCosine.col(j).array().square().sum();
        if(fNC > fNS) {
            matAmpSine.col(j) = matAmpCosine.col(j);
        }
    }
 
    return matAmpSine;
}
 
//=============================================================================================================
 
Eigen::MatrixXd InvHpiFit::computeSeedPoints(const Eigen::MatrixXd& matAmplitudes,
                                          const FIFFLIB::FiffCoordTrans& transDevHead,
                                          const QVector<double>& vecError,
                                          const Eigen::MatrixXd& matCoilsHead)
{
    const int iNumCoils = matCoilsHead.rows();
    MatrixXd matCoilsSeed = MatrixXd::Zero(iNumCoils,3);
 
    const double dError = std::accumulate(vecError.begin(), vecError.end(), .0) / vecError.size();
 
    if(transDevHead.trans != MatrixXd::Identity(4,4).cast<float>() && dError < 0.010) {
        // if good last fit, use old trafo
        matCoilsSeed = transDevHead.apply_inverse_trans(matCoilsHead.cast<float>()).cast<double>();
    } else {
        // if not, find max amplitudes in channels
        VectorXi vecChIdcs(iNumCoils);
 
        for (int j = 0; j < iNumCoils; j++) {
            int iChIdx = 0;
            VectorXd::Index indMax;
            matAmplitudes.col(j).maxCoeff(&indMax);
            if(indMax < m_sensors.ncoils()) {
                iChIdx = indMax;
            }
            vecChIdcs(j) = iChIdx;
        }
        // and go 3 cm inwards from max channels
        for (int j = 0; j < vecChIdcs.rows(); ++j) {
            if(vecChIdcs(j) < m_sensors.ncoils()) {
                Vector3d r0 = m_sensors.r0(vecChIdcs(j));
                Vector3d ez = m_sensors.ez(vecChIdcs(j));
                matCoilsSeed.row(j) = (-1 * ez * 0.03 + r0).transpose();
            }
        }
    }
    return matCoilsSeed;
}
 
//=============================================================================================================
 
CoilParam InvHpiFit::dipfit(const MatrixXd matCoilsSeed,
                         const InvSensorSet& sensors,
                         const MatrixXd& matData,
                         const int iNumCoils,
                         const MatrixXd& matProjectors,
                         const int iMaxIterations,
                         const float fAbortError)
{
    //Do this in conncurrent mode
    //Generate QList structure which can be handled by the QConcurrent framework
    QList<InvHpiFitData> lCoilData;
 
    for(qint32 i = 0; i < iNumCoils; ++i) {
        InvHpiFitData coilData;
        coilData.m_coilPos = matCoilsSeed.row(i);
        coilData.m_sensorData = matData.col(i);
        coilData.m_sensors = sensors;
        coilData.m_matProjector = matProjectors;
        coilData.m_iMaxIterations = iMaxIterations;
        coilData.m_fAbortError = fAbortError;
 
        lCoilData.append(coilData);
    }
    //Do the concurrent filtering
    CoilParam coil(iNumCoils);
 
    if(!lCoilData.isEmpty()) {
        //        //Do sequential
        //        for(int l = 0; l < lCoilData.size(); ++l) {
        //            doDipfitConcurrent(lCoilData[l]);
        //        }
 
        //Do concurrent
        QFuture<void> future = QtConcurrent::map(lCoilData,
                                                 &InvHpiFitData::doDipfitConcurrent);
        future.waitForFinished();
 
        //Transform results to final coil information
        for(qint32 i = 0; i < lCoilData.size(); ++i) {
            coil.pos.row(i) = lCoilData.at(i).m_coilPos;
            coil.mom = lCoilData.at(i).m_errorInfo.moment.transpose();
            coil.dpfiterror(i) = lCoilData.at(i).m_errorInfo.error;
            coil.dpfitnumitr(i) = lCoilData.at(i).m_errorInfo.numIterations;
 
            //std::cout<<std::endl<< "InvHpiFit::dipfit - Itr steps for coil " << i << " =" <<coil.dpfitnumitr(i);
        }
    }
    return coil;
}
 
//=============================================================================================================
 
std::vector<int> InvHpiFit::findCoilOrder(const MatrixXd& matCoilsDev,
                                       const MatrixXd& matCoilsHead)
{
    // extract digitized and fitted coils
    MatrixXd matCoilTemp = matCoilsDev;
    const int iNumCoils = matCoilsDev.rows();
 
    std::vector<int> vecOrder(iNumCoils);
    std::iota(vecOrder.begin(), vecOrder.end(), 0);;
 
    // maximum 10 mm mean error
    const double dErrorMin = 0.010;
    double dErrorActual = 0.0;
    double dErrorBest = dErrorMin;
 
    MatrixXd matTrans(4,4);
    std::vector<int> vecOrderBest = vecOrder;
 
    bool bSuccess = false;
    // permutation
    do {
        for(int i = 0; i < iNumCoils; i++) {
            matCoilTemp.row(i) =  matCoilsDev.row(vecOrder[i]);
        }
        matTrans = computeTransformation(matCoilsHead,matCoilTemp);
        dErrorActual = objectTrans(matCoilsHead,matCoilTemp,matTrans);
        if(dErrorActual < dErrorMin && dErrorActual < dErrorBest) {
            // exit
            dErrorBest = dErrorActual;
            vecOrderBest = vecOrder;
            bSuccess = true;
        }
    } while (std::next_permutation(vecOrder.begin(), vecOrder.end()));
    return vecOrderBest;
}
 
//=============================================================================================================
 
double InvHpiFit::objectTrans(const MatrixXd& matHeadCoil,
                           const MatrixXd& matCoil,
                           const MatrixXd& matTrans)
{
    // Compute the fiducial registration error - the lower, the better.
    const int iNumCoils = matHeadCoil.rows();
    MatrixXd matTemp = matCoil;
 
    // homogeneous coordinates
    matTemp.conservativeResize(matCoil.rows(),matCoil.cols()+1);
    matTemp.block(0,3,iNumCoils,1).setOnes();
    matTemp.transposeInPlace();
 
    // apply transformation
    MatrixXd matTestPos = matTrans * matTemp;
 
    // remove
    MatrixXd matDiff = matTestPos.block(0,0,3,iNumCoils) - matHeadCoil.transpose();
    VectorXd vecError = matDiff.colwise().norm();
 
    // compute error
    double dError = matDiff.colwise().norm().mean();;
 
    return dError;
}
 
//=============================================================================================================
 
Eigen::MatrixXd InvHpiFit::order(const std::vector<int>& vecOrder,
                              const Eigen::MatrixXd& matToOrder)
{
    const int iNumCoils = vecOrder.size();
    MatrixXd matToOrderTemp = matToOrder;
 
    for(int i = 0; i < iNumCoils; i++) {
        matToOrderTemp.row(i) =  matToOrder.row(vecOrder[i]);
    }
    return matToOrderTemp;
}
 
//=============================================================================================================
 
QVector<int> InvHpiFit::order(const std::vector<int>& vecOrder,
                           const QVector<int>& vecToOrder)
{
    const int iNumCoils = vecOrder.size();
    QVector<int> vecToOrderTemp = vecToOrder;
 
    for(int i = 0; i < iNumCoils; i++) {
        vecToOrderTemp[i] =  vecToOrder[vecOrder[i]];
    }
    return vecToOrderTemp;
}
 
//=============================================================================================================
 
Eigen::VectorXd InvHpiFit::computeGoF(const Eigen::VectorXd& vecDipFitError)
{
    VectorXd vecGoF(vecDipFitError.size());
    for(int i = 0; i < vecDipFitError.size(); ++i) {
        vecGoF(i) = 1 - vecDipFitError(i);
    }
    return vecGoF;
}
 
//=============================================================================================================
 
FIFFLIB::FiffCoordTrans InvHpiFit::computeDeviceHeadTransformation(const Eigen::MatrixXd& matCoilsDev,
                                                                const Eigen::MatrixXd& matCoilsHead)
{
    const MatrixXd matTrans = computeTransformation(matCoilsHead,matCoilsDev);
    return FiffCoordTrans(1,4,matTrans.cast<float>(),true);
}
 
//=============================================================================================================
 
Eigen::Matrix4d InvHpiFit::computeTransformation(Eigen::MatrixXd matNH, MatrixXd matBT)
{
    MatrixXd matXdiff, matYdiff, matZdiff, matC, matQ;
    Matrix4d matTransFinal = Matrix4d::Identity(4,4);
    Matrix4d matRot = Matrix4d::Zero(4,4);
    Matrix4d matTrans = Matrix4d::Identity(4,4);
    double dMeanX,dMeanY,dMeanZ,dNormf;
 
    for(int i = 0; i < 15; ++i) {
        // Calculate mean translation for all points -> centroid of both data sets
        matXdiff = matNH.col(0) - matBT.col(0);
        matYdiff = matNH.col(1) - matBT.col(1);
        matZdiff = matNH.col(2) - matBT.col(2);
 
        dMeanX = matXdiff.mean();
        dMeanY = matYdiff.mean();
        dMeanZ = matZdiff.mean();
 
        // Apply translation -> bring both data sets to the same center location
        for (int j = 0; j < matBT.rows(); ++j) {
            matBT(j,0) = matBT(j,0) + dMeanX;
            matBT(j,1) = matBT(j,1) + dMeanY;
            matBT(j,2) = matBT(j,2) + dMeanZ;
        }
 
        // Estimate rotation component
        matC = matBT.transpose() * matNH;
 
        JacobiSVD< MatrixXd > svd(matC ,Eigen::ComputeThinU | ComputeThinV);
 
        matQ = svd.matrixU() * svd.matrixV().transpose();
 
        //Handle special reflection case
        if(matQ.determinant() < 0) {
            matQ(0,2) = matQ(0,2) * -1;
            matQ(1,2) = matQ(1,2) * -1;
            matQ(2,2) = matQ(2,2) * -1;
        }
 
        // Apply rotation on translated points
        matBT = matBT * matQ;
 
        // Calculate GOF
        dNormf = (matNH.transpose()-matBT.transpose()).norm();
 
        // Store rotation part to transformation matrix
        matRot(3,3) = 1;
        for(int j = 0; j < 3; ++j) {
            for(int k = 0; k < 3; ++k) {
                matRot(j,k) = matQ(k,j);
            }
        }
 
        // Store translation part to transformation matrix
        matTrans(0,3) = dMeanX;
        matTrans(1,3) = dMeanY;
        matTrans(2,3) = dMeanZ;
 
        // Safe rotation and translation to final matrix for next iteration step
        // This step is safe to do since we change one of the input point sets (matBT)
        // ToDo: Replace this for loop with a least square solution process
        matTransFinal = matRot * matTrans * matTransFinal;
    }
    return matTransFinal;
}
 
//=============================================================================================================
 
QVector<double> InvHpiFit::computeEstimationError(const Eigen::MatrixXd& matCoilsDev,
                                               const Eigen::MatrixXd& matCoilsHead,
                                               const FIFFLIB::FiffCoordTrans& transDevHead)
{
    //Calculate Error
    MatrixXd matTemp = matCoilsDev;
    MatrixXd matTestPos = transDevHead.apply_trans(matTemp.cast<float>()).cast<double>();
    MatrixXd matDiffPos = matTestPos - matCoilsHead;
 
    // compute error
    int iNumCoils = matCoilsDev.rows();
    QVector<double> vecError(iNumCoils);
    for(int i = 0; i < matDiffPos.rows(); ++i) {
        vecError[i] = matDiffPos.row(i).norm();
    }
    return vecError;
}
 
//=============================================================================================================
 
FIFFLIB::FiffDigPointSet InvHpiFit::getFittedPointSet(const Eigen::MatrixXd& matCoilsDev)
{
    FiffDigPointSet fittedPointSet;
    const int iNumCoils = matCoilsDev.rows();
 
    for(int i = 0; i < iNumCoils; ++i) {
        FiffDigPoint digPoint;
        digPoint.kind = FIFFV_POINT_EEG; //Store as EEG so they have a different color
        digPoint.ident = i;
        digPoint.r[0] = matCoilsDev(i,0);
        digPoint.r[1] = matCoilsDev(i,1);
        digPoint.r[2] = matCoilsDev(i,2);
 
        fittedPointSet << digPoint;
    }
    return fittedPointSet;
}
 
//=============================================================================================================
 
void InvHpiFit::storeHeadPosition(float fTime,
                               const Eigen::MatrixXf& transDevHead,
                               Eigen::MatrixXd& matPosition,
                               const Eigen::VectorXd& vecGoF,
                               const QVector<double>& vecError)
 
{
    Matrix3f matRot = transDevHead.block(0,0,3,3);
 
    Eigen::Quaternionf quatHPI(matRot);
    double dError = std::accumulate(vecError.begin(), vecError.end(), .0) / vecError.size();     // HPI estimation Error
 
    matPosition.conservativeResize(matPosition.rows()+1, 10);
    matPosition(matPosition.rows()-1,0) = fTime;
    matPosition(matPosition.rows()-1,1) = quatHPI.x();
    matPosition(matPosition.rows()-1,2) = quatHPI.y();
    matPosition(matPosition.rows()-1,3) = quatHPI.z();
    matPosition(matPosition.rows()-1,4) = transDevHead(0,3);
    matPosition(matPosition.rows()-1,5) = transDevHead(1,3);
    matPosition(matPosition.rows()-1,6) = transDevHead(2,3);
    matPosition(matPosition.rows()-1,7) = vecGoF.mean();
    matPosition(matPosition.rows()-1,8) = dError;
    matPosition(matPosition.rows()-1,9) = 0;
}
 
//=============================================================================================================
 
bool InvHpiFit::compareTransformation(const MatrixX4f& mDevHeadT,
                                      const MatrixX4f& mDevHeadDest,
                                      const float& fThreshRot,
                                      const float& fThreshTrans)
{
    bool bState = false;
 
    Matrix3f mRot = mDevHeadT.block(0,0,3,3);
    Matrix3f mRotDest = mDevHeadDest.block(0,0,3,3);
 
    VectorXf vTrans = mDevHeadT.block(0,3,3,1);
    VectorXf vTransDest = mDevHeadDest.block(0,3,3,1);
 
    Quaternionf quat(mRot);
    Quaternionf quatNew(mRotDest);
 
    // Compare Rotation
    float fAngle = quat.angularDistance(quatNew);
    fAngle = fAngle * 180 / M_PI;
 
    // Compare translation
    float fMove = (vTrans-vTransDest).norm();
 
    // compare to thresholds and update
    if(fMove > fThreshTrans) {
        qInfo() << "Large movement: " << fMove*1000 << "mm";
        bState = true;
    } else if (fAngle > fThreshRot) {
        qInfo() << "Large rotation: " << fAngle << "degree";
        bState = true;
    } else {
        bState = false;
    }
 
    return bState;
}