利用Tsai-lenz算法实现手眼标定

目录

• • 利用Tsai-lenz算法实现手眼标定
• 自己的理解
• 算法的推导
• 标定数据的获取和使用

自己的理解

手眼标定我主要参考了 link.通过该作者的连续几篇内容，我基本搞清楚了利用Tsai-lenz算法实现手眼标定的基本原理和算法所做的工作。以防后期忘了，因此写出来以便自己查阅。
按照我的理解，手眼标定说白了就是各种坐标系之间的转换。通过坐标系的转换从而使得相机和机械臂的末端可以看作是同一个点。
而要进行手眼标定我们需要进行标定数据的获取。标定数据获取的精确度在上文链接的相关文章中也有提到。按照我自己实践来看，标定数据的精确度与标定的结果有着非常大的联系，影响着标定结果的准确度。

算法的推导

如何计算相机和机械臂末端的变换矩阵（包括旋转矩阵和偏移矩阵）？这方面的原理和推导。上面链接中也已经说明的非常详细了。（哈哈哈，怎么都是看别人的）
在理解之后我们就需要获取标定所需要的数据了。

标定数据的获取和使用

标定的数据需要通过相机和机械臂末端来获取，有的工业机械臂会有示教器？（或许是示教器，也可能记错了。）获取的数据是机械臂相对于世界坐标系的坐标和四元数，具体四元数请参考百度。同样，相机获取的数据也是标定板相对于相机坐标系的坐标和四元数。其实如果获取的是欧拉角也可以，因为四元数和欧拉角是可以相互转化的。但要注意的是，欧拉角可能有多种，机械臂有的是xyz的有的是yzx。（也许是这样，没有考证，凭记忆写的，需要用时还要查证）
然后在以下的程序中输入多组标定数据，一组数据分别对应机械臂和相机的数据。推荐输入的标定数据组数在15组左右。以下程序参考了我师兄的程序link
师兄使用的是欧拉角，而我使用的四元数，所以可以对照两份代码的不同进行修改。当然代码的主体是一样的。同时，代码也不是单纯参考师兄的，还参考了另外一位博主的，但是找不到了，惭愧。
归根结底，代码是想把位姿转换称旋转矩阵然后送入opencv内置的一个函数calibrateHandEye()进行求解，用的算法就是上文所说的Tsai-lenz算法。仔细阅读代码可对上述所说有所明白。

//



#include <opencv2/opencv.hpp>
#include <iostream>
#include <math.h>
#include<eigen3\Eigen\Dense>


using namespace std;
using namespace cv;

Mat R_T2HomogeneousMatrix(const Mat& R, const Mat& T);
void HomogeneousMtr2RT(Mat& HomoMtr, Mat& R, Mat& T);
bool isRotatedMatrix(Mat& R);
Mat eulerAngleToRotateMatrix(const Mat& eulerAngle, const std::string& seq);
Mat quaternionToRotatedMatrix(const Vec4d& q);
Mat attitudeVectorToMatrix(const Mat& m, bool useQuaternion, const string& seq);
Mat_<double> q = (cv::Mat_<double>(1, 4) << -0.774647132989, -0.138703323173, 0.59915243478, 0.147307730246);
//数据使用的已有的值
//相机中13组标定板的位姿，x,y,z,w,q1,q2,q3
Mat_<double> CalPose = (cv::Mat_<double>(13, 7) <<
	0.0452462647436,    0.0327054799247,    0.172991838627, 0.0632869471649,0.532853235423, 0.837595946624, 0.102446190836,
	0.0388497651175,    0.0479508427745,    0.171064977207, 0.0828774100789,0.539034747402, 0.833179689483, 0.0915668118087,
	0.028328088994,     0.0252225698526,    0.164136104336, 0.0766029914989,0.53219226938,  0.833107370839, 0.129751604074,
	0.00447711600106,   0.0147803504366,    0.149693748863, 0.0996515424584,0.527920850231, 0.826882414545, 0.16623663403,
	-0.000871782710522,	0.0280600137579,    0.148260774669,	0.119569784017, 0.533038536357, 0.822904197206, 0.1562103317,
	-0.00454990691132,	0.0375296320808,	0.146823354797,	0.133753766388, 0.536198873306, 0.820126393385, 0.148301709751,
	0.0159389062622,    - 0.0109475052566,	0.149740140779,	0.0630777786609,0.516394185309,	0.832275091103,	0.191510866397,
	0.0219877442865,    - 0.023570535672,   0.148370142112, 0.0459609200971,0.510359257262, 0.833721322589, 0.205742019556,
	-0.00985416634311,	0.00845427020451,	0.13914292002,	0.111806619583,	0.525036766535,	0.822392299937,	0.188431893816,
	-0.0235599361194,	0.00237936186087,	0.127320172953,	0.123256282431,	0.521372909897,	0.817810911646,	0.210151113391,
	0.00252505272966,	0.0141049454382,	0.139188107322,	0.0984359661148,0.527910010541,	0.826799153439,	0.167405320161,
	-0.0227503625961,	0.00290110800445,	0.11921334782,	0.120014391923,	0.521919452416,	0.819086060912,	0.205656645262,
	-0.0158975128932,	- 0.0131103269009,	0.119844419659,	0.101578321977,	0.514675553878,	0.82376078019,	0.214963010226);

//机械臂末端13组位姿,x,y,z,rx,ry,rz
Mat_<double> ToolPose = (cv::Mat_<double>(13, 7) <<
	-0.119068317273,	0.0183054173689,	0.326418520745,	0.147307730246,	- 0.774647132989,	- 0.138703323173,	0.59915243478,
	-0.117923938994,	0.0314365363501,	0.326422378073,	0.161032347485,	- 0.777629186448,	- 0.120838792607,	0.595616837949,
	-0.127588063019,	0.0188505930921,	0.308755034618,	0.142106184689,	- 0.790919630198,	- 0.141580853503,	0.578106246949,
	-0.138323018987,	0.0195375193875,	0.282058685087,	0.134329642947,	- 0.813716329775,	- 0.145670932314,	0.546444197666,
	-0.137298104833,	0.0317269498021,	0.282066453389,	0.145254214409,	- 0.816451379011,	- 0.129409496569,	0.54365571928,
	-0.136209260479,	0.0408421388867,	0.282066453389,	0.153394092149,	- 0.818300964964,	- 0.117146074751,	0.541415349249,
	-0.13890306169, 	- 0.00522042120178,	0.282066453389,	0.112124675281,	- 0.807139447744,	- 0.178509338653,	0.551442095874,
	-0.13844987551, 	- 0.0174444690322,	0.282066453389,	0.101078345521,	- 0.803409553394,	- 0.194609253649,	0.553573388067,
	-0.143939777501,	0.0198969339143,	0.264962341081,	0.129353382177, -0.827311957668,    - 0.148104812179,   0.526201094477,
	-0.148694820656,	0.0202012075591,	0.247613037613,	0.124309474069,	- 0.840381768577,	- 0.150463749095,	0.505634549757,
	-0.150318771474,	0.0203051236368,	0.272278728281,	0.134340699038,	- 0.813716006995,	- 0.145689454114,	0.546437022448,
	-0.158882254384,	0.0208530980868,	0.240284392514,	0.125332497512,	- 0.837807345188,	- 0.150012383713,	0.509771421472,
	-0.159518090201,	0.00453523383602,	0.240292118299,	0.112544043611,	- 0.833788262373,	- 0.1709033633,	    0.512760189748
	
	);












int main(int argc, char** argv)
{
	//数据声明
	vector<Mat> R_gripper2base;
	vector<Mat> T_gripper2base;
	vector<Mat> R_target2cam;
	vector<Mat> T_target2cam;
	Mat R_cam2gripper = Mat(3, 3, CV_64FC1);				//相机与机械臂末端坐标系的旋转矩阵与平移矩阵
	Mat T_cam2gripper = Mat(3, 1, CV_64FC1);
	Mat Homo_cam2gripper = Mat(4, 4, CV_64FC1);

	//Eigen::Quaterniond quaternion(
	//	0.147307730246, -0.774647132989, -0.138703323173, 0.59915243478,
	//	0.161032347485, - 0.777629186448, -0.120838792607, 0.595616837949);
	//Eigen::Vector3d eulerAngle = quaternion.matrix().eulerAngles(0, 1, 2);
	//cout << eulerAngle;



	vector<Mat> Homo_target2cam;
	vector<Mat> Homo_gripper2base;
	Mat tempR, tempT, temp;
	for (int i = 0; i < CalPose.rows; i++)				//计算标定板与相机间的齐次矩阵（旋转矩阵与平移向量）
	{
		temp = attitudeVectorToMatrix(CalPose.row(i), true, "");	//注意seq为空，相机与标定板间的为旋转向量
		Homo_target2cam.push_back(temp);
		HomogeneousMtr2RT(temp, tempR, tempT);
		/*cout << i << "::" << temp << endl;
		cout << i << "::" << tempR << endl;
		cout << i << "::" << tempT << endl;*/
		R_target2cam.push_back(tempR);
		T_target2cam.push_back(tempT);
	}
	for (int j = 0; j < ToolPose.rows; j++)				//计算机械臂末端坐标系与机器人基坐标系之间的齐次矩阵
	{
		temp = attitudeVectorToMatrix(ToolPose.row(j), true, "xyz");  //注意seq不是空，机械臂末端坐标系与机器人基坐标系之间的为欧拉角
		Homo_gripper2base.push_back(temp);
		HomogeneousMtr2RT(temp, tempR, tempT);
		/*cout << j << "::" << temp << endl;
		cout << j << "::" << tempR << endl;
		cout << j << "::" << tempT << endl;*/
		R_gripper2base.push_back(tempR);
		T_gripper2base.push_back(tempT);
	}
	//TSAI计算速度最快
	calibrateHandEye(R_gripper2base, T_gripper2base, R_target2cam, T_target2cam, R_cam2gripper, T_cam2gripper, CALIB_HAND_EYE_TSAI);

	Homo_cam2gripper = R_T2HomogeneousMatrix(R_cam2gripper, T_cam2gripper);
	cout << Homo_cam2gripper << endl;
	cout << "Homo_cam2gripper 是否包含旋转矩阵：" << isRotatedMatrix(Homo_cam2gripper) << endl;

	///

		/**************************************************
		* @note   手眼系统精度测试，原理是标定板在机器人基坐标系中位姿固定不变，
		*		  可以根据这一点进行演算
		**************************************************/
		//使用1,2组数据验证  标定板在机器人基坐标系中位姿固定不变
	cout << "1 : " << Homo_gripper2base[0] * Homo_cam2gripper * Homo_target2cam[0] << endl;
	cout << "2 : " << Homo_gripper2base[1] * Homo_cam2gripper * Homo_target2cam[1] << endl;
	//标定板在相机中的位姿
	cout << "3 : " << Homo_target2cam[1] << endl;
	cout << "4 : " << Homo_cam2gripper.inv() * Homo_gripper2base[1].inv() * Homo_gripper2base[0] * Homo_cam2gripper * Homo_target2cam[0] << endl;

	cout << "----手眼系统测试-----" << endl;
	cout << "机械臂下标定板XYZ为：" << endl;
	for (int i = 0; i < Homo_target2cam.size(); i++)
	{
		Mat chessPos{ 0.0,0.0,0.0,1.0 };  //4*1矩阵，单独求机械臂坐标系下，标定板XYZ
		Mat worldPos = Homo_gripper2base[i] * Homo_cam2gripper * Homo_target2cam[i] * chessPos;
		cout << i << ": " << worldPos.t() << endl;
	}
	waitKey(0);

	return 0;
}

/**************************************************
* @brief   将旋转矩阵与平移向量合成为齐次矩阵
* @note
* @param   Mat& R   3*3旋转矩阵
* @param   Mat& T   3*1平移矩阵
* @return  Mat      4*4齐次矩阵
**************************************************/
Mat R_T2HomogeneousMatrix(const Mat& R, const Mat& T)
{
	Mat HomoMtr;
	Mat_<double> R1 = (Mat_<double>(4, 3) <<
		R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2),
		R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2),
		R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2),
		0, 0, 0);
	Mat_<double> T1 = (Mat_<double>(4, 1) <<
		T.at<double>(0, 0),
		T.at<double>(1, 0),
		T.at<double>(2, 0),
		1);
	cv::hconcat(R1, T1, HomoMtr);		//矩阵拼接
	return HomoMtr;
}

/**************************************************
* @brief    齐次矩阵分解为旋转矩阵与平移矩阵
* @note
* @param	const Mat& HomoMtr  4*4齐次矩阵
* @param	Mat& R              输出旋转矩阵
* @param	Mat& T				输出平移矩阵
* @return
**************************************************/
void HomogeneousMtr2RT(Mat& HomoMtr, Mat& R, Mat& T)
{
	//Mat R_HomoMtr = HomoMtr(Rect(0, 0, 3, 3)); //注意Rect取值
	//Mat T_HomoMtr = HomoMtr(Rect(3, 0, 1, 3));
	//R_HomoMtr.copyTo(R);
	//T_HomoMtr.copyTo(T);
	/*HomoMtr(Rect(0, 0, 3, 3)).copyTo(R);
	HomoMtr(Rect(3, 0, 1, 3)).copyTo(T);*/
	Rect R_rect(0, 0, 3, 3);
	Rect T_rect(3, 0, 1, 3);
	R = HomoMtr(R_rect);
	T = HomoMtr(T_rect);

}

/**************************************************
* @brief	检查是否是旋转矩阵
* @note
* @param
* @param
* @param
* @return  true : 是旋转矩阵， false : 不是旋转矩阵
**************************************************/
bool isRotatedMatrix(Mat& R)		//旋转矩阵的转置矩阵是它的逆矩阵，逆矩阵 * 矩阵 = 单位矩阵
{
	Mat temp33 = R({ 0,0,3,3 });	//无论输入是几阶矩阵，均提取它的三阶矩阵
	Mat Rt;
	transpose(temp33, Rt);  //转置矩阵
	Mat shouldBeIdentity = Rt * temp33;//是旋转矩阵则乘积为单位矩阵
	Mat I = Mat::eye(3, 3, shouldBeIdentity.type());

	return cv::norm(I, shouldBeIdentity) < 1e-6;
}

/**************************************************
* @brief   欧拉角转换为旋转矩阵
* @note
* @param    const std::string& seq  指定欧拉角的排列顺序；（机械臂的位姿类型有xyz,zyx,zyz几种，需要区分）
* @param    const Mat& eulerAngle   欧拉角（1*3矩阵）, 角度值
* @param
* @return   返回3*3旋转矩阵
**************************************************/
Mat eulerAngleToRotateMatrix(const Mat& eulerAngle, const std::string& seq)
{
	CV_Assert(eulerAngle.rows == 1 && eulerAngle.cols == 3);//检查参数是否正确

	eulerAngle /= (180 / CV_PI);		//度转弧度

	Matx13d m(eulerAngle);				//<double, 1, 3>

	auto rx = m(0, 0), ry = m(0, 1), rz = m(0, 2);
	auto rxs = sin(rx), rxc = cos(rx);
	auto rys = sin(ry), ryc = cos(ry);
	auto rzs = sin(rz), rzc = cos(rz);

	//XYZ方向的旋转矩阵
	Mat RotX = (Mat_<double>(3, 3) << 1, 0, 0,
		0, rxc, -rxs,
		0, rxs, rxc);
	Mat RotY = (Mat_<double>(3, 3) << ryc, 0, rys,
		0, 1, 0,
		-rys, 0, ryc);
	Mat RotZ = (Mat_<double>(3, 3) << rzc, -rzs, 0,
		rzs, rzc, 0,
		0, 0, 1);
	//按顺序合成后的旋转矩阵
	cv::Mat rotMat;

	if (seq == "zyx") rotMat = RotX * RotY * RotZ;
	else if (seq == "yzx") rotMat = RotX * RotZ * RotY;
	else if (seq == "zxy") rotMat = RotY * RotX * RotZ;
	else if (seq == "yxz") rotMat = RotZ * RotX * RotY;
	else if (seq == "xyz") rotMat = RotZ * RotY * RotX;
	else if (seq == "xzy") rotMat = RotY * RotZ * RotX;
	else
	{
		cout << "Euler Angle Sequence string is wrong...";
	}
	if (!isRotatedMatrix(rotMat))		//欧拉角特殊情况下会出现死锁
	{
		cout << "Euler Angle convert to RotatedMatrix failed..." << endl;
		exit(-1);
	}
	return rotMat;
}

/**************************************************
* @brief   将四元数转换为旋转矩阵
* @note
* @param   const Vec4d& q   归一化的四元数: q = q0 + q1 * i + q2 * j + q3 * k;
* @return  返回3*3旋转矩阵R
**************************************************/
Mat quaternionToRotatedMatrix(const Vec4d& q)
{
	double q0 = q[0], q1 = q[1], q2 = q[2], q3 = q[3];

	double q0q0 = q0 * q0, q1q1 = q1 * q1, q2q2 = q2 * q2, q3q3 = q3 * q3;
	double q0q1 = q0 * q1, q0q2 = q0 * q2, q0q3 = q0 * q3;
	double q1q2 = q1 * q2, q1q3 = q1 * q3;
	double q2q3 = q2 * q3;
	//根据公式得来
	Mat RotMtr = (Mat_<double>(3, 3) << (q0q0 + q1q1 - q2q2 - q3q3), 2 * (q1q2 + q0q3), 2 * (q1q3 - q0q2),
		2 * (q1q2 - q0q3), (q0q0 - q1q1 + q2q2 - q3q3), 2 * (q2q3 + q0q1),
		2 * (q1q3 + q0q2), 2 * (q2q3 - q0q1), (q0q0 - q1q1 - q2q2 + q3q3));
	//这种形式等价
	/*Mat RotMtr = (Mat_<double>(3, 3) << (1 - 2 * (q2q2 + q3q3)), 2 * (q1q2 - q0q3), 2 * (q1q3 + q0q2),
										 2 * (q1q2 + q0q3), 1 - 2 * (q1q1 + q3q3), 2 * (q2q3 - q0q1),
										 2 * (q1q3 - q0q2), 2 * (q2q3 + q0q1), (1 - 2 * (q1q1 + q2q2)));*/

	return RotMtr;
}

/**************************************************
* @brief      将采集的原始数据转换为齐次矩阵（从机器人控制器中获得的）
* @note
* @param	  Mat& m    1*6//1*10矩阵 ， 元素为： x,y,z,rx,ry,rz  or x,y,z, q0,q1,q2,q3,rx,ry,rz
* @param	  bool useQuaternion      原始数据是否使用四元数表示
* @param	  string& seq         原始数据使用欧拉角表示时，坐标系的旋转顺序
* @return	  返回转换完的齐次矩阵
**************************************************/
Mat attitudeVectorToMatrix(const Mat& m, bool useQuaternion, const string& seq)
{
	CV_Assert(m.total() == 7 || m.total() == 10);
	//if (m.cols == 1)	//转置矩阵为行矩阵
	//	m = m.t();	

	Mat temp = Mat::eye(4, 4, CV_64FC1);

	if (useQuaternion)
	{
		Vec4d quaternionVec = m({ 3,0,4,1 });   //读取存储的四元数
		quaternionToRotatedMatrix(quaternionVec).copyTo(temp({ 0,0,3,3 }));
	}
	else
	{
		Mat rotVec;
		if (m.total() == 6)
		{
			rotVec = m({ 3,0,3,1 });   //读取存储的欧拉角
		}
		if (m.total() == 10)
		{
			rotVec = m({ 7,0,3,1 });
		}
		//如果seq为空，表示传入的是3*1旋转向量，否则，传入的是欧拉角
		if (0 == seq.compare(""))
		{
			Rodrigues(rotVec, temp({ 0,0,3,3 }));   //罗德利斯转换
		}
		else
		{
			eulerAngleToRotateMatrix(rotVec, seq).copyTo(temp({ 0,0,3,3 }));
		}
	}
	//存入平移矩阵
	temp({ 3,0,1,3 }) = m({ 0,0,3,1 }).t();
	return temp;   //返回转换结束的齐次矩阵
}



//#define _USE_MATH_DEFINES
//#include <cmath>

struct Quaternion {
	double w, x, y, z;
};

struct EulerAngles {
	double roll, pitch, yaw;
};

EulerAngles ToEulerAngles(Quaternion q) {
	EulerAngles angles;

	// roll (x-axis rotation)
	double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
	double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
	angles.roll = std::atan2(sinr_cosp, cosr_cosp);

	// pitch (y-axis rotation)
	double sinp = 2 * (q.w * q.y - q.z * q.x);
	if (std::abs(sinp) >= 1)
		angles.pitch = std::copysign(CV_PI / 2, sinp); // use 90 degrees if out of range
	else
		angles.pitch = std::asin(sinp);

	// yaw (z-axis rotation)
	double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
	double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
	angles.yaw = std::atan2(siny_cosp, cosy_cosp);

	return angles;
}