由空间三对点求解两个坐标系之间的变换矩阵

三维刚体变换模型（即旋转/平移矩阵，RT矩阵）的估计方法

原理简单阐述：
只要算出变换矩阵，就可以算出A坐标系的一个点P在坐标系B里的对应点坐标，即

T为3x3的转换矩阵， t 为3x1的位移变换向量，这里点坐标均为3x1的列向量（非齐次形式，齐次形式下为4x1列向量，多出的一个元素值补1而已）。理论上只要给定至少3对点，就能计算出 T 和 t 。自然的，点对越多，计算出来的转换就越精确。详细的原理请参考《Estimating 3-D Rigid Body Transformations: A Comparison of Four Major Algorithms》，它使用SVD方法计算 T 和 t 。

 

下面就话不多说，上代码。亲写亲测有效

cv::Mat Get3DR_TransMatrix(const std::vector<cv::Point3f>& srcPoints, const std::vector<cv::Point3f>&  dstPoints)
{
	double srcSumX = 0.0f;
	double srcSumY = 0.0f;
	double srcSumZ = 0.0f;

	double dstSumX = 0.0f;
	double dstSumY = 0.0f;
	double dstSumZ = 0.0f;

	//至少三组点
	if (srcPoints.size() != dstPoints.size() || srcPoints.size() < 3)
	{
		return cv::Mat();
	}

	int pointsNum = srcPoints.size();
	for (int i = 0; i < pointsNum; ++i)
	{
		srcSumX += srcPoints[i].x;
		srcSumY += srcPoints[i].y;
		srcSumZ += srcPoints[i].z;

		dstSumX += dstPoints[i].x;
		dstSumY += dstPoints[i].y;
		dstSumZ += dstPoints[i].z;
	}

	cv::Point3d centerSrc, centerDst;

	centerSrc.x = double(srcSumX / pointsNum);
	centerSrc.y = double(srcSumY / pointsNum);
	centerSrc.z = double(srcSumZ / pointsNum);

	centerDst.x = double(dstSumX / pointsNum);
	centerDst.y = double(dstSumY / pointsNum);
	centerDst.z = double(dstSumZ / pointsNum);

	//Mat::Mat(int rows, int cols, int type)
	cv::Mat srcMat(3, pointsNum, CV_64FC1);
	cv::Mat dstMat(3, pointsNum, CV_64FC1);
	---Modify
	for (int i = 0; i < pointsNum; ++i)//N组点
	{
		//三行
		srcMat.at<double>(0, i) = srcPoints[i].x - centerSrc.x;
		srcMat.at<double>(1, i) = srcPoints[i].y - centerSrc.y;
		srcMat.at<double>(2, i) = srcPoints[i].z - centerSrc.z;

		dstMat.at<double>(0, i) = dstPoints[i].x - centerDst.x;
		dstMat.at<double>(1, i) = dstPoints[i].y - centerDst.y;
		dstMat.at<double>(2, i) = dstPoints[i].z - centerDst.z;

	}

	cv::Mat matS = srcMat * dstMat.t();

	cv::Mat matU, matW, matV;
	cv::SVDecomp(matS, matW, matU, matV);

	cv::Mat matTemp = matU * matV;
	double det = cv::determinant(matTemp);//行列式的值

	double datM[] = { 1, 0, 0, 0, 1, 0, 0, 0, det };
	cv::Mat matM(3, 3, CV_64FC1, datM);

	cv::Mat matR = matV.t() * matM * matU.t();

	double* datR = (double*)(matR.data);
	double delta_X = centerDst.x - (centerSrc.x * datR[0] + centerSrc.y * datR[1] + centerSrc.z * datR[2]);
	double delta_Y = centerDst.y - (centerSrc.x * datR[3] + centerSrc.y * datR[4] + centerSrc.z * datR[5]);
	double delta_Z = centerDst.z - (centerSrc.x * datR[6] + centerSrc.y * datR[7] + centerSrc.z * datR[8]);


	//生成RT齐次矩阵(4*4)
	cv::Mat R_T = (cv::Mat_<double>(4, 4) <<
		matR.at<double>(0, 0), matR.at<double>(0, 1), matR.at<double>(0, 2), delta_X,
		matR.at<double>(1, 0), matR.at<double>(1, 1), matR.at<double>(1, 2), delta_Y,
		matR.at<double>(2, 0), matR.at<double>(2, 1), matR.at<double>(2, 2), delta_Z,
		0, 0, 0, 1
		);

	return R_T;
}

以下是简单自测代码:

int main()
{
	std::vector<cv::Point3f> srcPoints;
	std::vector<cv::Point3f>  dstPoints;

//模拟绕Z轴转90度，再X轴平移50.
//取三组点
	float NN = 100;
	srcPoints.push_back(cv::Point3f(NN, 0, 0));
	dstPoints.push_back(cv::Point3f(50+0, NN, 0));

	srcPoints.push_back(cv::Point3f(0, NN, 0));
	dstPoints.push_back(cv::Point3f(50 -NN, 0, 0));

	srcPoints.push_back(cv::Point3f(0, 0, NN));
	dstPoints.push_back(cv::Point3f(50+0, 0, NN));

	cv::Mat RT = Get3DR_TransMatrix(srcPoints, dstPoints);
	for (int r = 0; r < RT.rows; r++)
	{
		for (int c = 0; c < RT.cols; c++)
		{
			printf("%f, ", RT.at<double>(r, c));
		}
		printf("\n");
	}
	printf("**************************************\n");
	getchar();
	}

执行结果:

 

说明：

这个方法对固定的两个坐标系 转换矩阵比较精确，对于实际测量的数据，比如：GPS测量的位置数据，VIO测量的位置数据，因为他们之间存在噪声，此时得到的转换矩阵是不准确的，大多情况下都是不准确的！这个时候应该考虑基于优化的方法！

 

https://blog.csdn.net/sinat_29886521/article/details/77506426?utm_medium=distribute.pc_relevant.none-task-blog-baidujs_baidulandingword-2&spm=1001.2101.3001.4242