经典手眼标定算法之Navy

文章转载自：经典手眼标定算法之Navy的OpenCV实现 ：http://blog.csdn.net/yunlinwang/article/details/51871520

在我的上一篇博客中已经介绍了Tsai的手眼标定算法，下面主要介绍Frank C. Park and Bryan J. Martin在文献Robot sensor calibration: solving AX=XB on the Euclidean group中提出的手眼标定算法，该算法也被称为Navy手眼标定算法，该算法的主要创新点为利用李群理论的知识来求解手眼标定经典方程。该算法基于OpenCV的C++版本程序可去CSDN资源下载，MATLAB版本作者为苏黎世理工的Christian Wengert，也可在此处下载。

 

void Navy_HandEye(Mat Hcg, vector<Mat> Hgij, vector<Mat> Hcij)
{
    CV_Assert(Hgij.size() == Hcij.size());
    int nStatus = Hgij.size();

    Mat Rgij(3, 3, CV_64FC1);
    Mat Rcij(3, 3, CV_64FC1);

    Mat alpha1(3, 1, CV_64FC1);
    Mat beta1(3, 1, CV_64FC1);
    Mat alpha2(3, 1, CV_64FC1);
    Mat beta2(3, 1, CV_64FC1);
    Mat A(3, 3, CV_64FC1);
    Mat B(3, 3, CV_64FC1);

    Mat alpha(3, 1, CV_64FC1);
    Mat beta(3, 1, CV_64FC1);
    Mat M(3, 3, CV_64FC1, Scalar(0));

    Mat MtM(3, 3, CV_64FC1);
    Mat veMtM(3, 3, CV_64FC1);
    Mat vaMtM(3, 1, CV_64FC1);
    Mat pvaM(3, 3, CV_64FC1, Scalar(0));

    Mat Rx(3, 3, CV_64FC1);

    Mat Tgij(3, 1, CV_64FC1);
    Mat Tcij(3, 1, CV_64FC1);

    Mat eyeM = Mat::eye(3, 3, CV_64FC1);

    Mat tempCC(3, 3, CV_64FC1);
    Mat tempdd(3, 1, CV_64FC1);

    Mat C;
    Mat d;
    Mat Tx(3, 1, CV_64FC1);

    //Compute rotation
    if (Hgij.size() == 2) // Two (Ai,Bi) pairs
    {
        Rodrigues(Hgij[0](Rect(0, 0, 3, 3)), alpha1);
        Rodrigues(Hgij[1](Rect(0, 0, 3, 3)), alpha2);
        Rodrigues(Hcij[0](Rect(0, 0, 3, 3)), beta1);
        Rodrigues(Hcij[1](Rect(0, 0, 3, 3)), beta2);

        alpha1.copyTo(A.col(0));
        alpha2.copyTo(A.col(1));
        (alpha1.cross(alpha2)).copyTo(A.col(2));

        beta1.copyTo(B.col(0));
        beta2.copyTo(B.col(1));
        (beta1.cross(beta2)).copyTo(B.col(2));

        Rx = A*B.inv();

    }
    else // More than two (Ai,Bi) pairs
    {
        for (int i = 0; i < nStatus; i++)
        {
            Hgij[i](Rect(0, 0, 3, 3)).copyTo(Rgij);
            Hcij[i](Rect(0, 0, 3, 3)).copyTo(Rcij);

            Rodrigues(Rgij, alpha);
            Rodrigues(Rcij, beta);

            M = M + beta*alpha.t();
        }

        MtM = M.t()*M;
        eigen(MtM, vaMtM, veMtM);

        pvaM.at<double>(0, 0) = 1 / sqrt(vaMtM.at<double>(0, 0));
        pvaM.at<double>(1, 1) = 1 / sqrt(vaMtM.at<double>(1, 0));
        pvaM.at<double>(2, 2) = 1 / sqrt(vaMtM.at<double>(2, 0));

        Rx = veMtM*pvaM*veMtM.inv()*M.t();
    }

    //Computer Translation 
    for (int i = 0; i < nStatus; i++)
    {
        Hgij[i](Rect(0, 0, 3, 3)).copyTo(Rgij);
        Hcij[i](Rect(0, 0, 3, 3)).copyTo(Rcij);
        Hgij[i](Rect(3, 0, 1, 3)).copyTo(Tgij);
        Hcij[i](Rect(3, 0, 1, 3)).copyTo(Tcij);

        tempCC = eyeM - Rgij;
        tempdd = Tgij - Rx * Tcij;

        C.push_back(tempCC);
        d.push_back(tempdd);
    }

    Tx = (C.t()*C).inv()*(C.t()*d);

    Rx.copyTo(Hcg(Rect(0, 0, 3, 3)));
    Tx.copyTo(Hcg(Rect(3, 0, 1, 3)));
    Hcg.at<double>(3, 0) = 0.0;
    Hcg.at<double>(3, 1) = 0.0;
    Hcg.at<double>(3, 2) = 0.0;
    Hcg.at<double>(3, 3) = 1.0;

}