AX=XB李群李代数知识求解方程

文章转载自：经典手眼标定算法之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 alpha(3, 1, CV_64FC1);

Mat beta(3, 1, CV_64FC1);

Mat M(3, 3, CV_64FC1, Scalar(0));

Mat veMtM(3, 3, CV_64FC1);

Mat vaMtM(3, 1, CV_64FC1);

Mat pvaM(3, 3, CV_64FC1, Scalar(0));

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);

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.cross(beta2)).copyTo(B.col( 2));

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);

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();

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);

tempdd = Tgij - Rx * Tcij;

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;

1

网址：AX=XB李群李代数知识求解方程 https://www.yuejiaxmz.com/news/view/36270

相关内容

李小龙健身秘籍全公开
吃不完的李子=李子酱
李彦宏：八大关键技术推动数字经济发展
李渔的生活美学思想
求由方程siny=ln（x+y）确定的函数求详解求由方程si 爱问知识人
本体论的生活美学──杜威与李泽厚思想比照
合肥桃李春风楼盘降价了吗
李嘉诚的时间管理秘诀
李倩被封美容天后 代言护肤品透露护肤秘籍
暑假香港旅游出行攻略，如何解决行李过多的难题？

随便看看