Python求解两组三维点之间的刚体变换矩阵

给定两组对应的三维点的坐标，分别存储在变量 Points 和 Points_prime 中。代码首先对两组点分别计算了点集的重心，并将点集中心化（将每个点坐标减去点集重心）。然后，通过奇异值分解（SVD）求解旋转矩阵，使用 SVD 方法可以在保证计算稳定性的同时，可以在奇异矩阵（Singular matrix）存在的情况下计算出解。求出旋转矩阵后，根据重心的偏移量求出平移向量，并将旋转矩阵

和平移向量组合成一个的变换矩阵返回，即变量 RT。

import numpy as np
import scipy.io as io

real = np.mat([[2079.43, -1547.92, 1134.55], [2034.43, -278.79, 1077.51], \
          [2035.94, -275.07, 573.15], [2882.85, -1173.37, 1682.69]])

# [2079.43, -1547.92, 1134.55], [2034.43, -278.79, 1077.51], \
#           [2035.94, -275.07, 573.15], [2882.85, -1173.37, 1682.69]

point_real = np.zeros(16).reshape(4, 4)
point_real[:, 0:3] = real
point_real[:, 3] = 1
# point_real[:, 1] = real[:, 2].T
# point_real[:, 2] = real[:, 1].T
print(point_real)

model = np.mat([[-824.53, 589.02, -2779.23], [-869.54, 533.76, -1516.28], [-867.87, 30.86, -1508.99], \
               [-20.7, 1137.32, -2420.62]])

model_real = np.zeros(16).reshape(4, 4)
model_real[:, 0:3] = model
model_real[:, 3] = 1
model_real[:, 1] = model[:, 2].T
model_real[:, 2] = model[:, 1].T
print(model_real)

import numpy as np
import cv2

Points = model_real

Points_prime = point_real


P = Points[:, 0:3]
P_hat = Points_prime[:, 0:3]
points1_sum = P.sum(axis=0)
points2_sum = P_hat.sum(axis=0)
print(P, P_hat)
points1_mean = P.mean(axis=0)
points2_mean = P_hat.mean(axis=0)

srcDat = P - points1_mean
dstDat = P_hat - points2_mean

matS = srcDat.T.dot(dstDat)

w, u, v = cv2.SVDecomp(matS)

matTemp = u.dot(v)

det = cv2.determinant(matTemp)

matM = np.eye(3, dtype=np.float64)
matM[2, 2] = det

matR = v.T.dot(matM).dot(u.T)

print(matR)

t_x = points2_mean[0] - points1_mean.dot(matR[0, :].T)
t_y = points2_mean[1] - points1_mean.dot(matR[1, :].T)
t_z = points2_mean[2] - points1_mean.dot(matR[2, :].T)

print(t_x, t_y, t_z)
T = [t_x, t_y, t_z]
print(T)

RT = np.zeros(16).reshape(4, 4)
RT[0:3, 0:3] = matR
RT[0, 3] = t_x
RT[1, 3] = t_y
RT[2, 3] = t_z
RT[3, 3] = 1

# io.savemat("matR_right.mat", {'data': matR})
# io.savemat("T_right.mat", {'data': T})
print(RT)
print((np.matmul(RT, model_real.T) - point_real.T).T)
# io.savemat("RT.mat", {'data': RT})

具体来说，给定两组对应的三维点的坐标，分别存储在变量 Points 和 Points_prime 中。首先，将每个点的坐标都减去它所在点集的重心，这是为了保证变换不受点集的绝对位置影响，只考虑点集之间的相对位置和方向。

然后，假设存在一个的旋转矩阵和一个的平移向量，可以用它们来对第一个点集 进行变换，得到对应的第二个点集： 

我们的目标是求解旋转矩阵和平移向量， 使得变换后的点集 尽可能接近于目标点集 。可以最小化点集之间的距离，例如平均欧氏距离：

 

为了最小化这个距离，可以利用最小二乘法求解 和。可以将和拉成向量形式，然后用来逼近，即： 

 

两边同时左乘，得到： 

 

 这里是的转置。因为和是已知的，所以可以用的值来逼近 ，从而得到：

 

这里是的逆矩阵。将的值代入，可以得到： 

 

两边同时左乘，得到： 

接下来是对平移向量的求解。平移向量是指在变换过程中，整个物体沿着 x、y、z 三个方向发生的位移。可以通过计算两组点集的质心之间的差异来求解平移向量。质心是指物体在空间中的平均位置。

可以首先分别计算出两组点集的质心，然后计算它们之间的差异，就能得到平移向量了。在这里，首先用 sum 方法计算每个点集中所有点的坐标之和，然后用 mean 方法计算出所有点的平均坐标，从而得到两个点集的质心。

接下来，需要将每个点与它所在的点集的质心之间的差异计算出来，这样就能得到每个点在三维空间中的位置相对于它所在的点集的平移量。用 srcDat 和 dstDat 分别表示两个点集中每个点与质心之间的差异，然后将它们放在一个矩阵 matS 中，这个矩阵的大小为 3x3。在这里，使用了 NumPy 的广播机制，将质心向量重复了多次，然后再进行减法操作，从而得到每个点与质心之间的差异。

接下来，需要计算旋转矩阵和平移向量。为了求解旋转矩阵，需要进行奇异值分解（SVD），将矩阵 matS 分解为 U、W 和 V 三个矩阵的乘积。然后，需要将 U 和 V 进行转置，将它们乘起来得到旋转矩阵 matR。

为了求解平移向量，可以使用矩阵运算来计算，将平移向量记为 T，然后计算出每个分量的值，这里的计算方法是将两个质心之间的向量乘以旋转矩阵，再取相反数。最后，将旋转矩阵和平移向量合并成一个 4x4 的变换矩阵 RT，其中最后一个元素设置为 1。

这样，就得到了从一个点集到另一个点集的变换矩阵。可以使用这个矩阵来对点云进行变换，从而得到对应的形状变化和位姿变化。