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

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

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

[cpp]  view plain  copy

1. struct TRigidTrans3D  
2. {  
3.     double matR[9];  
4.   
5.     double X;  
6.     double Y;  
7.     double Z;  
8. };  
9.   
10. void GetRigidTrans3D(cv::Point3f* srcPoints, cv::Point3f* dstPoints, int pointsNum, TRigidTrans3D& transform)  
11. {  
12.     double srcSumX = 0.0f;  
13.     double srcSumY = 0.0f;  
14.     double srcSumZ = 0.0f;  
15.   
16.     double dstSumX = 0.0f;  
17.     double dstSumY = 0.0f;  
18.     double dstSumZ = 0.0f;  
19.   
20.     for (int i = 0; i < pointsNum; ++ i)  
21.     {  
22.         srcSumX += srcPoints[i].x;  
23.         srcSumY += srcPoints[i].y;  
24.         srcSumZ += srcPoints[i].z;  
25.   
26.         dstSumX += dstPoints[i].x;  
27.         dstSumY += dstPoints[i].y;  
28.         dstSumZ += dstPoints[i].z;  
29.     }  
30.   
31.     cv::Point3f centerSrc, centerDst;  
32.   
33.     centerSrc.x = float(srcSumX / pointsNum);  
34.     centerSrc.y = float(srcSumY / pointsNum);  
35.     centerSrc.z = float(srcSumZ / pointsNum);  
36.   
37.     centerDst.x = float(dstSumX / pointsNum);  
38.     centerDst.y = float(dstSumY / pointsNum);  
39.     centerDst.z = float(dstSumZ / pointsNum);  
40.   
41.     cv::Mat srcMat(3, pointsNum, CV_32FC1);  
42.     cv::Mat dstMat(3, pointsNum, CV_32FC1);  
43.       
44.     float* srcDat = (float*)(srcMat.data);  
45.     float* dstDat = (float*)(dstMat.data);  
46.     for (int i = 0; i < pointsNum; ++ i)  
47.     {  
48.         srcDat[i] = srcPoints[i].x - centerSrc.x;  
49.         srcDat[pointsNum + i] = srcPoints[i].y - centerSrc.y;  
50.         srcDat[pointsNum * 2 + i] = srcPoints[i].z - centerSrc.z;  
51.   
52.         dstDat[i] = dstPoints[i].x - centerDst.x;  
53.         dstDat[pointsNum + i] = dstPoints[i].y - centerDst.y;  
54.         dstDat[pointsNum * 2 + i] = dstPoints[i].z - centerDst.z;  
55.     }  
56.   
57.     cv::Mat matS = srcMat * dstMat.t();  
58.   
59.     cv::Mat matU, matW, matV;  
60.     cv::SVDecomp(matS, matW, matU, matV);  
61.   
62.     cv::Mat matTemp = matU * matV;  
63.     double det = cv::determinant(matTemp);  
64.   
65.     double datM[] = {1, 0, 0, 0, 1, 0, 0, 0, det};  
66.     cv::Mat matM(3, 3, CV_64FC1, datM);  
67.   
68.     cv::Mat matR = matV.t() * matM * matU.t();  
69.   
70.     memcpy(transform.matR, matR.data, sizeof(double) * 9);  
71.   
72.     double* datR = (double*)(matR.data);  
73.     transform.X = centerDst.x - (centerSrc.x * datR[0] + centerSrc.y * datR[1] + centerSrc.z * datR[2]);  
74.     transform.Y = centerDst.y - (centerSrc.x * datR[3] + centerSrc.y * datR[4] + centerSrc.z * datR[5]);  
75.     transform.Z = centerDst.z - (centerSrc.x * datR[6] + centerSrc.y * datR[7] + centerSrc.z * datR[8]);  
76. }  

参考文章：A  comparison of four algorithm for estimating 3-D rigid transformations.pdf