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

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

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

手眼标定问题描述

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

       将手眼标定系统如下图所示，其中$H_{gij}$为机器人执行器末端坐标系之间相对位置姿态的齐次变换矩阵；$H_{cij}$为摄像机坐标系之间相对位置姿态的齐次变换矩阵；$H_{cg}$为像机与机器人执行器末端之间的相对位置姿态齐次矩阵。

       经过坐标系变换，$H_{gij}$、$H_{cij}$和$H_{cg}$满足如下关系：
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_{cg}$和$T_{cg}$，下面就按照TSAI文献所述求解该方程组。

“两步法”手眼标定

       一般用“两步法”求解基本方程，即先从基本方程上式求解出$R_{cg}$，再代入下式求解出$T_{cg}$。在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_{cg}^{\prime }$

$$skew\left(P_{gij}+P_{cij}\right){P_{cg}}^{\prime }=P_{cij}-P_{gij}$$

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

Step5：计算旋转向量$P_{cg}$

$$P_{cg}=\frac{2{P_{cg}}^{\prime }}{\sqrt{1+{\left|{P_{cg}}^{\prime }\right|}^2}}$$

Step6：计算旋转矩阵$R_{cg}$

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

Step7：计算平移向量$T_{cg}$

$$\left(R_{gij}-I\right)T_{cg}=R_{cg}{T}_{cij}-T_{gij}$$

算法源代码

       根据上述基本计算步骤，在利用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;
}