已有CAMERA下的DH参数，标定TCP到MARKER

   控制机器人在base坐标系下沿着base的X轴正向运动，并在camera坐标中测量，可以得到base坐标系的X方向向量在camera中的描述；

   另外，在DH参数标定中可以确定轴1的Z轴方向，将此方向作为base的Z轴方向，由右手定则可以确定base的姿态 $R_B$。在camera系中，设base的位置为$T_B$，则其齐次矩阵可表示为：
$$M_B=\left[\begin{array}{cc}{R}_B& T_B\\ 0& 1\end{array}\right]$$

   从XarmAPI的getposition方法可以获取TCP相对于base的齐次矩阵：
$$M_T=\left[\begin{array}{cc}{R}_T& T_T\\ 0& 1\end{array}\right]$$

   而根据DH模型可以得到camera下Marker的齐次矩阵：
$$M_M=\left[\begin{array}{cc}{R}_M& T_M\\ 0& 1\end{array}\right]$$

   设Marker在TCP坐标中的旋转和位置为Q和X并记其齐次矩阵为P，则有以下关系：
$$M_B.M_T.P=M_M$$
   使用分块矩阵乘法展开,并联立两次测量的数据可以得到：
$$Q=(R_B.R_T{)}^{-1}.R_M$$
$$X=[R_B.(R_T^1-R_T^2){]}^{-1}.[(T_M^1-T_M^2)-R_B(T_T^1-T_T^2)]$$

   实际计算中发现，任何两个旋转矩阵之差秩为2（运用正交矩阵特征值为1以及正交矩阵有群结构可以证明这一点），故上式中无法求逆。为解决这个问题，只需要测量第三组数据，得到
$$R_B.(R_T^1-R_T^3)和[(T_M^1-T_M^3)-R_B(T_T^1-T_T^3)]$$

   使用这两个矩阵的第一行去替换前面相应矩阵的第三行，得到满秩的矩阵，继续计算即可得到TCP到MARKER的矩阵，唯一未求出的$T_B$ 可得到：
$$T_B=T_M-R_B.R_T.X-R_B.T_T$$