Subversion Repositories seema-scanner

// Function to solve the hand-eye (or eye-in-hand) calibration problem.

// Finds [Omega | tau], to minimize ||[R_mark | t_mark][Omega | tau] - [Omega | tau][R | t]||^2

// Algorithm according to Tsai, Lenz, A new technique for fully autonomous and efficient 3d robotics hand-eye calibration

// DTU, 2014, Jakob Wilm

void handEyeCalibrationTsai(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());

    // construct equations for rotation

    cv::Mat A(3*N, 3, CV_32F);

    cv::Mat b(3*N, 1, CV_32F);

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

        // angle axis representations

        cv::Vec3f rot;

        cv::Vec3f rot_mark;

        cv::Rodrigues(R[i], rot);

        cv::Rodrigues(R_mark[i], rot_mark);

        cv::Vec3f P = 2.0*sin(cv::norm(rot)/2.0)*cv::normalize(rot);

        cv::Vec3f P_mark = 2.0*sin(cv::norm(rot_mark)/2.0)*cv::normalize(rot_mark);

        cv::Vec3f sum = P+P_mark;

        cv::Mat crossProduct = (cv::Mat_<float>(3,3) << 0.0, -sum(2), sum(1), sum(2), 0.0, -sum(0), -sum(1), sum(0), 0.0);

        crossProduct.copyTo(A.rowRange(i*3, i*3+3));

        cv::Mat(P-P_mark).copyTo(b.rowRange(i*3, i*3+3));

    }

    // solve for rotation

    cv::Vec<float, 9> P_prime;

    cv::solve(A, b, P_prime);

    cv::Vec<float, 9> P = 2.0*P_prime/(cv::sqrt(1.0 + cv::norm(P_prime)*cv::norm(P_prime)));

    float nP = cv::norm(P);

    cv::Mat crossProduct = (cv::Mat_<float>(3,3) << 0.0, -P(2), P(1), P(2), 0.0, -P(0), -P(1), P(0), 0.0);

    cv::Mat OmegaMat = (1.0-nP*nP/2.0)*cv::Mat::eye(3,3,CV_32F) + 0.5*(cv::Mat(P)*cv::Mat(P).t() + cv::sqrt(4.0 - nP*nP)*crossProduct);

    Omega = cv::Matx33f(OmegaMat);

    // construct equations for translation

    A.setTo(0.0);

    b.setTo(0.0);

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

        cv::Mat diff = cv::Mat(R[i]) - cv::Mat::eye(3, 3, CV_32F);

        diff.copyTo(A.rowRange(i*3, i*3+3));

        cv::Mat diff_mark = cv::Mat(Omega*R_mark[i] - R[i]);

        diff_mark.copyTo(b.rowRange(i*3, i*3+3));

    }

    // solve for translation

    cv::solve(A, b, tau);

}

// Finds [Omega | tau], to minimize ||[R_mark | t_mark] - [Omega | tau][R | t]||^2