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>
 
// MSVC builds already define _USE_MATH_DEFINES globally (see src/CMakeLists.txt),
// so define it here only for the toolchains that do not.
#ifndef _USE_MATH_DEFINES
#define _USE_MATH_DEFINES
#endif
#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;
 
    // 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;
        }
    } 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 = static_cast<int>(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 = static_cast<int>(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;
 
    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;
 
        // 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;
}