手眼标定公式推理和代码，Tsai算法和Navy算法

​
最近因为工作原因需要用到手眼标定相关知识，在网上查了一番大部分文章都写得似懂非懂，感觉都是从别处copy过来，很少有人能清晰的描述整个完整流程，因此在这里对手眼标定的原理进行自己的推理，代码也在后面附上。

1. 问题建模（AX=XB）

假设$\mathbf{k}$时刻，机械臂相对于世界坐标系的位姿为${\mathbf{A}}_{\mathbf{k}}$ （末端到世界），相机相对于世界坐标系的位姿为 ${\mathbf{B}}_{\mathbf{k}}$ （相机到世界），相机相对于机械臂的位姿$\mathbf{X}$（相机到末端），那么有

$${\mathbf{A}}_{\mathbf{k}}\mathbf{X}={\mathbf{B}}_{\mathbf{k}}$$

同样$\mathbf{k}+1$时刻有

$${\mathbf{A}}_{\mathbf{k}+1}\mathbf{X}={\mathbf{B}}_{\mathbf{k}+1}$$

所有矩阵均属于SE3，两式联立，则有

$$\begin{array}{rl}({\mathbf{A}}_{\mathbf{k}+1}\mathbf{X}{)}^{-1}{\mathbf{A}}_{\mathbf{k}}\mathbf{X}& ={\mathbf{B}}_{\mathbf{k}+1}^{-1}{\mathbf{B}}_{k+1}\\ {\mathbf{A}}_{\mathbf{k}+1}^{-1}{\mathbf{A}}_{\mathbf{k}}\mathbf{X}& =\mathbf{X}{\mathbf{B}}_{\mathbf{k}+1}^{-1}{\mathbf{B}}_{k+1}\end{array}$$

令$\mathbf{A}={\mathbf{A}}_{\mathbf{k}+1}^{-1}{\mathbf{A}}_{\mathbf{k}}$，$\mathbf{B}={\mathbf{B}}_{\mathbf{k}+1}^{-1}{\mathbf{B}}_{\mathbf{k}}$，则有

$$\begin{array}{c}\mathbf{AX}=\mathbf{XB}\end{array}$$

其中$\mathbf{A}$表示末端两个时刻之间的相对位姿态关系，可以由棋盘格或Tag码获取。$\mathbf{B}$表示相机两个时刻之间的相对位姿态关系，可由机械臂读数获取。手眼标定即为利用不同时刻得到的末端和相机位姿来求解$\mathbf{X}$。

$$\mathbf{A}=\left[\begin{array}{cc}{\mathbf{R}}_{\mathbf{A}}& {\mathbf{t}}_{\mathbf{A}}\\ 0& 1\end{array}\right]，\mathbf{B}=[\begin{array}{c}{\mathbf{R}}_{}\end{array}$$