三点or多点的变换矩阵求解opencv & eigen

《Estimating 3-D Rigid Body Transformations: A Comparison of Four Major Algorithms》，它使用SVD方法计算 T 和 t 。

只要算出变换矩阵，就可以算出A坐标系的一个点P在坐标系B里的对应点坐标，即
R为3x3的转换矩阵， t 为3x1的位移变换向量，这里点坐标均为3x1的列向量（非齐次形式，齐次形式下为4x1列向量，多出的一个元素值补1而已）。理论上只要给定至少3对点，就能计算出 R 和 t 。自然的，点对越多，计算出来的转换就越精确。

Eigen

Eigen::Isometry3d Get3DR_TransMatrix(const std::vector<Eigen::Vector3d>& srcPoints, const std::vector<Eigen::Vector3d>&  dstPoints)
{
   
	Eigen::Isometry3d T = Eigen::Isometry3d::Identity();

	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 T;
	}
	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;
	}
	Eigen::Vector3d 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);

	Eigen::MatrixX3d  srcMat;
	Eigen::MatrixX3d  dstMat;


	for (int i = 0; i < pointsNum; ++i)//N组点
	{
   
		//三行
		srcMat(0, i) = srcPoints[i].x - centerSrc.x;
		srcMat(1, i) = srcPoints[i].y - centerSrc.y;
		srcMat(2, i) = srcPoints[i].z - centerSrc.z;

		dstMat(0, i) = dstPoints[i].x - centerDst