fiff_coord_trans.cpp

Go to the documentation of this file.

//=============================================================================================================
 
//=============================================================================================================
// INCLUDES
//=============================================================================================================
 
#include "fiff_coord_trans.h"
 
#include "fiff_stream.h"
#include "fiff_tag.h"
#include "fiff_dir_node.h"
 
 
#include <QFile>
#include <QTextStream>
 
// 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 <math.h>
 
//=============================================================================================================
// EIGEN INCLUDES
//=============================================================================================================
 
#include <Eigen/Dense>
#include <QDebug>
 
#include <stdexcept>
//=============================================================================================================
// USED NAMESPACES
//=============================================================================================================
 
using namespace FIFFLIB;
using namespace Eigen;
 
//=============================================================================================================
// DEFINE MEMBER METHODS
//=============================================================================================================
 
FiffCoordTrans::FiffCoordTrans()
: from(-1)
, to(-1)
, trans(MatrixXf::Identity(4, 4))
, invtrans(MatrixXf::Identity(4, 4))
{
}
 
//=============================================================================================================
 
FiffCoordTrans::FiffCoordTrans(QIODevice& p_IODevice)
: from(-1)
, to(-1)
, trans(MatrixXf::Identity(4, 4))
, invtrans(MatrixXf::Identity(4, 4))
{
    if (!read(p_IODevice, *this)) {
        throw std::runtime_error("Coordinate transform not found");
    }
}
 
//=============================================================================================================
 
FiffCoordTrans::FiffCoordTrans(const FiffCoordTrans& p_FiffCoordTrans)
: from(p_FiffCoordTrans.from)
, to(p_FiffCoordTrans.to)
, trans(p_FiffCoordTrans.trans)
, invtrans(p_FiffCoordTrans.invtrans)
{
}
 
//=============================================================================================================
 
FiffCoordTrans::~FiffCoordTrans()
{
}
 
//=============================================================================================================
 
void FiffCoordTrans::clear()
{
    from = -1;
    to = -1;
    trans.setIdentity();
    invtrans.setIdentity();
}
 
//=============================================================================================================
 
bool FiffCoordTrans::invert_transform()
{
    fiff_int_t from_new = this->to;
    this->to = this->from;
    this->from = from_new;
    this->trans = this->trans.inverse().eval();
    this->invtrans = this->invtrans.inverse().eval();
 
    return true;
}
 
//=============================================================================================================
 
bool FiffCoordTrans::read(QIODevice& p_IODevice, FiffCoordTrans& p_Trans)
{
    FiffStream::SPtr pStream(new FiffStream(&p_IODevice));
 
    qInfo("Reading coordinate transform from %s...\n", pStream->streamName().toUtf8().constData());
    if (!pStream->open())
        return false;
 
    //
    //   Locate and read the coordinate transformation
    //
    FiffTag::UPtr t_pTag;
    bool success = false;
 
    //
    //   Get the MRI <-> head coordinate transformation
    //
    for (qint32 k = 0; k < pStream->dir().size() && !success; ++k) {
        if (pStream->dir()[k]->kind == FIFF_COORD_TRANS && pStream->read_tag(t_pTag, pStream->dir()[k]->pos)) {
            p_Trans = t_pTag->toCoordTrans();
            success = true;
        }
    }
 
    return success;
}
 
//=============================================================================================================
 
bool FiffCoordTrans::write(QIODevice& qIODevice)
{
    // Create the file and save the essentials
    FiffStream::SPtr pStream = FiffStream::start_file(qIODevice);
    if (!pStream) {
        return false;
    }
    qInfo("Write coordinate transform in %s...\n", pStream->streamName().toUtf8().constData());
    this->writeToStream(pStream.data());
    pStream->end_file();
    qIODevice.close();
    return true;
}
 
//=============================================================================================================
 
void FiffCoordTrans::writeToStream(FiffStream* pStream)
{
    pStream->write_coord_trans(*this);
}
 
//=============================================================================================================
 
MatrixX3f FiffCoordTrans::apply_trans(const MatrixX3f& rr, bool do_move) const
{
    MatrixX4f rr_ones(rr.rows(), 4);
    if (do_move) {
        rr_ones.setOnes();
    } else {
        rr_ones.setZero();
    }
    rr_ones.block(0, 0, rr.rows(), 3) = rr;
    return rr_ones * trans.block<3, 4>(0, 0).transpose();
}
 
//=============================================================================================================
 
MatrixX3f FiffCoordTrans::apply_inverse_trans(const MatrixX3f& rr, bool do_move) const
{
    MatrixX4f rr_ones(rr.rows(), 4);
    if (do_move) {
        rr_ones.setOnes();
    } else {
        rr_ones.setZero();
    }
    rr_ones.block(0, 0, rr.rows(), 3) = rr;
    return rr_ones * invtrans.block<3, 4>(0, 0).transpose();
}
 
//=============================================================================================================
 
QString FiffCoordTrans::frame_name(int frame)
{
    switch (frame) {
        case FIFFV_COORD_UNKNOWN:
            return "unknown";
        case FIFFV_COORD_DEVICE:
            return "MEG device";
        case FIFFV_COORD_ISOTRAK:
            return "isotrak";
        case FIFFV_COORD_HPI:
            return "hpi";
        case FIFFV_COORD_HEAD:
            return "head";
        case FIFFV_COORD_MRI:
            return "MRI (surface RAS)";
        case FIFFV_MNE_COORD_MRI_VOXEL:
            return "MRI voxel";
        case FIFFV_COORD_MRI_SLICE:
            return "MRI slice";
        case FIFFV_COORD_MRI_DISPLAY:
            return "MRI display";
        case FIFFV_MNE_COORD_CTF_DEVICE:
            return "CTF MEG device";
        case FIFFV_MNE_COORD_CTF_HEAD:
            return "CTF/4D/KIT head";
        case FIFFV_MNE_COORD_RAS:
            return "RAS (non-zero origin)";
        case FIFFV_MNE_COORD_MNI_TAL:
            return "MNI Talairach";
        case FIFFV_MNE_COORD_FS_TAL_GTZ:
            return "Talairach (MNI z > 0)";
        case FIFFV_MNE_COORD_FS_TAL_LTZ:
            return "Talairach (MNI z < 0)";
        default:
            return "unknown";
    }
}
 
//=============================================================================================================
 
FiffCoordTrans::FiffCoordTrans(int from, int to, const Matrix3f& rot, const Vector3f& move)
{
    this->trans = MatrixXf::Zero(4, 4);
 
    this->from = from;
    this->to = to;
 
    this->trans.block<3, 3>(0, 0) = rot;
    this->trans.block<3, 1>(0, 3) = move;
    this->trans(3, 3) = 1.0f;
 
    FiffCoordTrans::addInverse(*this);
}
 
//=============================================================================================================
 
FiffCoordTrans::FiffCoordTrans(int from, int to, const Matrix4f& matTrans, bool bStandard)
{
    this->trans = matTrans;
    this->from = from;
    this->to = to;
 
    if (bStandard) {
        // make sure that it is a standard transform if requested
        this->trans.row(3) = Vector4f(0, 0, 0, 1).transpose();
    }
 
    FiffCoordTrans::addInverse(*this);
}
 
//=============================================================================================================
 
bool FiffCoordTrans::addInverse(FiffCoordTrans& t)
{
    t.invtrans = t.trans.inverse().eval();
    return true;
}
 
//=============================================================================================================
 
void FiffCoordTrans::print() const
{
    qInfo().noquote() << QString("Coordinate transformation: %1 -> %2").arg(frame_name(this->from), frame_name(this->to));
 
    for (int p = 0; p < 3; p++)
        qInfo("\t% 8.6f % 8.6f % 8.6f\t% 7.2f mm", trans(p, 0), trans(p, 1), trans(p, 2), 1000 * trans(p, 3));
    qInfo("\t% 8.6f % 8.6f % 8.6f   % 7.2f", trans(3, 0), trans(3, 1), trans(3, 2), trans(3, 3));
}
 
//=============================================================================================================
 
float FiffCoordTrans::angleTo(Eigen::MatrixX4f mTransDest)
{
    MatrixX4f mDevHeadT = this->trans;
    Matrix3f mRot = mDevHeadT.block(0, 0, 3, 3);
    Matrix3f mRotNew = mTransDest.block(0, 0, 3, 3);
 
    Quaternionf quat(mRot);
    Quaternionf quatNew(mRotNew);
 
    float fAngle;
 
    // calculate rotation
    Quaternionf quatCompare;
 
    quatCompare = quat * quatNew.inverse();
    fAngle = quat.angularDistance(quatNew);
    fAngle = fAngle * 180 / M_PI;
 
    return fAngle;
}
 
//=============================================================================================================
 
float FiffCoordTrans::translationTo(Eigen::MatrixX4f mTransDest)
{
    VectorXf vTrans = this->trans.col(3);
    VectorXf vTransDest = mTransDest.col(3);
 
    float fMove = (vTrans - vTransDest).norm();
    return fMove;
}
 
//=============================================================================================================
 
void FiffCoordTrans::apply_trans(float r[3], const FiffCoordTrans& t, bool do_move)
{
    float res[3];
    for (int j = 0; j < 3; j++) {
        res[j] = do_move ? t.trans(j, 3) : 0.0f;
        for (int k = 0; k < 3; k++)
            res[j] += t.trans(j, k) * r[k];
    }
    for (int j = 0; j < 3; j++)
        r[j] = res[j];
}
 
//=============================================================================================================
 
void FiffCoordTrans::apply_inverse_trans(float r[3], const FiffCoordTrans& t, bool do_move)
{
    float res[3];
    for (int j = 0; j < 3; j++) {
        res[j] = do_move ? t.invtrans(j, 3) : 0.0f;
        for (int k = 0; k < 3; k++)
            res[j] += t.invtrans(j, k) * r[k];
    }
    for (int j = 0; j < 3; j++)
        r[j] = res[j];
}
 
//=============================================================================================================
 
FiffCoordTrans FiffCoordTrans::identity(int from, int to)
{
    FiffCoordTrans t;
    t.from = from;
    t.to = to;
    // trans and invtrans are already identity from default constructor
    return t;
}
 
//=============================================================================================================
 
FiffCoordTrans FiffCoordTrans::inverted() const
{
    FiffCoordTrans t;
    t.from = this->to;
    t.to = this->from;
    t.trans = this->invtrans;
    t.invtrans = this->trans;
    return t;
}
 
//=============================================================================================================
 
FiffCoordTrans FiffCoordTrans::combine(int from, int to, const FiffCoordTrans& t1, const FiffCoordTrans& t2)
{
    FiffCoordTrans a, b;
    bool found = false;
 
    for (int swapped = 0; swapped < 2 && !found; swapped++) {
        const FiffCoordTrans& s1 = (swapped == 0) ? t1 : t2;
        const FiffCoordTrans& s2 = (swapped == 0) ? t2 : t1;
 
        if (s1.to == to && s2.from == from) {
            a = s1;
            b = s2;
            found = true;
        } else if (s1.from == to && s2.from == from) {
            a = s1.inverted();
            b = s2;
            found = true;
        } else if (s1.to == to && s2.to == from) {
            a = s1;
            b = s2.inverted();
            found = true;
        } else if (s1.from == to && s2.to == from) {
            a = s1.inverted();
            b = s2.inverted();
            found = true;
        }
    }
 
    if (!found || a.from != b.to) {
        qCritical("Cannot combine coordinate transforms");
        return FiffCoordTrans();
    }
 
    // Catenate: result = a * b (apply b first, then a)
    FiffCoordTrans result;
    result.from = b.from;
    result.to = a.to;
    result.trans = a.trans * b.trans;
    result.trans.row(3) << 0.0f, 0.0f, 0.0f, 1.0f;
    addInverse(result);
    return result;
}
 
//=============================================================================================================
 
FiffCoordTrans FiffCoordTrans::fromCardinalPoints(int from, int to,
                                                  const float* rL,
                                                  const float* rN,
                                                  const float* rR)
{
    Map<const Vector3f> L(rL);
    Map<const Vector3f> N(rN);
    Map<const Vector3f> R(rR);
 
    Vector3f diff1 = N - L;
    Vector3f diff2 = R - L;
 
    float alpha = diff1.dot(diff2) / diff2.dot(diff2);
    Vector3f r0 = (1.0f - alpha) * L + alpha * R;
    Vector3f ex = diff2.normalized();
    Vector3f ey = (N - r0).normalized();
    Vector3f ez = ex.cross(ey);
 
    FiffCoordTrans result;
    result.from = from;
    result.to = to;
    result.rot().col(0) = ex;
    result.rot().col(1) = ey;
    result.rot().col(2) = ez;
    result.move() = r0;
    addInverse(result);
    return result;
}
 
//=============================================================================================================
 
FiffCoordTrans FiffCoordTrans::readTransform(const QString& name, int from, int to)
{
    QFile file(name);
    FiffStream::SPtr stream(new FiffStream(&file));
 
    if (!stream->open()) {
        return FiffCoordTrans();
    }
 
    FiffTag::UPtr t_pTag;
    for (int k = 0; k < stream->dir().size(); k++) {
        if (stream->dir()[k]->kind == FIFF_COORD_TRANS) {
            if (!stream->read_tag(t_pTag, stream->dir()[k]->pos))
                continue;
 
            FiffCoordTrans t = t_pTag->toCoordTrans();
            if (t.from == from && t.to == to) {
                stream->close();
                return t;
            } else if (t.from == to && t.to == from) {
                t.invert_transform();
                stream->close();
                return t;
            }
        }
    }
 
    qCritical("No suitable coordinate transformation found in %s.", name.toUtf8().constData());
    stream->close();
    return FiffCoordTrans();
}
 
//=============================================================================================================
 
FiffCoordTrans FiffCoordTrans::readMriTransform(const QString& name)
{
    return readTransform(name, FIFFV_COORD_MRI, FIFFV_COORD_HEAD);
}
 
//=============================================================================================================
 
FiffCoordTrans FiffCoordTrans::readMeasTransform(const QString& name)
{
    return readTransform(name, FIFFV_COORD_DEVICE, FIFFV_COORD_HEAD);
}
 
//=============================================================================================================
 
FiffCoordTrans FiffCoordTrans::readTransformAscii(const QString& name, int from, int to)
{
    QFile file(name);
    if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) {
        qCritical("Cannot open %s", name.toUtf8().constData());
        return FiffCoordTrans();
    }
 
    QTextStream in(&file);
    Matrix3f rot;
    Vector3f moveVec;
 
    int row = 0;
    while (!in.atEnd() && row < 4) {
        QString line = in.readLine();
        // Strip comments and whitespace
        int commentIdx = line.indexOf('#');
        if (commentIdx >= 0)
            line = line.left(commentIdx);
        line = line.trimmed();
        if (line.isEmpty())
            continue;
 
        if (row < 3) {
            QStringList parts = line.simplified().split(' ', Qt::SkipEmptyParts);
            if (parts.size() < 4) {
                qCritical("Cannot read the coordinate transformation from %s",
                          name.toUtf8().constData());
                return FiffCoordTrans();
            }
            bool ok1, ok2, ok3, ok4;
            rot(row, 0) = parts[0].toFloat(&ok1);
            rot(row, 1) = parts[1].toFloat(&ok2);
            rot(row, 2) = parts[2].toFloat(&ok3);
            moveVec[row] = parts[3].toFloat(&ok4) / 1000.0f;
            if (!ok1 || !ok2 || !ok3 || !ok4) {
                qCritical("Bad floating point number in coordinate transformation");
                return FiffCoordTrans();
            }
        }
        // Row 3 (the last row of the 4x4 matrix) is consumed but ignored
        row++;
    }
 
    if (row < 4) {
        qCritical("Cannot read the coordinate transformation from %s",
                  name.toUtf8().constData());
        return FiffCoordTrans();
    }
 
    return FiffCoordTrans(from, to, rot, moveVec);
}
 
//=============================================================================================================
 
FiffCoordTrans FiffCoordTrans::readFShead2mriTransform(const QString& name)
{
    return readTransformAscii(name, FIFFV_COORD_HEAD, FIFFV_COORD_MRI);
}
 
//=============================================================================================================
 
FiffCoordTrans FiffCoordTrans::readFromTag(const FiffTag::UPtr& tag)
{
    FiffCoordTrans t;
    if (tag->isMatrix() || tag->getType() != FIFFT_COORD_TRANS_STRUCT || tag->data() == nullptr)
        return t;
 
    qint32* t_pInt32 = reinterpret_cast<qint32*>(tag->data());
    t.from = t_pInt32[0];
    t.to = t_pInt32[1];
 
    float* t_pFloat = reinterpret_cast<float*>(tag->data());
    int count = 0;
    int r, c;
    for (r = 0; r < 3; ++r) {
        t.trans(r, 3) = t_pFloat[11 + r];
        for (c = 0; c < 3; ++c) {
            t.trans(r, c) = t_pFloat[2 + count];
            ++count;
        }
    }
    t.trans(3, 0) = 0.0f;
    t.trans(3, 1) = 0.0f;
    t.trans(3, 2) = 0.0f;
    t.trans(3, 3) = 1.0f;
 
    count = 0;
    for (r = 0; r < 3; ++r) {
        t.invtrans(r, 3) = t_pFloat[23 + r];
        for (c = 0; c < 3; ++c) {
            t.invtrans(r, c) = t_pFloat[14 + count];
            ++count;
        }
    }
    t.invtrans(3, 0) = 0.0f;
    t.invtrans(3, 1) = 0.0f;
    t.invtrans(3, 2) = 0.0f;
    t.invtrans(3, 3) = 1.0f;
 
    return t;
}
 
//=============================================================================================================
 
FiffCoordTrans FiffCoordTrans::readTransformFromNode(FiffStream::SPtr& stream,
                                                     const FiffDirNode::SPtr& node,
                                                     int from, int to)
{
    FiffTag::UPtr t_pTag;
 
    // Scan every directory entry of the node for a coordinate transformation
    // matching the requested frames. The loop previously had no braces, so it
    // only recorded the last entry's kind and then indexed dir[nent()], which
    // is one past the end - and left `kind` uninitialised for an empty node.
    for (int k = 0; k < node->nent(); ++k) {
        const FiffDirEntry::SPtr& entry = node->dir[k];
        if (entry->kind != FIFF_COORD_TRANS)
            continue;
 
        if (!stream->read_tag(t_pTag, entry->pos))
            continue;
 
        FiffCoordTrans res = readFromTag(t_pTag);
        if (res.isEmpty())
            continue;
 
        if (res.from == from && res.to == to) {
            return res;
        }
        if (res.from == to && res.to == from) {
            return res.inverted();
        }
    }
 
    qWarning("No suitable coordinate transformation found");
    return FiffCoordTrans();
}
 
//=============================================================================================================
 
FiffCoordTrans FiffCoordTrans::procrustesAlign(int from_frame,
                                               int to_frame,
                                               const Eigen::MatrixXf& fromPts,
                                               const Eigen::MatrixXf& toPts,
                                               const Eigen::VectorXf& w,
                                               float max_diff)
{
    int np = fromPts.rows();
 
    /* Calculate centroids and subtract */
    Eigen::Vector3f from0 = fromPts.colwise().mean();
    Eigen::Vector3f to0 = toPts.colwise().mean();
 
    Eigen::MatrixXf fromC = fromPts.rowwise() - from0.transpose();
    Eigen::MatrixXf toC = toPts.rowwise() - to0.transpose();
 
    /* Compute the cross-covariance matrix S */
    Eigen::Matrix3f S;
    if (w.size() > 0) {
        S = fromC.transpose() * w.asDiagonal() * toC;
    } else {
        S = fromC.transpose() * toC;
    }
 
    /* SVD of S to solve the orthogonal Procrustes problem */
    Eigen::JacobiSVD<Eigen::Matrix3f> svd(S, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Matrix3f R = svd.matrixV() * svd.matrixU().transpose();
 
    /* Translation */
    Eigen::Vector3f moveVec = to0 - R * from0;
 
    /* Test the transformation */
    for (int p = 0; p < np; ++p) {
        Eigen::Vector3f rr = R * fromPts.row(p).transpose() + moveVec;
        float diff = (toPts.row(p).transpose() - rr).norm();
        if (diff > max_diff) {
            qWarning("Too large difference in matching : %7.1f > %7.1f mm",
                     1000.0f * diff, 1000.0f * max_diff);
            return FiffCoordTrans();
        }
    }
 
    return FiffCoordTrans(from_frame, to_frame, R, moveVec);
}