经典手眼标定算法之Navy
来源:互联网 发布:苹果系统php集成环境 编辑:程序博客网 时间:2024/06/01 10:10
文章转载自:经典手眼标定算法之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;}
阅读全文
0 0
- 经典手眼标定算法之Navy
- 经典手眼标定算法之Navy的OpenCV实现
- 经典手眼标定算法之Tsai-Lenz
- 经典手眼标定算法之Tsai-Lenz的OpenCV实现
- 经典手眼标定算法之Tsai-Lenz的OpenCV实现
- Halcon学习(五)手眼标定之眼在手外
- 手眼标定中的closed chain
- 手眼标定的两种方式
- (5-III) 机器人手眼标定(手眼标定系列-仅限内部学员) (黑土 Q
- 经典算法之汉诺塔
- 经典算法之查找
- 经典算法之:贪心算法
- 经典算法之-----贪心算法
- 算法基础之经典算法
- 经典算法之Prim算法
- 经典算法之Kruskal算法
- 张正友标定算法
- IMU标定算法流程
- 如何提高程序员学习能力?
- 一 汇编指令与C语言
- 解决mac版本的Navicat链接oracle报错 ORA-21561: OID generation failed
- Python机器学习案例之朴素贝叶斯分类--文本、邮件分类
- 经典手眼标定算法之Tsai-Lenz
- 经典手眼标定算法之Navy
- win10+cuda+cudnn
- Web前端H5之HTML标签总结(一)
- 二 C语言的流程与处理
- Using OpenMP (三) 同步
- 三 练习反汇编C语言程序
- [转载浏览]HTTP 缓存机制
- python内置模块(loggin、os)
- Python数据分析利器——pandas简单教学