### Python 实现根据两组点集计算旋转平移矩阵 (RT) 对于给定的两组三维点集 `Points` 和 `Points_prime`，可以通过以下方法来计算它们之间的刚体变换矩阵 \( \text{RT} \)，该过程涉及到了重心计算、中心化处理以及利用奇异值分解(SVD)求解旋转矩阵和平移向量。 #### 1. 计算点集的重心并进行中心化 首先，针对每组点集分别计算其几何中心（即平均位置），接着将各点相对于各自中心的位置矢量化表示出来： ```python import numpy as np def center_points(points): """Center the points by subtracting their mean.""" centroid = points.mean(axis=0) centered = points - centroid return centered, centroid ``` 此函数接收一组三维坐标数组作为输入参数，并返回经过中心化的点阵及其原始质心[^1]。 #### 2. 利用SVD求解旋转矩阵 基于上述得到的已中心化的源点集(`src`)与目标点集(`dst`)构建协方差矩阵H，并对其进行奇异值分解以获取最佳拟合旋转矩阵\( R \): ```python def compute_rotation_matrix(src_centered, dst_centered): """Compute rotation matrix using SVD on covariance matrix of source and destination points.""" H = src_centered.T @ dst_centered U, _, Vt = np.linalg.svd(H) # Ensure a proper rotation without reflection if np.linalg.det(Vt.T @ U.T) < 0: Vt[2,:] *= -1 R = Vt.T @ U.T return R ``` 这里特别注意当检测到反射情况时调整V的最后一行符号，从而保证最终获得的是纯旋转变换而非镜像翻转操作[^4]。 #### 3. 计算平移向量 最后一步则是依据之前所求得的旋转矩阵及两个点集中各自的质心信息来决定具体的位移分量: ```python def calculate_translation_vector(R, src_centroid, dst_centroid): """Calculate translation vector based on centroids and computed rotation matrix.""" t = dst_centroid.reshape(-1, 1) - R @ src_centroid.reshape(-1, 1) return t.flatten() ``` 综上所述，完整的流程如下所示： ```python def solve_rt_by_svd(src, dst): """ Solve for Rotation Matrix R and Translation Vector T between two sets of corresponding 3D points. Parameters: src : ndarray shape(N,3), Source point set dst : ndarray shape(N,3), Destination point set Returns: tuple(ndarray(shape=(3,3)),ndarray(shape=(3,))): The rotation matrix R and translation vector t """ # Step 1: Center both point clouds around origin src_centered, src_centroid = center_points(src) dst_centered, dst_centroid = center_points(dst) # Step 2: Compute optimal rotation via SVD R = compute_rotation_matrix(src_centered, dst_centered) # Step 3: Calculate final translation from centriods after applying rotation to source cloud's centroid t = calculate_translation_vector(R, src_centroid, dst_centroid) return R, t ``` 通过调用`solve_rt_by_svd()` 函数即可完成整个转换关系的学习工作。值得注意的是，在实际应用过程中可能还需要考虑诸如噪声过滤等问题，以便提高匹配精度和鲁棒性。