Subversion Repositories seema-scanner

#include "eigentools.h"

namespace eigentools{

// Function to fit two sets of corresponding transformation data.
// Algorithm and notation according to Mili Shah, Comparing two sets ofcorresponding six degree of freedom data, CVIU 2011.
// DTU, 2013, Oline V. Olesen, Jakob Wilm
void fitSixDofData(const std::vector<Eigen::Affine3f> X, const std::vector<Eigen::Affine3f> X_mark, Eigen::Affine3f &H){
// NOT DEBUGGED!
    int N = X.size();
    assert(N == X_mark.size());

    // Mean translations
    Eigen::Vector3f t;
    Eigen::Vector3f t_mark;
    for(int i=0; i<N; i++){
        t += 1.0/N * X[i].translation();
        t_mark += 1.0/N * X[i].translation();
    }

    // Data with mean adjusted translations
    Eigen::MatrixXf X_bar(3, 4*N);
    Eigen::MatrixXf X_mark_bar(3, 4*N);
    for(int i=0; i<N; i++){
        X_bar.block(0,i*4,3,3) = X[i].rotation();
        X_bar.block(0,i*4+3,3,1) = X[i].translation() - t;
        X_mark_bar.block(0,i*4,3,3) = X_mark[i].rotation();
        X_mark_bar.block(0,i*4+3,3,1) = X_mark[i].translation() - t_mark;
    }

    // SVD
    Eigen::JacobiSVD<Eigen::MatrixXf> svd(X_bar*X_mark_bar.transpose(), Eigen::ComputeThinU | Eigen::ComputeThinV);

    Eigen::Matrix3f D = Eigen::Matrix3f::Identity();
    if((svd.matrixV()*svd.matrixU().transpose()).determinant() < 0)
        D(3,3) = -1;

    // Best rotation
    Eigen::Matrix3f Omega = svd.matrixV()*D*svd.matrixU().transpose();

    // Best translation
    Eigen::Vector3f tau = t_mark - Omega*t;

    // Complete solution
    H.linear() = Omega;
    H.translation() = tau;

}


}