经典手眼标定算法之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=XBAX=XB方程问题。其中AA为机器人末端连杆坐标架在机器人-摄像机系统移动前后的转换关系，BB为摄像机坐标架在移动前后的相对关系。Tsai指出要唯一确定手眼矩阵的各分量，至少需要旋转轴不平行的两组运动。由于在观测中一般存在噪声，因此在实际测量中一般需要多组运动来求解该方程。

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

min∑i=1kd(AiX,XBi)min∑i=1kd(AiX,XBi)

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

李群基本知识

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

[θ0b1][θb01]

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

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

指数映射

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

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

对数映射

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

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

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

利用李群知识求解AX=XB

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

logA=logXBXTlog⁡A=log⁡XBXT

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

α=Xβα=Xβ

AX=XBAX=XB写成矩阵形式为

[θA0bA1][θX0bX1]=[θX0bX1][θB0bB1][θAbA01][θXbX01]=[θXbX01][θBbB01]

将上式展开可以得到

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

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

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

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

θX=(MTM)−12MTθX=(MTM)−12MT

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

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

当只有两组AA、BB时，即有A1,A2,B1,B2A1,A2,B1,B2，

α1=logA1,α2=logA2,β1=logB1,β2=logB2α1=log⁡A1,α2=log⁡A2,β1=log⁡B1,β2=log⁡B2

旋转矩阵的解为：

θX=MN−1θX=MN−1

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

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

算法源代码

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

<span style="color:#000000"><code><span style="color:#000088 !important">void</span> Navy_HandEye(Mat Hcg, <span style="color:#4f4f4f !important">vector</span><Mat> Hgij, <span style="color:#4f4f4f !important">vector</span><Mat> Hcij)
{CV_Assert(Hgij.size() == Hcij.size());<span style="color:#000088 !important">int</span> nStatus = Hgij.size();Mat Rgij(<span style="color:#006666 !important">3</span>, <span style="color:#006666 !important">3</span>, CV_64FC1);Mat Rcij(<span style="color:#006666 !important">3</span>, <span style="color:#006666 !important">3</span>, CV_64FC1);Mat alpha1(<span style="color:#006666 !important">3</span>, <span style="color:#006666 !important">1</span>, CV_64FC1);Mat beta1(<span style="color:#006666 !important">3</span>, <span style="color:#006666 !important">1</span>, CV_64FC1);Mat alpha2(<span style="color:#006666 !important">3</span>, <span style="color:#006666 !important">1</span>, CV_64FC1);Mat beta2(<span style="color:#006666 !important">3</span>, <span style="color:#006666 !important">1</span>, CV_64FC1);Mat A(<span style="color:#006666 !important">3</span>, <span style="color:#006666 !important">3</span>, CV_64FC1);Mat B(<span style="color:#006666 !important">3</span>, <span style="color:#006666 !important">3</span>, CV_64FC1);Mat alpha(<span style="color:#006666 !important">3</span>, <span style="color:#006666 !important">1</span>, CV_64FC1);Mat beta(<span style="color:#006666 !important">3</span>, <span style="color:#006666 !important">1</span>, CV_64FC1);Mat M(<span style="color:#006666 !important">3</span>, <span style="color:#006666 !important">3</span>, CV_64FC1, Scalar(<span style="color:#006666 !important">0</span>));Mat MtM(<span style="color:#006666 !important">3</span>, <span style="color:#006666 !important">3</span>, CV_64FC1);Mat veMtM(<span style="color:#006666 !important">3</span>, <span style="color:#006666 !important">3</span>, CV_64FC1);Mat vaMtM(<span style="color:#006666 !important">3</span>, <span style="color:#006666 !important">1</span>, CV_64FC1);Mat pvaM(<span style="color:#006666 !important">3</span>, <span style="color:#006666 !important">3</span>, CV_64FC1, Scalar(<span style="color:#006666 !important">0</span>));Mat Rx(<span style="color:#006666 !important">3</span>, <span style="color:#006666 !important">3</span>, CV_64FC1);Mat Tgij(<span style="color:#006666 !important">3</span>, <span style="color:#006666 !important">1</span>, CV_64FC1);Mat Tcij(<span style="color:#006666 !important">3</span>, <span style="color:#006666 !important">1</span>, CV_64FC1);Mat eyeM = Mat::eye(<span style="color:#006666 !important">3</span>, <span style="color:#006666 !important">3</span>, CV_64FC1);Mat tempCC(<span style="color:#006666 !important">3</span>, <span style="color:#006666 !important">3</span>, CV_64FC1);Mat tempdd(<span style="color:#006666 !important">3</span>, <span style="color:#006666 !important">1</span>, CV_64FC1);Mat C;Mat d;Mat Tx(<span style="color:#006666 !important">3</span>, <span style="color:#006666 !important">1</span>, CV_64FC1);<span style="color:#880000 !important"><em>//Compute rotation</em></span><span style="color:#000088 !important">if</span> (Hgij.size() == <span style="color:#006666 !important">2</span>) <span style="color:#880000 !important"><em>// Two (Ai,Bi) pairs</em></span>{Rodrigues(Hgij[<span style="color:#006666 !important">0</span>](Rect(<span style="color:#006666 !important">0</span>, <span style="color:#006666 !important">0</span>, <span style="color:#006666 !important">3</span>, <span style="color:#006666 !important">3</span>)), alpha1);Rodrigues(Hgij[<span style="color:#006666 !important">1</span>](Rect(<span style="color:#006666 !important">0</span>, <span style="color:#006666 !important">0</span>, <span style="color:#006666 !important">3</span>, <span style="color:#006666 !important">3</span>)), alpha2);Rodrigues(Hcij[<span style="color:#006666 !important">0</span>](Rect(<span style="color:#006666 !important">0</span>, <span style="color:#006666 !important">0</span>, <span style="color:#006666 !important">3</span>, <span style="color:#006666 !important">3</span>)), beta1);Rodrigues(Hcij[<span style="color:#006666 !important">1</span>](Rect(<span style="color:#006666 !important">0</span>, <span style="color:#006666 !important">0</span>, <span style="color:#006666 !important">3</span>, <span style="color:#006666 !important">3</span>)), beta2);alpha1.copyTo(A.col(<span style="color:#006666 !important">0</span>));alpha2.copyTo(A.col(<span style="color:#006666 !important">1</span>));(alpha1.cross(alpha2)).copyTo(A.col(<span style="color:#006666 !important">2</span>));beta1.copyTo(B.col(<span style="color:#006666 !important">0</span>));beta2.copyTo(B.col(<span style="color:#006666 !important">1</span>));(beta1.cross(beta2)).copyTo(B.col(<span style="color:#006666 !important">2</span>));Rx = A*B.inv();}<span style="color:#000088 !important">else</span> <span style="color:#880000 !important"><em>// More than two (Ai,Bi) pairs</em></span>{<span style="color:#000088 !important">for</span> (<span style="color:#000088 !important">int</span> i = <span style="color:#006666 !important">0</span>; i < nStatus; i++){Hgij[i](Rect(<span style="color:#006666 !important">0</span>, <span style="color:#006666 !important">0</span>, <span style="color:#006666 !important">3</span>, <span style="color:#006666 !important">3</span>)).copyTo(Rgij);Hcij[i](Rect(<span style="color:#006666 !important">0</span>, <span style="color:#006666 !important">0</span>, <span style="color:#006666 !important">3</span>, <span style="color:#006666 !important">3</span>)).copyTo(Rcij);Rodrigues(Rgij, alpha);Rodrigues(Rcij, beta);M = M + beta*alpha.t();}MtM = M.t()*M;eigen(MtM, vaMtM, veMtM);pvaM.at<<span style="color:#000088 !important">double</span>>(<span style="color:#006666 !important">0</span>, <span style="color:#006666 !important">0</span>) = <span style="color:#006666 !important">1</span> / <span style="color:#4f4f4f !important">sqrt</span>(vaMtM.at<<span style="color:#000088 !important">double</span>>(<span style="color:#006666 !important">0</span>, <span style="color:#006666 !important">0</span>));pvaM.at<<span style="color:#000088 !important">double</span>>(<span style="color:#006666 !important">1</span>, <span style="color:#006666 !important">1</span>) = <span style="color:#006666 !important">1</span> / <span style="color:#4f4f4f !important">sqrt</span>(vaMtM.at<<span style="color:#000088 !important">double</span>>(<span style="color:#006666 !important">1</span>, <span style="color:#006666 !important">0</span>));pvaM.at<<span style="color:#000088 !important">double</span>>(<span style="color:#006666 !important">2</span>, <span style="color:#006666 !important">2</span>) = <span style="color:#006666 !important">1</span> / <span style="color:#4f4f4f !important">sqrt</span>(vaMtM.at<<span style="color:#000088 !important">double</span>>(<span style="color:#006666 !important">2</span>, <span style="color:#006666 !important">0</span>));Rx = veMtM*pvaM*veMtM.inv()*M.t();}<span style="color:#880000 !important"><em>//Computer Translation </em></span><span style="color:#000088 !important">for</span> (<span style="color:#000088 !important">int</span> i = <span style="color:#006666 !important">0</span>; i < nStatus; i++){Hgij[i](Rect(<span style="color:#006666 !important">0</span>, <span style="color:#006666 !important">0</span>, <span style="color:#006666 !important">3</span>, <span style="color:#006666 !important">3</span>)).copyTo(Rgij);Hcij[i](Rect(<span style="color:#006666 !important">0</span>, <span style="color:#006666 !important">0</span>, <span style="color:#006666 !important">3</span>, <span style="color:#006666 !important">3</span>)).copyTo(Rcij);Hgij[i](Rect(<span style="color:#006666 !important">3</span>, <span style="color:#006666 !important">0</span>, <span style="color:#006666 !important">1</span>, <span style="color:#006666 !important">3</span>)).copyTo(Tgij);Hcij[i](Rect(<span style="color:#006666 !important">3</span>, <span style="color:#006666 !important">0</span>, <span style="color:#006666 !important">1</span>, <span style="color:#006666 !important">3</span>)).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(<span style="color:#006666 !important">0</span>, <span style="color:#006666 !important">0</span>, <span style="color:#006666 !important">3</span>, <span style="color:#006666 !important">3</span>)));Tx.copyTo(Hcg(Rect(<span style="color:#006666 !important">3</span>, <span style="color:#006666 !important">0</span>, <span style="color:#006666 !important">1</span>, <span style="color:#006666 !important">3</span>)));Hcg.at<<span style="color:#000088 !important">double</span>>(<span style="color:#006666 !important">3</span>, <span style="color:#006666 !important">0</span>) = <span style="color:#006666 !important">0.0</span>;Hcg.at<<span style="color:#000088 !important">double</span>>(<span style="color:#006666 !important">3</span>, <span style="color:#006666 !important">1</span>) = <span style="color:#006666 !important">0.0</span>;Hcg.at<<span style="color:#000088 !important">double</span>>(<span style="color:#006666 !important">3</span>, <span style="color:#006666 !important">2</span>) = <span style="color:#006666 !important">0.0</span>;Hcg.at<<span style="color:#000088 !important">double</span>>(<span style="color:#006666 !important">3</span>, <span style="color:#006666 !important">3</span>) = <span style="color:#006666 !important">1.0</span>;}</code></span>