经典手眼标定算法之Tsai-Lenz的OpenCV实现
来源:互联网 发布:淘宝风筝 编辑:程序博客网 时间:2024/05/16 09:13
本文主要是讲解经典手眼标定问题中的TSAI-LENZ 文献方法,参考文献为“A New Technique for Fully Autonomous and Efficient 3D Robotics Hand/Eye Calibration”,并且实现了基于OpenCV的C++代码程序,code可去CSDN资源下载,MATLAB版本作者为苏黎世理工的Christian Wengert,也可在此处下载。
手眼标定问题描述
在机器人校准测量、机器人手眼协调以及机器人辅助测量等领域,都要求知道机器人执行器末端(抓取臂)坐标系和传感器(比如用来测量三维空间中目标位置和方向并固定在机器人手上的摄像机)坐标系之间的相互关系,确定这种转换关系在机器人领域就是通常所说的手眼标定。
将手眼标定系统如下图所示,其中
经过坐标系变换,
将上式展开,可以得到手眼标定的基本方程:
因此,手眼标定问题也就转化为从上述方程组中求解出
“两步法”手眼标定
一般用“两步法”求解基本方程,即先从基本方程上式求解出
Step1:利用罗德里格斯变换将旋转矩阵转换为旋转向量
Step2:向量归一化
Step3:修正的罗德里格斯参数表示姿态变化
Step4:计算初始旋转向量P′cg
其中,skew为反对称运算,假设一个三维向量
Step5:计算旋转向量Pcg
Step6:计算旋转矩阵Rcg
Step7:计算平移向量Tcg
算法源代码
根据上述基本计算步骤,在利用OpenCV 2.0开源库的基础上,编写Tsai手眼标定方法的c++程序,其实现函数代码如下:
void Tsai_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 rgij(3, 1, CV_64FC1); Mat rcij(3, 1, CV_64FC1); double theta_gij; double theta_cij; Mat rngij(3, 1, CV_64FC1); Mat rncij(3, 1, CV_64FC1); Mat Pgij(3, 1, CV_64FC1); Mat Pcij(3, 1, CV_64FC1); Mat tempA(3, 3, CV_64FC1); Mat tempb(3, 1, CV_64FC1); Mat A; Mat b; Mat pinA; Mat Pcg_prime(3, 1, CV_64FC1); Mat Pcg(3, 1, CV_64FC1); Mat PcgTrs(1, 3, CV_64FC1); Mat Rcg(3, 3, CV_64FC1); Mat eyeM = Mat::eye(3, 3, CV_64FC1); Mat Tgij(3, 1, CV_64FC1); Mat Tcij(3, 1, CV_64FC1); Mat tempAA(3, 3, CV_64FC1); Mat tempbb(3, 1, CV_64FC1); Mat AA; Mat bb; Mat pinAA; Mat Tcg(3, 1, CV_64FC1); 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, rgij); Rodrigues(Rcij, rcij); theta_gij = norm(rgij); theta_cij = norm(rcij); rngij = rgij / theta_gij; rncij = rcij / theta_cij; Pgij = 2 * sin(theta_gij / 2)*rngij; Pcij = 2 * sin(theta_cij / 2)*rncij; tempA = skew(Pgij + Pcij); tempb = Pcij - Pgij; A.push_back(tempA); b.push_back(tempb); } //Compute rotation invert(A, pinA, DECOMP_SVD); Pcg_prime = pinA * b; Pcg = 2 * Pcg_prime / sqrt(1 + norm(Pcg_prime) * norm(Pcg_prime)); PcgTrs = Pcg.t(); Rcg = (1 - norm(Pcg) * norm(Pcg) / 2) * eyeM + 0.5 * (Pcg * PcgTrs + sqrt(4 - norm(Pcg)*norm(Pcg))*skew(Pcg)); //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); tempAA = Rgij - eyeM; tempbb = Rcg * Tcij - Tgij; AA.push_back(tempAA); bb.push_back(tempbb); } invert(AA, pinAA, DECOMP_SVD); Tcg = pinAA * bb; Rcg.copyTo(Hcg(Rect(0, 0, 3, 3))); Tcg.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;}
- 经典手眼标定算法之Tsai-Lenz的OpenCV实现
- 经典手眼标定算法之Tsai-Lenz的OpenCV实现
- 经典手眼标定算法之Tsai-Lenz
- 经典手眼标定算法之Navy的OpenCV实现
- 经典手眼标定算法之Navy
- 手眼标定的两种方式
- Halcon学习(五)手眼标定之眼在手外
- 经典:传统相机标定方法解析:直接线性法和Tsai两步标定法
- Opencv双目标定的实现
- TSAI与Zhang标定程序
- Tsai两步标定法
- Tsai两步标定法
- 手眼标定中的closed chain
- opencv实现摄像机标定(张正友的标定方法)
- OpenCV 张正友标定法的实现
- Opencv摄像机的标定
- (5-III) 机器人手眼标定(手眼标定系列-仅限内部学员) (黑土 Q
- 张正友相机标定 Opencv实现
- 用 Scrapy+Mariadb 实现汉典数据爬虫(二)——Scrapy简单介绍
- 最小生成树(一)....Prme算法
- Maven 版本对应的 JDK 版本
- 51nod 1770 数数字
- hibernate 和jdbc
- 经典手眼标定算法之Tsai-Lenz的OpenCV实现
- 染纱厂ERP系统截图(2)
- php小总结
- ubuntu 12.04 LTS 如何使用更快的更新源
- 创建,删除,显示数据库
- hdu 3836 Equivalent Sets【强连通Kosaraju+缩点染色】
- [Matlab] MxArray 与 MwArray 使用区别
- TCP/IP详解
- 运输层协议概述