手眼標定演算法---Navy演算法（Robot sensor calibration: solving AX=XB on the Euclidean group）
本文主要介紹Frank C. Park and Bryan J. Martin在文獻Robot sensor calibration: solving AX=XB on the Euclidean group中提出的手眼標定演算法，該演算法也被稱為Navy手眼標定演算法，該演算法的主要創新點為利用李群理論的知識來求解手眼標定經典方程。該演算法基於OpenCV的C++版本程式可去CSDN資源下載，MATLAB版本作者為蘇黎世理工的Christian Wengert，也可在此處下載。轉自：https://blog.csdn.net/YunlinWang/article/details/51871520
正如Tsai等在文獻中指出的，手眼標定問題其實就是求解AX=XB方程問題。其中AA為機器人末端連桿座標架在機器人-攝像機系統移動前後的轉換關係，B為攝像機座標架在移動前後的相對關係。Tsai指出要唯一確定手眼矩陣的各分量，至少需要旋轉軸不平行的兩組運動。由於在觀測中一般存在噪聲，因此在實際測量中一般需要多組運動來求解該方程。
假設有多組觀測值{(A1,B1),(A2,B2),⋯,(Ak,Bk)}，求解AX=XB方程可以轉化為如下最小化問題
其中，d表示在歐式群上的距離測度。利用李群理論知識可以將上述最小化問題轉化為最小二乘擬合問題，從而可以得到簡單而又明確的解。下面介紹李群中的基本知識。
李群基本知識
描述剛體運動可以用歐式群表示，由如下形式的矩陣表示：
其中，θ∈SO(3),b∈ℜ3，SO(3)表示旋轉矩陣組成的群。
每個李群
指數對映
李代數到李群的轉換滿足指數對映關係，假設[w]∈so(3)，而exp[w]∈SO(3)，則其指數對映滿足羅德里格斯公式：
對數對映
李群到李代數的轉換滿足對數對映關係，假設θ∈SO(3)，則其對數對映為：
其中，1+2cosϕ=tr(θ)。
利用李群知識求解AX=XB
我們知道，手眼標定問題其實就是求解方程AX=XB，將X移到方程的右邊，可以得到
令 logA=[α],logB=[β]，則上式可以化為[α]=X[β]XT=[Xβ]，從而
α=Xβ
AX=XB寫成矩陣形式為
將上式展開可以得到
與Tsai手眼標定類似，同樣採用“兩步法”求解上述方程，先解算旋轉矩陣，再求得平移向量。由上面推導 α=Xβ，因此 θAθX=θXθB可以寫成 α=θXβ。當存在多組觀測值時，求解該方程可以轉化為下面最小二乘擬合問題：
很顯然，上述問題是典型的絕對定向問題，因而求解上式與絕對定向相同，其解為
其中，
然而，上式的求解是有條件的，即MM不奇異，因而當AA、BB只有兩組時，不能用上述方法求解。
當只有兩組A、B時，即有A1,A2,B1,B2，
旋轉矩陣的解為：
其中，
在求得旋轉矩陣後，求解平移向量的步驟與Tsai手眼標定相同，讀者可以檢視上篇文獻，這裡就不在贅述。
演算法原始碼
根據上述基本計算步驟，在利用OpenCV 2.0開源庫的基礎上，編寫Tsai手眼標定方法的c++程式，其實現函式程式碼如下：
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;
}