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

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

---

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

       假设有多组观测值$\left\{ {\left( {{A_1},{B_1}} \right),\left( {{A_2},{B_2}} \right), \cdots ,\left( {{A_k},{B_k}} \right)} \right\}$，求解$AX=XB$方程可以转化为如下最小化问题

$$\min \sum\limits_{i = 1}^k {d\left( {{A_i}X,X{B_i}} \right)}$$

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

---

李群基本知识

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

$$\left[ {\begin{array}{*{20}{c}} \theta &b \\ {\mathbf{0}}&1 \end{array}} \right]$$

其中，$\theta \in SO\left( 3 \right),b \in {\Re ^3}$，$SO\left( 3 \right)$表示旋转矩阵组成的群。

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

指数映射

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

exp[w]=I+