经典手眼标定算法之Tsai-Lenz的OpenCV实现

经典手眼标定算法之Tsai-Lenz的OpenCV实现本文主要是讲解经典手眼标定问题中的TSAI-LENZ文献方法,参考文献为“ANewTechniqueforFullyAutonomousandEfficient3DRoboticsHand/EyeCalibration”,并且实现了基于OpenCV的C++代码程序,code可去CSDN资源下载。

大家好,又见面了,我是你们的朋友全栈君。

       本文主要是讲解经典手眼标定问题中的TSAI-LENZ 文献方法,参考文献为“A New Technique for Fully Autonomous and Efficient 3D Robotics Hand/Eye Calibration”,并且实现了基于OpenCV的C++代码程序,code可去CSDN资源下载,MATLAB版本作者为苏黎世理工的Christian Wengert,也可在此处下载。


手眼标定问题描述

       在机器人校准测量、机器人手眼协调以及机器人辅助测量等领域,都要求知道机器人执行器末端(抓取臂)坐标系和传感器(比如用来测量三维空间中目标位置和方向并固定在机器人手上的摄像机)坐标系之间的相互关系,确定这种转换关系在机器人领域就是通常所说的手眼标定

       将手眼标定系统如下图所示,其中 H g i j H_{gij} Hgij为机器人执行器末端坐标系之间相对位置姿态的齐次变换矩阵; H c i j H_{cij} Hcij为摄像机坐标系之间相对位置姿态的齐次变换矩阵; H c g H_{cg} Hcg为像机与机器人执行器末端之间的相对位置姿态齐次矩阵。

手眼标定系统

       经过坐标系变换, H g i j H_{gij} Hgij H c i j H_{cij} Hcij H c g H_{cg} Hcg满足如下关系:
KaTeX parse error: Unknown column alignment: * at position 77: …{\begin{array}{*̲{20}{c}} {
{R_…

       将上式展开,可以得到手眼标定的基本方程
KaTeX parse error: Unknown column alignment: * at position 24: …{\begin{array}{*̲{20}{c}} {
{R_…

       因此,手眼标定问题也就转化为从上述方程组中求解出 R c g R_{cg} Rcg T c g T_{cg} Tcg,下面就按照TSAI文献所述求解该方程组。


“两步法”手眼标定

       一般用“两步法”求解基本方程,即先从基本方程上式求解出 R c g R_{cg} Rcg,再代入下式求解出 T c g T_{cg} Tcg。在TSAI文献中引入旋转轴-旋转角系统来描述旋转运动来进行求解该方程组,具体的公式推导可以查看原始文献,这里只归纳计算步骤,不明白的地方可阅读文献,计算步骤如下:

Step1:利用罗德里格斯变换将旋转矩阵转换为旋转向量

KaTeX parse error: Unknown column alignment: * at position 24: …{\begin{array}{*̲{20}{c}} {
{r_…

Step2:向量归一化

KaTeX parse error: Unknown column alignment: * at position 24: …{\begin{array}{*̲{20}{c}} {\be…

Step3:修正的罗德里格斯参数表示姿态变化

KaTeX parse error: Unknown column alignment: * at position 24: …{\begin{array}{*̲{20}{c}} {
{P_…

Step4:计算初始旋转向量 P c g ′ P_{cg}^\prime Pcg

s k e w ( P g i j + P c i j ) P c g ′ = P c i j − P g i j skew\left( {
{P_{gij}} + {P_{cij}}} \right){P_{cg}}^\prime = {P_{cij}}- {P_{gij}}
skew(Pgij+Pcij)Pcg=PcijPgij

       其中,skew为反对称运算,假设一个三维向量 V = [ v x ; v y ; v z ] V=[v_x;v_y;v_z] V=[vx;vy;vz],其反对称矩阵为:
KaTeX parse error: Unknown column alignment: * at position 46: …{\begin{array}{*̲{20}{c}} {\be…

Step5:计算旋转向量 P c g P_{cg} Pcg

P c g = 2 P c g ′ 1 + ∣ P c g ′ ∣ 2 {P_{cg}} = \frac{
{2{P_{cg}}^\prime }}{
{\sqrt {1 + {
{\left| {
{P_{cg}}^\prime } \right|}^2}} }}
Pcg=1+Pcg2
2Pcg

Step6:计算旋转矩阵 R c g R_{cg} Rcg

R c g = ( 1 − ∣ P c g ∣ 2 2 ) I + 1 2 ( P c g P c g T + 4 − ∣ P c g ∣ 2 s k e w ( P c g ) ) {R_{cg}} = \left( {1 – \frac{
{
{
{\left| {
{P_{cg}}} \right|}^2}}}{2}} \right)I + \frac{1}{2}\left( {
{P_{cg}}{P_{cg}}^T + \sqrt {4 – {
{\left| {
{P_{cg}}} \right|}^2}} skew\left( {
{P_{cg}}} \right)} \right)
Rcg=(12Pcg2)I+21(PcgPcgT+4Pcg2
skew(Pcg)
)

Step7:计算平移向量 T c g T_{cg} Tcg

( R g i j − I ) T c g = R c g T c i j − T g i j \left( {
{R_{gij}} – I} \right){T_{cg}} = {R_{cg}}{T_{cij}} – {T_{gij}}
(RgijI)Tcg=RcgTcijTgij


算法源代码

       根据上述基本计算步骤,在利用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));
//Compute 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;
}
Mat skew(const Mat A) //Compute the skew symmetric matrix.
{ 

CV_Assert(A.cols == 1 && A.rows == 3);
Mat B(3, 3, CV_64FC1);
B.at<double>(0, 0) = 0.0;
B.at<double>(0, 1) = -A.at<double>(2, 0);
B.at<double>(0, 2) = A.at<double>(1, 0);
B.at<double>(1, 0) = A.at<double>(2, 0);
B.at<double>(1, 1) = 0.0;
B.at<double>(1, 2) = -A.at<double>(0, 0);
B.at<double>(2, 0) = -A.at<double>(1, 0);
B.at<double>(2, 1) = A.at<double>(0, 0);
B.at<double>(2, 2) = 0.0;
return B;
}
版权声明:本文内容由互联网用户自发贡献,该文观点仅代表作者本人。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容, 请发送邮件至 举报,一经查实,本站将立刻删除。

发布者:全栈程序员-用户IM,转载请注明出处:https://javaforall.cn/132187.html原文链接:https://javaforall.cn

【正版授权,激活自己账号】: Jetbrains全家桶Ide使用,1年售后保障,每天仅需1毛

【官方授权 正版激活】: 官方授权 正版激活 支持Jetbrains家族下所有IDE 使用个人JB账号...

(0)
blank

相关推荐

  • 人工神经网络(ANN)及BP算法[通俗易懂]

    人工神经网络(ANN)及BP算法[通俗易懂]1什么是神经网络1.1基本结构说明:通常一个神经网络由一个inputlayer,多个hiddenlayer和一个outputlayer构成。图中圆圈可以视为一个神经元(又可以称为感知器)设计神经网络的重要工作是设计hiddenlayer,及神经元之间的权重添加少量隐层获得浅层神经网络SNN;隐层很多时就是深层神经网络DNN1.2从逻辑回归到神经元LinearRegres

  • pycharm创建python虚拟环境好处_pycharm创建虚拟环境很慢

    pycharm创建python虚拟环境好处_pycharm创建虚拟环境很慢很多时候由于每个项目所需要的库和其版本都不一样,在根目录下运行项目的复杂性会大很多,这是很多人会选择使用虚拟环境,今天给大家介绍一些pycharm中傻瓜式添加虚拟环境的方法。pycharm首页:File=>Settings=>你的项目名下的pythoninterpreter=>设置按钮=>addpythoninterpreter图1在该页面下的VirtualenvEnvironment(虚拟环境设置界面),有两种设置添加虚拟环境的选项。Ne

  • 不止编码,还要好好生活和思考[通俗易懂]

    做一个积极的人编码、改bug、提升自己我有一个乐园,面向编程,春暖花开!2019年4月最后一天,不写技术,谈谈我的乐园,我喜欢的一首诗,分享给你!面朝大海,春暖花开[作者] 海子从明天起,做一个幸福的人喂马,劈柴,周游世界从明天起,关心粮食和蔬菜我有一所房子,面朝大海,春暖花开从明天起,和每一个亲人通信告诉他们我的幸福那幸福的闪电告诉我的我将告诉每一个人给每一条河…

  • kali最新版安装教程_kali linux安卓版安装

    kali最新版安装教程_kali linux安卓版安装百度搜索kali,就是它了顺着箭头安装,建议使用网盘或者IDM下载,浏览器等待时间太长。下载后解压文件夹,然后打开VMware输入默认的虚拟机账号密码,均为kali选择第一个,然后我们的kali就安装好了。之后就是获取root最高权限方便我们使用kaili所有的工具点击openterminalhere输入sudopasswdroot回车,输入原来密码:kali设置新密码.之前的用户会被注销,登录新的账号回到桌面再次点击openterminalhere,可以看到我们的权限已

  • pycharm怎么用啊_我不想用失去来教会你

    pycharm怎么用啊_我不想用失去来教会你一、PyCharm设置中文(无需汉化包)1、点击左上角的File选项2、选择Settings选项3、点击Plugins选项,等待一会儿,页面出来比较慢。

发表回复

您的电子邮箱地址不会被公开。

关注全栈程序员社区公众号