相机标定、手眼标定（理论）

一、理论篇

1.1 齐次变换矩阵（homogeneous transformation matrix ）

我们知道，在三维空间准确描述执行器的状态，不仅要包括位置信息，也要包含姿态信息，即位姿（Pose）。齐次变换矩阵就是在机器人学中描述一个坐标系到另一个坐标系的变换关系的矩阵，包含了姿态旋转分量和位置平移分量。
$$F{=}_1^2\text{T}=\left[\begin{array}{cc}{R}_{3\ast 3}& T_{3\ast 1}\\ 0& 1\end{array}\right]=\left[\begin{array}{cccc}{n}_x& o_x& a_x& x\\ n_y& o_y& a_y& y\\ n_z& o_z& a_z& z\\ 0& 0& 0& 1\end{array}\right]$$
其中，${}_1^2\text{T}$表示从坐标系1到坐标系2的变换，$R_{3\ast 3}$表示旋转矩阵，$T_{3\ast 1}$表示平移矩阵。

此外，值得注意的是关于齐次变换矩阵的左乘右乘问题。我的理解是以全局坐标系为参照做变换时用左乘，以当前坐标系为参照做变换时用右乘。举例：

• 若执行器当前的位姿为$T_A$,若其绕全局坐标系xyz下做了一个变换，变换矩阵为$T_B$,则其当前位姿为$T_B\ast T_A$;
• 若执行器当前的位姿为$T_A$,若其绕当前坐标系$n_A{o}_A{a}_A$下做了一个变换，变换矩阵为$T_B$,则其当前位姿为$T_A\ast T_B$;

1.2 摄像机成像模型

1.3 成像相关坐标系定义

• 世界坐标系$(X_w,Y_w,Z_w)$：在环境中由用户任意选择一个基准坐标系作为参考来描述摄像机位置，在相机标定中一般选择标定板坐标系作为世界坐标系。
• 相机坐标系$(X_c,Y_c,Z_c)$：以针孔模型的聚焦中心$O_c$（光心）为原点，摄像机光轴为 Z 轴建立三维坐标系，其中$X_c$轴、$Y_c$轴与成像平面 x、y 轴平 行。光心到图像平面的距离为有效焦距 $f$。
• **成像平面坐标系$(x,y)$：**光轴与成像平面交点，称为主点。成像平面坐标系以主点为原点，是以物理单位表示的平面直角坐标系。
• **图像坐标系$(u,v)$：**固定在图像上以像素为单位的平面直角坐标系， 通常以图像左上角为像素原点，每一像素的$(u,v)$分别表示该像素在图像数组中的行数和列数。u、v 轴分别与 x、y 轴平行，该坐标系的单位为像素。

1.4 图像坐标系到相机坐标系的转换关系

如上图所示，设在图像坐标系有一点A坐标为$[u,v{]}^T$，在成像平面坐标系有一点B坐标为$[X^{\prime },Y^{\prime },Z^{\prime }{]}^T$，相机坐标系有一点C坐标为$[X,Y,Z{]}^T$。我们的问题可以转化为求A点坐标与C点坐标之间的变换关系。

首先，由相似三角形定理我们可以知道，
$$\begin{gathered} X^{\prime }=f\frac{X}{Z} \\ {Y}^{\prime }=f\frac{Y}{Z} \end{gathered}$$
其次，由像素平面和物理成像平面的关系有：
$$\{\begin{array}{c}u=\alpha X^{\prime }+c_x\\ v=\beta Y^{\prime }+c_y\end{array}$$

将（2）代入（3）得：
$$\begin{gathered} u=f_x\frac{X}{Z}+c_x \\ v=f_y\frac{Y}{Z}+c_y \end{gathered}$$
将（4）写成矩阵的形式：
$$Z\left(\begin{array}{c}u\\ v\\ 1\end{array}\right)=\left(\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right)\left(\begin{array}{c}X\\ Y\\ Z\end{array}\right)$$
（5）式就是我们所需要的变换关系，其中，矩阵$\left(\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right)$就是摄像机的内参，后面我们会用标定的方法得到该矩阵。

**小结：**通过上述的理论推导，我们可以得到图片上像素点坐标与相机坐标系上坐标点的变换关系。

1.5 摄像机标定

当我们在拍照时，其实是将三维空间下的3D坐标转化为平面图像中的2D坐标，那么从三维到二维，这之间的转换关系是怎么样呢？摄像机标定解决的就是这个事。具体的标定方法我会在下一次的博客中向大家介绍。这里只需要知道通过相机标定我们可以获得相机的内参和外参。

那么内参和外参又是干什么用的呢？首先我们来梳理一下这其中的坐标变换。

标定板坐标系->相机坐标系->成像平面坐标系->图像坐标系

从1.4节我们可以看到，内参参与了从相机坐标系到图像坐标系的变换（公式（5）），很容易猜到，外参就是描述从标定板坐标系到相机坐标系的变换关系。

1.6 手眼标定

从摄像机标定我们可以得到目标在摄像机坐标系下的坐标变换关系，而对于一个机械臂抓取问题来说，我们还需要知道目标物体在机器人坐标系下的坐标，这就需要手眼标定，得到摄像机与工具手的变换关系。

设 $G$ 为末端执行器（工具手）坐标系，$B$ 为标定板坐标系，$C$ 为摄像机坐标系， $W$ 为机器人坐标系。

变换关系为：
$${}_B^W\text{T}{=}_G^W\text{T}{\ast }_C^G\text{T}{\ast }_B^C\text{T}$$
其中，${}_B^W\text{T}$为标定板相对于机器人的位姿关系，当标定板不动时， ${}_B^W\text{T}$为定量保持不 变；${}_G^W\text{T}$是执行器相对于机器人的位姿关系，由机器人控制器给出；${}_C^G\text{T}$是摄像机相对于末端执行器的位姿关系，即为手眼标定所求量； ${}_B^C\text{T}$是标定板相对于摄像机 的位姿关系，即摄像机标定所得的外参。

我们的问题可以转化为如何求出${}_C^G\text{T}$？

保持标定板不动，机器人运动到两个不同的位置下，则：
$${}_B^W{\text{T}}_1{=}_B^W{\text{T}}_2$$
所以：
$${}_G^W{\text{T}}_1{\ast }_C^G{\text{T}}_1{\ast }_B^C{\text{T}}_1{=}_G^W{\text{T}}_2{\ast }_C^G{\text{T}}_2{\ast }_B^C{\text{T}}_2$$
简化为：
$$AX=XB$$
其中：$A={(}_G^W{\text{T}}_2{)}^{-1}{\ast }_G^W{\text{T}}_1$，$B{=}_G^W{\text{T}}_2\ast {(}_G^W{\text{T}}_1{)}^{-1}$，$X{=}_C^G\text{T}$

从上式可以看出，A 为工具手两次运动对应变换矩阵，B 为摄像机两次运动对应的变换矩阵。

我们的问题可以转化为如何解矩阵方程（9）？

为了实现唯一解，需要知道两组 A 和 B，需要3个位置的摄像机标定结果。

step1:

保持标定板不动，从三个不同的位置拍摄标定板采集图像，三个位置分别命名为：初始位置、位置1、位置2。如下图：

![1

step2:

由机器人控制器提供的末端位姿经过变换可以得到机器人的三个位姿矩阵$H_0、H_1、H_2$，由此可得工具手两次运动对应变换矩阵$A_1$、$A_2$（$A_1$：初始位置到位置1，$A_1$：初始位置到位置2）
$$\begin{gathered} A_1=(H_1{)}^{-1}\ast H_0 \\ {A}_2=(H_2{)}^{-1}\ast H_0 \end{gathered}$$
step3:

利用相机标定得到初始位置、位置1、位置2分别对应的外参矩阵$M_0、M_1、M_2$，由此可得摄像机两次运动对应的变换矩阵$B_1$、$B_2$（$B_1$：初始位置到位置1， $B_2$：初始位置到位置2）：B −，B −
$$\begin{gathered} B_1=M_1\ast M_0^{-1} \\ {B}_2=M_2\ast M_0^{-1} \end{gathered}$$
step4:

最后求出手眼关系矩阵 ${}_C^G\text{T}$

本人数学功底有限，以下内容摘自他人。

参考博客：

https://blog.csdn.net/weixin_41074793/article/details/89477476?utm_medium=distribute.pc_aggpage_search_result.none-task-blog-2_alltop_click~default-1-89477476.nonecase&utm_term=%E6%89%8B%E7%9C%BC%E6%A0%87%E5%AE%9A%E7%AE%97%E6%B3%95

下面利用李群知识求解$AX=XB$

首先，将$X$移到方程的右边，得到：
$$A=XB{X}^T$$
根据李代数到李群的指数映射关系，在方程两边取对数，则可以得到：
$$logA=logXB{X}^T$$
令$logA=[\alpha ]$，$logB=[\beta ]$,则上式可以化为$[\alpha ]=X[\beta ]X^T=[X\beta ]$,从而
$$\alpha =X\beta$$
$AX=XB$**写成矩阵形式为：

$$\left[\begin{array}{cc}{\theta }_A& b_A\\ 0& 1\end{array}\right]\left[\begin{array}{cc}{\theta }_X& b_X\\ 0& 1\end{array}\right]=\left[\begin{array}{cc}{\theta }_X& b_X\\ 0& 1\end{array}\right]\left[\begin{array}{cc}{\theta }_B& b_B\\ 0& 1\end{array}\right]$$

展开得:

$$\begin{gathered} {\theta }_A{\theta }_X={\theta }_X{\theta }_B \\ {\theta }_A{b}_X+b_A={\theta }_X{b}_B+b_X \end{gathered}$$
采用“两步法“求解上述方程，先解算旋转矩阵，再求平移向量。

由上面推导$\alpha =X\beta$,因此${\theta }_A{\theta }_X={\theta }_X{\theta }_B$可以写成$\alpha ={\theta }_X\beta$.

当存在多组观测值时，求解该方程转化为下面最小二乘拟合问题：
$$min\sum _{i=1}^k\Vert {\theta }_X{\beta }_i-{\alpha }_i\Vert$$
上述是一个典型的绝对定向问题，因而求解上式与绝对定向相同，其解为：
$${\theta }_X=(M^{T}M{)}^{-\frac{1}{2}}{M}^T$$
其中，$M={\sum }_{i=1}^k{\beta }_i{\alpha }_i^T$

当只有两组$A、B$时，即有$A_1、A_2、B_1、B_2$,
$${\alpha }_1=log{A}_1,{\alpha }_2=log{A}_2,{\beta }_1=log{B}_1,{\beta }_2=log{B}_2$$
旋转矩阵的解为：
$${\theta }_X=M{N}^{-1}$$
其中，$M=（{\alpha }_1{\alpha }_2{\alpha }_1\times {\alpha }_2)$，$N=（{\beta }_1{\beta }_2{\beta }_1\times {\beta }_2)$

求得旋转矩阵后，根据式（16）算出平移向量。

没有学过李代数，上述式（13）（14）并没有弄懂，有大佬看懂可以评论区解答！

源码：

//Hgij为机器人执行器末端坐标系之间相对位置姿态的齐次变换矩阵；[A1,A2]

//Hcij为摄像机坐标系之间相对位置姿态的齐次变换矩阵；[B1,B2]
	
//Hcg为相机与机器人执行器末端之间的相对位置姿态齐次矩阵。

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;

}

gij - 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;

}