文章轉載自：經典手眼標定算法之Navy的OpenCV實現 ：http://blog.csdn.net/yunlinwang/article/details/51871520

在我的上一篇博客中已經介紹了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，也可在此處下載。

?

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 rotationif (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;}

總結

以上是生活随笔為你收集整理的经典手眼标定算法之Navy的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。