Subversion Repositories seema-scanner

// Function to fit two sets of corresponding transformation data.

// Algorithm and notation according to Mili Shah, Comparing two sets of corresponding six degree of freedom data, CVIU 2011.

// DTU, 2013, Oline V. Olesen, Jakob Wilm

void fitSixDofData(const std::vector<cv::Matx33f> R, const std::vector<cv::Vec3f> t, const std::vector<cv::Matx33f> R_mark, const std::vector<cv::Vec3f> t_mark, cv::Matx33f &Omega, cv::Vec3f &tau){

// NOT DEBUGGED!

    int N = R.size();

    assert(N == R_mark.size());

    assert(N == t.size());

    assert(N == t_mark.size());

    // Mean translations

    cv::Vec3f t_mean;

    cv::Vec3f t_mark_mean;

    for(int i=0; i<N; i++){

        t_mean += 1.0/N * t[i];

        t_mark_mean += 1.0/N * t_mark[i];

    }

    // Data with mean adjusted translations

    cv::Mat X_bar(3, 4*N, CV_32F);

    cv::Mat X_mark_bar(3, 4*N, CV_32F);

    for(int i=0; i<N; i++){

        // ASSIGNEMENTS?!

        X_bar.colRange(i*4,i*4+3) = cv::Mat(R[i]);

        X_bar.col(i*4+3) = cv::Mat(t[i] - t_mean);

        X_mark_bar.colRange(i*4,i*4+3) = cv::Mat(R_mark[i]);

        X_mark_bar.col(i*4+3) = cv::Mat(t_mark[i] - t_mark_mean);

    }

    // SVD

    cv::Matx33f W, U, VT;

    cv::SVDecomp(X_bar*X_mark_bar.t(), W, U, VT);

    cv::Matx33f D = cv::Matx33f::eye();

    if(cv::determinant(VT*U) < 0)

        D(3,3) = -1;

    // Best rotation

    Omega = VT*D*U;

    // Best translation

    tau = t_mark_mean - Omega*t_mean;

}