手眼标定（眼在手上）代码及注意事项

前言

本人最近在做单目相机的手眼标定，相机的安装方式为固定在机械臂末端，为此最近翻阅了很多的资料和代码，很多资料已经将手眼标定的原理解释的非常清楚，因此本文就不再过多的阐述手眼标定的原理，如果希望熟悉原理的话可以参考这篇文章：手眼标定之基本原理。

本文重点放在了手眼标定的代码以及使用代码的注意事项，虽然网上有很多相关代码，但是个人感觉解释不够清楚或者没有提示需要注意的地方，因此本人希望本篇文章帮助大家少走一些弯路，这也是本人第一次写文章，如果有什么不合适的地方，希望大家多多担待，当然大家有什么问题也可以给我留言，我会尽力解决各位的疑惑。

运行环境

程序的运行环境为Visual Studio+Opencv4.8.0，opencv版本在4以上较好；

程序源码

本人的代码主要参考自：手眼标定（一）：Opencv4实现手眼标定及手眼系统测试

程序大体上与参考文章一样，但是有几处需要说明并进行修改。

注意事项：

（一）关于标定板相对于相机的位姿关系，由平移向量和旋转向量组成，关于平移向量和旋转向量获取的方式为Matlab的相机标定工具箱，关于该标定的工具箱的使用，可以参考：使用Matlab做相机标定（获取相机的内外参数矩阵），在标定完成后注意检查标定板的起点，一定要保证所有标定板的起点一致。

（二）关于机械臂末端相对于基座的位姿关系，本人所使用的是史陶比尔机械臂，可以直接从示教器上获取末端的位姿关系，前三个数据为末端坐标系相对于基坐标系的平移，后三个数据为末端坐标系相对于基坐标系的旋转，其旋转是以欧拉角的方式进行描述。在这里有一个容易忽略的细节，就是欧拉角是内旋还是外旋，关于内旋和外旋的区别可以参考：欧拉角细节/旋转顺序/内旋外旋，简单来说内旋每次是绕着自身轴进行旋转，而外旋每次是绕着固定轴进行旋转。内旋是矩阵右乘，外旋是矩阵左乘，举个例子就是内旋X-Y-Z等价于外旋Z-Y-X。在欧拉角转旋转矩阵的函数中本人做了相应的修改，需要额外传入一个内旋还是外旋的参数。

此外，在欧拉角转旋转矩阵还需要注意的是单位问题，C++中的三角函数是以弧度制进行计算的，本人传入的是角度，需要进行转换，如果本身传入的就是弧度就不需要进行转换。

整理代码如下：

#include <iostream>
#include <string>
#include <opencv2/opencv.hpp>
#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/calib3d.hpp>

//导入静态链接库
#if (defined _WIN32 || defined WINCE || defined __CYGWIN__)
#define OPENCV_VERSION "480"
#pragma comment(lib, "opencv_world" OPENCV_VERSION ".lib")
#endif

using namespace cv;
using namespace std;

//第二组
//标定板相对于相机的位姿，平移向量+旋转向量
Mat_<double> CalPose = (Mat_<double>(13, 6) << 
	-17.00157419, -81.58147217, 355.7542094, 0.029088994, -0.018921115, -0.05339676,
	-28.96389924, -51.10505073, 356.782175, -0.003821439, 0.036377063, -0.312840676,
	-1.336799619, -14.59851038, 343.103271, -0.064835906, -0.027856025, 0.11723227,
	0.599159361, -29.21464607, 317.5905327, 0.153990913, -0.164997269, 0.141422701,
	-30.57797266, -15.33702401, 307.928564, -0.051195625, -0.09995785, -0.300064261,
	-13.17969414, -32.4384572, 379.3117752, -0.08341498, 0.099063095, -0.154433415,
	-9.912571488, -51.70341331, 377.3018235, -0.119340607, -0.091310513, 0.334251393,
	43.93753639, -98.11986502, 351.5100989, 0.305854801, -0.036788466, 0.47603346,
	7.27764392, -115.6325578, 289.6246863, 0.191094099, -0.314029023, 0.219271655,
	-41.66080761, -165.070534, 372.1309944, 0.111627518, 0.321585867, 0.38798498,
	-96.27253589, -50.22770075, 457.0584011, -0.492185963, -0.002850737, -0.387823227,
	5.183372925, -89.01940034, 303.5791468, 0.374755954, -0.246685178, 0.351494064,
	-63.27585387, -12.24742338, 395.4984493, -0.374408602, -0.356181557, -0.12614873
	);

//机械臂末端相对于机器人基座的位姿变化，X,Y,Z,Rx,Ry,Rz
Mat_<double> ToolPose = (cv::Mat_<double>(13, 6) <<
	-99.76, 711.11, -311.65, -1.09, 178.64, 4.02,
	-89.47, 740.35, -311.65, 0.85, -178.23, 18.88,
	-110.38, 755.38, -328.44, 4.36, 178.29, -5.81,
	-130.99, 771.58, -355.7, -7.6, 169.59, -6.48,
	-98.51, 771.65, -363.99, 2.27, 173.69, 18.15,
	-82.1, 739.79, -291.04, 5.51, -175.02, 10.07,
	-93.76, 698.95, -304.06, 8.47, 175.66, -18.49,
	-116.96, 713.27, -291.63, -15.43, 173.6, -25.81,
	-129.88, 680.86, -376.39, -8.29, 160.56, -10.32,
	54.49, 671.77, -300.37, -10.33, -164.05, -23.05,
	-61.9, 596.03, -276.03, 27.64, 174.52, 22.48,
	-123.6, 731.8, -349.62, -18.21, 162.17, -16.75,
	-129.45, 696.37, -302.57, 21.49, 158.61, 4.54
	);



//R和T合并为4×4齐次矩阵
Mat R_T2RT(Mat& R, Mat& T)
{
	Mat RT;
	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>(0, 1), T.at<double>(0, 2), 1);
	hconcat(R1, T1, RT);
	return RT;
}

//齐次矩阵分为R矩阵和T矩阵
void RT2R_T(Mat& RT, Mat& R, Mat& T)
{
	R = (Mat_<double>(3, 3) <<
		RT.at<double>(0, 0), RT.at<double>(0, 1), RT.at<double>(0, 2),
		RT.at<double>(1, 0), RT.at<double>(1, 1), RT.at<double>(1, 2),
		RT.at<double>(2, 0), RT.at<double>(2, 1), RT.at<double>(2, 2));
	T = (Mat_ <double>(3, 1) << RT.at<double>(0, 3), RT.at<double>(1, 3), RT.at<double>(2, 3));
}

//判断是否为旋转矩阵
bool isRotationMatrix(const cv::Mat& R)
{
	Mat tmp33 = R({ 0,0,3,3 });
	Mat shouldBeIdentity;
	shouldBeIdentity = tmp33.t() * tmp33;
	Mat I = Mat::eye(3, 3, shouldBeIdentity.type());
	return  norm(I, shouldBeIdentity) < 1e-6;
}

/** @brief 欧拉角 -> 3*3 的R
*	@param 	eulerAngle		角度值
*	@param 	seq				指定欧拉角xyz的排列顺序如："xyz" "zyx"
*/
//注意自己带入的欧拉角是内旋还是外旋,在传入旋转顺序之前一定要确定旋转方式
//内旋：绕着自身轴进行旋转，如果旋转顺序为X-Y-Z，则旋转矩阵为Rx*Ry*Rz
//外旋：绕着固定轴进行旋转，如果旋转顺序为X-Y-Z，则旋转矩阵为Rz*Ry*Rz
Mat eulerAngleToRotatedMatrix(const Mat& eulerAngle, const string &rotationseq,const string& seq)
{
	CV_Assert(eulerAngle.rows == 1 & eulerAngle.cols == 3);
	eulerAngle /= (180 / CV_PI);//将角度转为弧度，C++的三角函数是以弧度进行计算的
	Matx13d m(eulerAngle);
	auto rx = m(0, 0), ry = m(0, 1), rz = m(0, 2);
	auto xs = sin(rx), xc = cos(rx);
	auto ys = sin(ry), yc = cos(ry);
	auto zs = sin(rz), zc = cos(rz);
	Mat rotX = (cv::Mat_<double>(3, 3) <<
		1, 0, 0,
		0, xc, -1*xs,
		0, xs, xc);
	Mat rotY = (cv::Mat_<double>(3, 3) <<
		yc, 0, ys,
		0, 1, 0,
		-1*ys, 0, yc);
	Mat rotZ = (cv::Mat_<double>(3, 3) <<
		zc, -1*zs, 0,
		zs, zc, 0,
		0, 0, 1);
	Mat rotMat;
	if (rotationseq == "wai")
	{
		//外旋左乘
		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 == "xzy")	rotMat = rotY * rotZ * rotX;
		else if (seq == "yxz")	rotMat = rotZ * rotX * rotY;
		else if (seq == "xyz")	rotMat = rotZ * rotY * rotX;
		else {
			error(Error::StsAssert, "Euler angle sequence string is wrong.",
				__FUNCTION__, __FILE__, __LINE__);
		}
		if (!isRotationMatrix(rotMat)) {
			error(Error::StsAssert, "Euler angle can not convert to rotated matrix",
				__FUNCTION__, __FILE__, __LINE__);
		}
	}
	else if (rotationseq == "nei")
	{
		//内旋右乘
		if (seq == "xyz")		rotMat = rotX * rotY * rotZ;
		else if (seq == "xzy")	rotMat = rotX * rotZ * rotY;
		else if (seq == "yxz")	rotMat = rotY * rotX * rotZ;
		else if (seq == "yzx")	rotMat = rotY * rotZ * rotX;
		else if (seq == "zxy")	rotMat = rotZ * rotX * rotY;
		else if (seq == "zyx")	rotMat = rotZ * rotY * rotX;
		else {
			error(Error::StsAssert, "Euler angle sequence string is wrong.",
				__FUNCTION__, __FILE__, __LINE__);
		}
		if (!isRotationMatrix(rotMat)) {
			error(Error::StsAssert, "Euler angle can not convert to rotated matrix",
				__FUNCTION__, __FILE__, __LINE__);
		}
	}
	return rotMat;
}

/** @brief 四元数转旋转矩阵
*	@note  数据类型double； 四元数定义 q = w + x*i + y*j + z*k
*	@param q 四元数输入{w,x,y,z}向量
*	@return 返回旋转矩阵3*3
*/
Mat quaternionToRotatedMatrix(const Vec4d& q)
{
	double w = q[0], x = q[1], y = q[2], z = q[3];
	double x2 = x * x, y2 = y * y, z2 = z * z;
	double xy = x * y, xz = x * z, yz = y * z;
	double wx = w * x, wy = w * y, wz = w * z;
	Matx33d res{
		1 - 2 * (y2 + z2),	2 * (xy - wz),		2 * (xz + wy),
		2 * (xy + wz),		1 - 2 * (x2 + z2),	2 * (yz - wx),
		2 * (xz - wy),		2 * (yz + wx),		1 - 2 * (x2 + y2),
	};
	return Mat(res);
}

/** @brief (平移向量 && (四元数||欧拉角||旋转向量)) -> 4*4 的Rt
*	@param 	m				1*6 || 1*10的矩阵  -> 6  {x,y,z, rx,ry,rz}   10 {x,y,z, qw,qx,qy,qz, rx,ry,rz}
*	@param 	useQuaternion	如果是1*10的矩阵，判断是否使用四元数计算旋转矩阵
*	@param 	seq				如果通过欧拉角计算旋转矩阵，需要指定欧拉角xyz的排列顺序如："xyz" "zyx" 为空表示旋转向量
*/
Mat attitudeVectorToMatrix(Mat& m, bool useQuaternion, const string& rotationseq, const string& seq)
{
	CV_Assert(m.total() == 6 || m.total() == 10);
	if (m.cols == 1)
		m = m.t();
	Mat tmp = Mat::eye(4, 4, CV_64FC1);
	//如果使用四元数转换为旋转矩阵从m矩阵的第四个成员开始读取，读取四个数据
	if (useQuaternion)
	{
		Vec4d quaternionVec = m({ 3,0,4,1 });
		quaternionToRotatedMatrix(quaternionVec).copyTo(tmp({ 0,0,3,3 }));
	}
	else
	{
		Mat rotVec;
		if (m.total() == 6)
			rotVec = m({ 3,0,3,1 });	//6
		else
			rotVec = m({ 7,0,3,1 });	//10
		if (0 == seq.compare(""))
		{
			Rodrigues(rotVec, tmp({ 0,0,3,3 }));
		}
		else
			eulerAngleToRotatedMatrix(rotVec, rotationseq, seq).copyTo(tmp({ 0,0,3,3 }));
	}
	tmp({ 3, 0, 1, 3 }) = m({ 0, 0, 3, 1 }).t();
	return tmp;
}

int main()
{
	vector<Mat>R_gripper2base;
	vector<Mat>T_gripper2base;
	vector<Mat>R_target2cam;
	vector<Mat>T_target2cam;
	Mat R_cam2gripper = (Mat_<double>(3, 3));
	Mat T_cam2gripper = (Mat_<double>(3, 1));

	vector<Mat> images;
	size_t num_images = 13;//标定数据数量
	vector<Mat>vecHg;//保存末端到基座的位姿矩阵
	vector<Mat>vecHc;//保存标定板到相机的位姿矩阵
	Mat Hcg;//保存相机到末端的位姿矩阵
	Mat tempR, tempT;
	//标定板位姿转换
	for (size_t i = 0; i < num_images; i++)
	{
		Mat tmp = attitudeVectorToMatrix(CalPose.row(i), false, "","");
		vecHc.push_back(tmp);
		RT2R_T(tmp, tempR, tempT);
		R_target2cam.push_back(tempR);
		T_target2cam.push_back(tempT);
	}
	//机械臂位姿转换
	for (size_t i = 0; i < num_images; i++)
	{
		Mat tmp = attitudeVectorToMatrix(ToolPose.row(i), false, "nei", "xyz");
		vecHg.push_back(tmp);
		RT2R_T(tmp, tempR, tempT);
		R_gripper2base.push_back(tempR);
		T_gripper2base.push_back(tempT);
	}
	//手眼标定，CALIB_HAND_EYE_TSAI法为11ms，最快
	calibrateHandEye(R_gripper2base, T_gripper2base, R_target2cam, T_target2cam, R_cam2gripper, T_cam2gripper, CALIB_HAND_EYE_TSAI);
	Hcg = R_T2RT(R_cam2gripper, T_cam2gripper);//矩阵合并
	cout << "Hcg 矩阵为： " << endl;
	cout << Hcg << endl;
	cout << "是否为旋转矩阵：" << isRotationMatrix(Hcg) << endl << endl;//判断是否为旋转矩阵
	//Tool_In_Base*Hcg*Cal_In_Cam，用第一组和第二组进行对比验证
	cout << "第一组和第二组手眼数据验证：" << endl;
	cout << vecHg[0] * Hcg * vecHc[0] << endl << vecHg[1] * Hcg * vecHc[1] << endl << endl;//.inv()

	cout << "标定板在相机中的位姿：" << endl;
	cout << vecHc[1] << endl;
	cout << "手眼系统反演的位姿为：" << endl;
	//用手眼系统预测第一组数据中标定板相对相机的位姿，是否与vecHc[1]相同
	cout << Hcg.inv() * vecHg[1].inv() * vecHg[0] * Hcg * vecHc[0] << endl << endl;

	cout << "----手眼系统测试----" << endl;
	cout << "机械臂下标定板XYZ为：" << endl;
	for (int i = 0; i < vecHc.size(); ++i)
	{
		Mat cheesePos{ 0.0,0.0,0.0,1.0 };//4*1矩阵，单独求机械臂下，标定板的xyz
		Mat worldPos = vecHg[i] * Hcg * vecHc[i] * cheesePos;
		cout << i << ": " << worldPos.t() << endl;
	}
	getchar();
	return 0;
}

运行结果

程序中的数据是本人采集了13张标定板的图片并记录了拍摄每张图片时记录的位姿，标定板为12×9，大小为20mm的棋盘格。关于棋盘格的获取，大家可以去棋盘格生成进行生成然后打印在A4纸上，打印时注意选择按实际大小打印。

程序运行结果如下：

Hcg 矩阵为：
[-0.9990454704212959, -0.04238522090013363, 0.01056603425787106, -44.96410892580678;
 0.04231801734668055, -0.9990830155543935, -0.006504878060788962, 5.968794328297632;
 0.01083205606233982, -0.006051535341263867, 0.9999230197777607, -171.296159884876;
 0, 0, 0, 1]
是否为旋转矩阵：1

第一组和第二组手眼数据验证：
[0.9996535308298152, 0.02559364590422199, -0.006146835674782315, -68.75214158039236;
 0.02562052249176114, -0.9996623447059848, 0.004334213213756957, 790.3723085544281;
 -0.006033831844908959, -0.004490196684160321, -0.9999717151034848, -497.2474810245966;
 0, 0, 0, 1]
[0.9996323685494792, 0.02603527427932847, -0.007569163848032558, -68.35639943681868;
 0.02606313919440882, -0.9996537936695049, 0.003606324652617171, 790.44443930086;
 -0.007472651704120613, -0.00380227502521064, -0.9999648504728262, -497.1223300221939;
 0, 0, 0, 1]

标定板在相机中的位姿：
[0.9508123825271663, 0.3076246664810327, 0.03637138718068197, -28.96389924;
 -0.3077625337469542, 0.9514613344103248, -0.001884660122944243, -51.10505073;
 -0.03518573652303708, -0.00940179209286761, 0.9993365650524221, 356.782175;
 0, 0, 0, 1]
手眼系统反演的位姿为：
[0.9508935102902822, 0.3072346284095964, 0.03752885814732836, -29.31712610571623;
 -0.3073431642061819, 0.9515940669701087, -0.00298515041836284, -50.92399568965126;
 -0.03662938033269298, -0.008695677851983271, 0.9992910855617282, 356.9233391451787;
 0, 0, 0, 1]

----手眼系统测试----
机械臂下标定板XYZ为：
0: [-68.75214158039236, 790.3723085544281, -497.2474810245966, 1]
1: [-68.35639943681868, 790.44443930086, -497.1223300221939, 1]
2: [-67.93511404415696, 790.4488236881581, -496.9255297819951, 1]
3: [-68.16612119340775, 790.6114021116049, -496.8191849206758, 1]
4: [-68.02219614847647, 790.7649086921571, -497.1676079976034, 1]
5: [-68.09228690059392, 790.0085079250829, -497.2195653114088, 1]
6: [-68.15764972599764, 789.538410116062, -497.6606948748483, 1]
7: [-69.11488569076617, 790.0387198493054, -496.4647603265529, 1]
8: [-69.43043243828146, 790.3231655243442, -498.3967464742822, 1]
9: [-70.31049357421708, 788.6010496221037, -498.2754660641958, 1]
10: [-69.50329717739049, 789.085088951899, -498.9327080177022, 1]
11: [-69.13181339344797, 790.2106436923892, -497.3082000034407, 1]
12: [-68.26098173569075, 789.2537519940606, -498.2797994765572, 1]

结果分析

X,Y,Z值的误差均在2.5mm以内，满足实际使用需求。