OpenCV 根据对应的三维点估计刚体变换的旋转平移矩阵（RT矩阵）

OpenCV通过estimateAffine3D() 提供了三维仿射变换模型的最小二乘估计方法，但是遗憾的是没有提供三维刚体变换模型（即旋转/平移矩阵，RT矩阵）的估计方法。

下面的代码提供了对该方法的一种实现。

struct TRigidTrans3D
{
	float matR[9];

	float X;
	float Y;
	float Z;
};

void GetRigidTrans3D(cv::Point3f* srcPoints, cv::Point3f* dstPoints, int pointsNum, TRigidTrans3D& transform)
{
	float srcSumX = 0.0f;
	float srcSumY = 0.0f;
	float srcSumZ = 0.0f;

	float dstSumX = 0.0f;
	float dstSumY = 0.0f;
	float dstSumZ = 0.0f;

	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::Point3f centerSrc, centerDst;

	centerSrc.x = float(srcSumX / pointsNum);
	centerSrc.y = float(srcSumY / pointsNum);
	centerSrc.z = float(srcS