已知原矩阵A与含噪变换矩阵B，求对齐两者的旋转角及平移向量方法咨询

核心解法：基于SVD的正交Procrustes方法（质心法是核心前置步骤） #

已知点云矩阵A和变换后的B（含噪声），要还原旋转和平移，最直接有效的方法是结合质心法和奇异值分解（SVD），具体步骤如下：

步骤1：用质心法消除全局平移 #

• 计算A、B的质心：

  centroid_A = np.mean(A, axis=0)
  centroid_B = np.mean(B, axis=0)
  

• 对两点云做去中心化处理（消除平移影响，把问题简化为仅求解旋转）：

  A_centered = A - centroid_A
  B_centered = B - centroid_B
  

步骤2：通过SVD求解最优旋转矩阵 #

• 构造协方差矩阵，描述去中心化后两点云的相关性：

  H = A_centered.T @ B_centered
  

• 对协方差矩阵做奇异值分解：

  U, S, Vt = np.linalg.svd(H)
  

• 计算旋转矩阵，同时处理可能出现的反射情况（保证旋转矩阵行列式为1）：

  R = Vt.T @ U.T
  # 若行列式为负，修正最后一列
  if np.linalg.det(R) < 0:
      Vt[-1, :] *= -1
      R = Vt.T @ U.T
  

步骤3：计算平移向量 #

利用质心的变换关系，直接推导平移向量：

t = centroid_B - centroid_A @ R

关于质心法的定位 #

质心法不是孤立的“最优方案”，它是上述正交Procrustes方法的必要环节——通过消除全局平移，把配准问题简化为纯旋转求解，而结合SVD的整个方法本身就是噪声存在下的最优解法（最小化点对间的平方误差）。

其他可选方法 #

• 迭代最近点（ICP）：如果A和B的点没有明确的一一对应关系（比如点云采样不同），ICP会迭代寻找最近点对，重复SVD求解步骤直到收敛，适合更复杂的无对应点云配准场景。
• 鲁棒配准算法：如果噪声极大或存在大量异常点，可以用RANSAC结合配准逻辑，先剔除异常点再计算旋转和平移，提升结果稳定性。

完整实现代码 #

import numpy as np

# 模拟已知的A和B（实际场景中替换为你的真实数据）
np.random.seed(42)
A = np.random.rand(100, 3)
# 生成真实旋转矩阵、平移向量和噪声
theta = np.pi/4  # 绕Z轴旋转45度
R_true = np.array([
    [np.cos(theta), -np.sin(theta), 0],
    [np.sin(theta), np.cos(theta), 0],
    [0, 0, 1]
])
t_true = np.array([0.2, 0.3, 0.1])
noise = np.random.normal(0, 0.01, A.shape)
B = A @ R_true + t_true + noise

# 步骤1：质心计算与去中心化
centroid_A = np.mean(A, axis=0)
centroid_B = np.mean(B, axis=0)
A_centered = A - centroid_A
B_centered = B - centroid_B

# 步骤2：SVD求解旋转矩阵
H = A_centered.T @ B_centered
U, S, Vt = np.linalg.svd(H)
R = Vt.T @ U.T

# 修正反射情况
if np.linalg.det(R) < 0:
    Vt[-1, :] *= -1
    R = Vt.T @ U.T

# 步骤3：计算平移向量
t = centroid_B - centroid_A @ R

# 输出结果对比
print("估计旋转矩阵：")
print(np.round(R, 3))
print("\n真实旋转矩阵：")
print(np.round(R_true, 3))
print("\n估计平移向量：")
print(np.round(t, 3))
print("\n真实平移向量：")
print(np.round(t_true, 3))

内容的提问来源于stack exchange，提问作者cerv21