经典手眼标定算法之Navy的OpenCV实现

       在我的上一篇博客中已经介绍了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，也可在此处下载。

---

       正如Tsai等在文献中指出的，手眼标定问题其实就是求解 AX=XB 方程问题。其中 A 为机器人末端连杆坐标架在机器人-摄像机系统移动前后的转换关系， B 为摄像机坐标架在移动前后的相对关系。Tsai指出要唯一确定手眼矩阵的各分量，至少需要旋转轴不平行的两组运动。由于在观测中一般存在噪声，因此在实际测量中一般需要多组运动来求解该方程。

       假设有多组观测值 {(A1,B1),(A2,B2),⋯,(Ak,Bk)} ，求解 AX=XB 方程可以转化为如下最小化问题

min∑i=1kd(AiX,XBi)

其中， d 表示在欧式群上的距离测度。利用李群理论知识可以将上述最小化问题转化为最小二乘拟合问题，从而可以得到简单而又明确的解。下面介绍李群中的基本知识。

---

李群基本知识

       描述刚体运动可以用欧式群表示，由如下形式的矩阵表示：

[θ0b1]

其中， θ∈SO(3),b∈R3 ， SO(3) 表示旋转矩阵组成的群。

       每个李群都有其对应的李代数，李群与李代数之间的转换可以由指数映射和对数映射完成。由于本人数学知识比较单薄，如果读者想要继续深入理解李群和李代数知识，可以自行查阅相关文献或者书籍。在这里我就不详细介绍，只简单介绍Park文献中出现的知识。

指数映射

       李代数到李群的转换满足指数映射关系，假设 [w]∈so(3) ，而 exp[w]∈SO(3) ，则其指数映射满足罗德里格斯公式：

exp[w]=I+sin∥w∥∥w∥[w]+1−cos∥w∥∥w∥2[w]2

对数映射

       李群到李代数的转换满足对数映射关系，假设 θ∈SO(3) ，则其对数映射为：

logθ=ϕ2sinϕ(θ−θT)

其中， 1+2cosϕ=tr(θ) 。

---

利用李群知识求解AX=XB

       我们知道，手眼标定问题其实就是求解方程 AX=XB ，将 X 移到方程的右边，可以得到 A=XBXT ，根据上文介绍的对数映射关系，在方程的两边取对数，则可以得到

logA=logXBXT

令 logA=[α],logB=[β] ，则上式可以化为 [α]=X[β]XT=[Xβ] ，从而

α=Xβ

        AX=XB 写成矩阵形式为

[θA0bA1][θX0bX1]=[θX0bX1][θB0bB1]

将上式展开可以得到

θAθX=θXθBθAbX+bA=θXbB+bX

       与Tsai手眼标定类似，同样采用“两步法”求解上述方程，先解算旋转矩阵，再求得平移向量。由上面推导 α=Xβ ，因此 θAθX=θXθB 可以写成 α=θXβ 。当存在多组观测值时，求解该方程可以转化为下面最小二乘拟合问题：

min∑i=1k∥θXβi−αi∥2

       很显然，上述问题是典型的绝对定向问题，因而求解上式与绝对定向相同，其解为

θX=(MTM)−12MT

其中， M=∑i=1kβiαiT 。

然而，上式的求解是有条件的，即 M 不奇异，因而当 A 、 B 只有两组时，不能用上述方法求解。

       当只有两组 A 、 B 时，即有 A1,A2,B1,B2 ，

α1=logA1,α2=logA2,β1=logB1,β2=logB2

旋转矩阵的解为：

θX=MN−1

其中， M=(α1α2α1×α2),N=(β1β2β1×β2) ， × 表示叉乘。

       在求得旋转矩阵后，求解平移向量的步骤与Tsai手眼标定相同，读者可以查看上篇文献，这里就不在赘述。

---

算法源代码

       根据上述基本计算步骤，在利用OpenCV 2.0开源库的基础上，编写Tsai手眼标定方法的c++程序，其实现函数代码如下：

void Navy_HandEye(Mat Hcg, vector Hgij, vector 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;

}