手眼标定之已知同名点对，求解转换RT，备份记录

机器人手眼标定，往往需要获取相机和机器人末端工具和相机坐标系的转换关系，此处只介绍一种眼在手外，3D相机的解算方法

前提条件

获取到机器人工具坐标系下和3D相机坐标系下的同名点

方法：

1.可以自动获取，通过放固定模型的靶标，识别靶标中心点一类的，如小圆柱体顶面圆心

2.手动拾取，关联2D和3D相机像素和深度，直接在图像上拾取平面位置和深度

求解转换关系

公式可以完成参考这篇文章，人家写的很好，而且另一篇有严密推导，似懂非懂，我直接借用公式了

转载：https://blog.csdn.net/u012836279/article/details/80203170

这篇文章是拿python写的，我不会用，我用C++复现和测试一下
基本原理和公式贴一下图

去中心化，如果两组点是同名点，去中心化后应该只有旋转变换了，先求旋转在算偏移

1.计算重心，即XYZ求均值即可

2.去中心化，即每个点减去重心

3.构建协方差矩阵

4.SVD分解，SVD分解后R = VUT

5. 求平移量，用俩重心算
   

贴上C++代码验证一下

我找了三组数据，都是先找的点，在3D软件中，任意做了旋转平移刚性变换，数据抄出来使用的，算是能验证精度了，当前可以适当更改一下加点误差，看看算出来误差和坐标还准不准

#include <Eigen/Dense>
#include <Eigen/SVD>
#include <iostream>
#include <vector>

Eigen::Vector3d CalculateCenxyz(Eigen::MatrixXd xyzvec)
{
	double xsum = 0;
	double ysum = 0;
	double zsum = 0;
	int num = xyzvec.rows();
	for (int i = 0; i < num; i++) 
	{
		xsum += xyzvec(i,0);
		ysum += xyzvec(i,1);
		zsum += xyzvec(i,2);
	}
	return Eigen::Vector3d(xsum / num, ysum / num, zsum / num);
}
Eigen::MatrixXd CalculateSubCenxyz(Eigen::MatrixXd xyzvec, Eigen::Vector3d cenxyz) {
	int num = xyzvec.rows();
	Eigen::MatrixXd out;
	out.resize(num, 3);
	for (int i = 0; i < num; i++) {
		out(i, 0) = xyzvec(i, 0) - cenxyz[0];
		out(i, 1) = xyzvec(i, 1) - cenxyz[1];
		out(i, 2) = xyzvec(i, 2) - cenxyz[2];
	}
	return out;
}
float CalculateRT(std::vector<Eigen::Vector3d>src, std::vector<Eigen::Vector3d>dst, Eigen::Matrix4d &RT) {
	//参数计算的格式
	Eigen::MatrixXd A, B;
	A.resize(src.size(), 3);
	B.resize(dst.size(), 3);
	for (int i = 0; i < src.size(); i++) {
		A(i, 0) = src[i][0]; A(i, 1) = src[i][1]; A(i, 2) = src[i][2];
	}
	for (int i = 0; i < dst.size(); i++) {
		B(i, 0) = dst[i][0]; B(i, 1) = dst[i][1]; B(i, 2) = dst[i][2];
	}
	if (src.size() != dst.size())
	{
		return -1;
	}
	const int N = src.size(); // 点数
	const int d = 3; // 维度
					 //计算质心点
	Eigen::Vector3d srccenxyz, dstcenxyz;
	srccenxyz = CalculateCenxyz(A);
	dstcenxyz = CalculateCenxyz(B);
	//去中心化
	Eigen::MatrixXd srccentered = CalculateSubCenxyz(A, srccenxyz);
	Eigen::MatrixXd dstcentered = CalculateSubCenxyz(B, dstcenxyz);
	//构建协方差矩阵
	Eigen::MatrixXd H = srccentered.transpose() * dstcentered;
	//对H进行SVD分解
	Eigen::JacobiSVD<Eigen::MatrixXd> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
	//计算旋转矩阵R (使用SVD结果)
	Eigen::MatrixXd R = svd.matrixV() * svd.matrixU().transpose();
	// 确保旋转矩阵的行列式为+1 (避免反射)
	if (R.determinant() < 0) {
		Eigen::MatrixXd V = svd.matrixV();
		V.col(3 - 1) *= -1; // 将最后一列取反
		R = V * svd.matrixU().transpose();
	}
	//计算平移矩阵
	Eigen::Vector3d t = dstcenxyz - R * srccenxyz;
	// 8. 计算变换后的点集
	Eigen::MatrixXd A_transformed = (A * R.transpose()).rowwise() + t.transpose();
	std::cout << "变换后的点集A:\n" << A_transformed << "\n\n";
	std::cout << "原始点集B:\n" << B << "\n\n";
	// 9. 计算变换误差
	Eigen::MatrixXd error = B - A_transformed;
	double rmse = sqrt(error.array().square().sum() / (N * d));

	RT(0, 0) = R(0, 0); RT(0, 1) = R(0, 1); RT(0, 2) = R(0, 2); RT(0, 3) = t[0];
	RT(1, 0) = R(1, 0); RT(1, 1) = R(1, 1); RT(1, 2) = R(1, 2); RT(1, 3) = t[1];
	RT(2, 0) = R(2, 0); RT(2, 1) = R(2, 1); RT(2, 2) = R(2, 2); RT(2, 3) = t[2];
	RT(3, 0) = 0; RT(3, 1) = 0; RT(3, 2) = 0; RT(3, 3) = 1;

	return rmse;
}
int main() {
	//外部做接口做成这个Eigen::Vector3d格式
	std::vector<Eigen::Vector3d>src,dst;
	src.push_back(Eigen::Vector3d(-307.233, -265.14, 930));
	src.push_back(Eigen::Vector3d(33.5072, -196.628, 974));
	src.push_back(Eigen::Vector3d(-153.525, -38.2655, 1007));
	src.push_back(Eigen::Vector3d(288.805, -167.142, 999));
	src.push_back(Eigen::Vector3d(142.261, -437.487, 912));
	src.push_back(Eigen::Vector3d(-104.493, -435.548, 896));

	dst.push_back(Eigen::Vector3d(-937.93444824 , -807.26367188, 291.15930176));
	dst.push_back(Eigen::Vector3d(-1075.77783203, -888.99725342, 602.69195557));
	dst.push_back(Eigen::Vector3d(-859.97985840 , -968.87390137, 512.13989258));
	dst.push_back(Eigen::Vector3d(-1195.63476563, -937.24676514, 826.24877930));
	dst.push_back(Eigen::Vector3d(-1307.30078125, -747.51934814, 594.59216309));
	dst.push_back(Eigen::Vector3d(-1170.11645508, -720.66943359, 390.61462402));

	/*src.push_back(Eigen::Vector3d(100, 0, 0));
	src.push_back(Eigen::Vector3d(100, 100, 100));
	src.push_back(Eigen::Vector3d(0, 100, 0));
	src.push_back(Eigen::Vector3d(0, 0, 100));

	dst.push_back(Eigen::Vector3d(-108.54067993, -37.01885223, -190.20104980));
	dst.push_back(Eigen::Vector3d(-2.08551025, -94.98533630, -117.35063171));
	dst.push_back(Eigen::Vector3d(-135.92652893, -121.73773193, -80.32509613));
	dst.push_back(Eigen::Vector3d(-83.49967957, -175.86019897, -200.00042725));*/

	//src.push_back(Eigen::Vector3d(100, 0, 1));
	//src.push_back(Eigen::Vector3d(500, 0, 5));
	//src.push_back(Eigen::Vector3d(0, 100, 3));
	//src.push_back(Eigen::Vector3d(0, 500, 4));

	//dst.push_back(Eigen::Vector3d(6040.03369141, 2504.92944336, -64.71488953));
	//dst.push_back(Eigen::Vector3d(6108.16162109, 2302.78710938, 273.68286133));
	//dst.push_back(Eigen::Vector3d(5981.70751953, 2629.77734375, -96.57476807));
	//dst.push_back(Eigen::Vector3d(5826.48486328, 2931.66308594, 115.01904297));

	Eigen::Matrix4d RT;
	float rmse = CalculateRT(src, dst, RT);
	std::cout << RT(0, 0) << " " << RT(0, 1) << " " << RT(0, 2) << " " << RT(0, 3) << std::endl;
	std::cout << RT(1, 0) << " " << RT(1, 1) << " " << RT(1, 2) << " " << RT(1, 3) << std::endl;
	std::cout << RT(2, 0) << " " << RT(2, 1) << " " << RT(2, 2) << " " << RT(2, 3) << std::endl;
	std::cout << RT(3, 0) << " " << RT(3, 1) << " " << RT(3, 2) << " " << RT(3, 3) << std::endl;

	std::cout << "rmse:" << rmse<<std::endl;

	for (int i = 0; i < src.size(); i++) {
		Eigen::Vector4d srcp(src[i][0], src[i][1], src[i][2], 1.0);
		Eigen::Vector4d dstp = RT * srcp;
		std::cout << "转换前: " << srcp[0] << " " << srcp[1] << " " << srcp[2] << std::endl;
		std::cout << "转换后: " << dstp[0] << " " << dstp[1] << " " << dstp[2] << std::endl;
	}
	return 0;
}

算出来的RT 将A应用旋转和平移后，坐标和B一致，rmse基本为0，说明是刚性变换

留下赞赞和收藏 哈哈...