这篇文章的相应数学推到在这个地方，有兴趣的可以瞧一瞧计算两个点集合的旋转矩阵R和T的数学推导
假设有两个点集A和B，且这两个点集合的元素数目相同且一一对应。为了寻找这两个点集之间的旋转矩阵$R$和转移矩阵$t$。可以将这个问题建模成如下的公式：
$$B=R\ast A+t$$
为了寻找$R$和$t$，通常需要一下三个步骤：

• 计算点集合的中心点
• 将点集合移动到原点，计算最优旋转矩阵$R$
• 计算转移矩阵$t$

计算中心点

$$\begin{gathered} P=\left[\begin{array}{c}x\\ y\\ z\end{array}\right] \\ {\mu }_A=\frac{1}{N}\sum _{i=1}^N{P}_A^i \\ {\mu }_B=\frac{1}{N}\sum _{i=1}^N{P}_B^i \end{gathered}$$

将点集合移动到原点，计算最优旋转矩阵$R$

为了计算旋转矩阵$R$，需要消除转移矩阵$t$的影响，所以我们首先需要将点集重新中心化，生成新点集合$A^{\prime }$ 和$B^{\prime }$，然后计算性的点集之间的协方差矩阵：

点集重新中心化

A i ′ = { P A i − μ A } B i ′ = { P B i − μ B } A'_i = \{ P_A^i-\mu_A\} \\ B'_i = \{ P_B^i-\mu_B \} Ai′​={PAi​−μA​}Bi′​={PBi​−μB​}
注意其中的，$P_A^i$ 、$ P_B^i$ 、${\mu }_A$和${\mu }_B$不是标量是向量。

计算点集之间的协方差矩阵H

$$\begin{gathered} H=\sum _{i=1}^N{A}_i^{{}^{\prime }}{B_i^{{}^{\prime }}}^T \\ =\sum _{i=1}^N(P_A^i-{\mu }_A)(P_B^i-{\mu }_B{)}^T \end{gathered}$$
通过SVD方法获得矩阵的$U$、$S$和$V$,可以计算点集之间的旋转矩阵$R$

$$\begin{gathered} \left[U,S,V\right]=SVD(H) \\ R=V{U}^T \end{gathered}$$

计算转移矩阵$t$

最后，通过$R$可以获得转移矩阵$t$
$$t=-R\times {\mu }_A+{\mu }_B$$

下面通过python代码编写一个小例子验证一下上面的公式。
下面这个例子，首先通过随机函数生成两个三维点集A，旋转矩阵$R$和$t$。通过公式$B=R{A}^T+t$获得新的点集B。然后通过上述的方法计算点集A和B之间的旋转矩阵$R^{\prime }$和$t’$，通过公式$B^{\prime }=R^{\prime }{A}^T+t^{\prime }$生成新的点集合B’。并计算两个点集合之间的RMSE。

from numpy import *
from math import sqrt
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as pltdef rigid_transform_3D(A, B):assert len(A) == len(B)N = A.shape[0];mu_A = mean(A, axis=0)mu_B = mean(B, axis=0)AA = A - tile(mu_A, (N, 1))BB = B - tile(mu_B, (N, 1))H = transpose(AA) * BBU, S, Vt = linalg.svd(H)R = Vt.T * U.Tif linalg.det(R) < 0:print "Reflection detected"Vt[2, :] *= -1R = Vt.T * U.Tt = -R * mu_A.T + mu_B.Treturn R, tif __name__=='__main__':R = mat(random.rand(3,3))t = mat(random.rand(3,1))U,S,Vt = linalg.svd(R)R = U*Vtif linalg.det(R) < 0:Vt[2,:]*=-1R = U*Vtn = 10A = mat(random.rand(n,3))B = R*A.T + tile(t,(1,n))B = B.Tret_R, ret_t = rigid_transform_3D(A,B)A2 = (ret_R*A.T)+ tile(ret_t,(1,n))A2 =A2.Terr = A2-Berr = multiply(err,err)err = sum(err)rmse = sqrt(err/n)print "points A2"print A2print ""print "points B"print Bprint ""print rmsefig = plt.figure()ax=fig.add_subplot(111,projection='3d')ax.scatter(A[:,0],A[:,1],A[:,2])ax.scatter(B[:,0],B[:,1],B[:,2],s=100,marker='x')ax.scatter(A2[:,0],A2[:,1],A2[:,2],s=100,marker= 'o')plt.legend()plt.show()

通过上述代码生成的点集$B^{\prime }$和$B$中的所有元素如下所示：

points A2
[[0.65985419 1.60528398 1.38340275][0.92739301 1.68052107 0.90692937][0.3398634  1.20672748 1.24869353][0.76117272 1.46282089 1.35712503][0.45657103 1.17657746 0.9993309 ][0.86068981 1.76370772 0.53447625][0.46723696 0.98764769 1.06947054][0.67152812 1.00675099 0.73363394][0.3102857  1.23971537 0.86977264][0.495524   1.10873545 0.93223688]]points B
[[0.65985419 1.60528398 1.38340275][0.92739301 1.68052107 0.90692937][0.3398634  1.20672748 1.24869353][0.76117272 1.46282089 1.35712503][0.45657103 1.17657746 0.9993309 ][0.86068981 1.76370772 0.53447625][0.46723696 0.98764769 1.06947054][0.67152812 1.00675099 0.73363394][0.3102857  1.23971537 0.86977264][0.495524   1.10873545 0.93223688]]

上面小程序的可视化结果如下所示。其中，绿色的圆球和红色的"X"分别表示利用上述方法生成点集$B^{\prime }$和$B$。蓝色的小圆球表示点集合$A$。

【引用】

1. Finding optimal rotation and translation between corresponding 3D points