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

AX=XB李群李代数知识求解方程相关推荐

opencv4.2中求解手眼标定方程AX=XB
opencv函数: void cv::calibrateHandEye (InputArrayOfArrays R_gripper2base, InputArrayOfArrays t_gripper ...
手眼标定AX=XB求解方法（文献总结）
本人在看论文时碰到了手眼标定问题,看了几篇论文,顺便来总结一下关于公式AX=XB的解决方法和论文的内容. 在手眼标定问题中,AX=XB公式的求解方法很关键,手眼即机器人末端和相机,两者有两种组合形式: ...
手眼标定eye-in-hand（一）AX=XB方程推导
1. 前言本文主要介绍eye-in-hand的手眼标定,引用OpenCV的文档原话和原图1:"The following picture describes the Hand-Eye ca ...
Matlab求解AX=XB（手眼标定用）
描述使用matlab求解AX=XB的问题可以正确求出X的值,我已经成功验证了至于说X具体代表什么,要结合你自己推导出的公式AX=XB之中,A.X.B分别代表什么代码 main.m 包含我做手眼 ...
SLAM中相机位姿求解（李群李代数）
前言 slam中一个关键问题之一就是求解相机的位姿,人们找了很多以相机位姿为变量的误差函数,比如光度误差,重投影误差,3D几何误差等等,希望使得误差最小,进而求得比较准确的相机位姿.举一个重投影例子: ...
手眼标定算法---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 g ...
数学小抄: 李群李代数再回顾 [SLAM十四讲]
前言最近阅读了高翔老师的视觉SLAM十四讲, 也算是了解了当下机器人领域最炙热的机器人领域前沿研究问题大概是怎么样的数学问题. 不得不说视频与书籍真的是深入浅出. 所有第三库无痛安装教程: 知乎链接 ...
李群李代数简明教程
李群李代数简明教程 slam算法中往往需要用到李群李代数的一些知识,最近看到一个李群李代数的简明教程,就翻译下来,有些地方翻译的可能不准,主要是作为一个学习记录. 文章目录李群李代数简明教程 2 ...
多视图几何的数学基础知识的掌握（2）--李群李代数
文章目录前言多视图几何的数学基础知识的掌握(2)--李群李代数三维重建的起源三维欧式空间叉乘与反对称阵刚体运动刚体运动的表示旋转的指数坐标李群$SO(3)$ 李群与李代数李群定义 ...

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

AX=XB李群李代数知识求解方程相关推荐

最新文章

热门文章