已知点到点对应关系的两个点云对齐方法与变换矩阵求解

你当前已经掌握两组点云的严格一一对应关系，无需使用ICP、深度学习这类需要额外匹配点对的复杂方案，直接用**奇异值分解(SVD)**即可求解最小二乘意义下最优的变换矩阵，误差最小、计算效率极高。

适用前提 #

默认求解仅包含旋转、平移的刚性变换（无全局缩放），如果你的重建点云存在尺度偏移，可参考下文带缩放的扩展方案。

刚性变换（无缩放）求解步骤 #

1. 计算两组点云的质心

• 真值点云X（即[x1, x2, x3...]）的质心：μ_x = (1/N) * Σ_{i=1}^N x_i，N为点的总数量
• 重建点云Y（即[y1, y2, y3...]）的质心：μ_y = (1/N) * Σ_{i=1}^N y_i

2. 对两组点云做去质心处理，得到去中心化点集

• X' = [x1-μ_x, x2-μ_x, ..., xN-μ_x]
• Y' = [y1-μ_y, y2-μ_y, ..., yN-μ_y]

3. 计算协方差矩阵H

• 3D点云场景下每个点为3维向量，H = Y'.T * X'，得到3×3的协方差矩阵

4. 对H做奇异值分解

• [U, S, Vt] = svd(H)，其中U、Vt是正交矩阵，S是奇异值对角矩阵

5. 求解旋转矩阵R

• R = Vt.T * U.T
• 手性校验：如果计算得到的det(R) < 0，说明当前结果是反射矩阵而非合法旋转矩阵，将Vt的最后一行乘以-1后重新计算R即可

6. 求解平移向量t

• t = μ_x - R * μ_y

得到R和t后，对每个重建点做变换R*y_i + t即可对齐到真值点云的坐标系。

带全局缩放的扩展方案 #

如果重建时存在尺度不确定问题（比如单目重建场景），可在上述步骤基础上增加全局缩放因子s的计算：
s = trace(S) / trace(Y'.T * Y')，其中S是SVD分解得到的奇异值对角矩阵
变换公式更新为：s*R*y_i + t

异常点过滤优化 #

如果你的点对中存在少量误差极大的外点，可结合RANSAC框架迭代求解：每次随机采样少量点对计算变换，统计符合变换误差阈值的内点数量，迭代多次后取内点最多的变换结果，再用所有内点重新计算最终的R和t，可大幅降低外点对对齐效果的影响。

对齐后效果评估 #

对齐完成后可直接计算误差量化重建效果：

• 平均绝对误差：MAE = (1/N) * Σ||x_i - 变换后的y_i||_2
• 也可统计误差的中位数、最大值，或绘制误差分布直方图做可视化分析。

参考代码实现（Python） #

import numpy as np
from scipy.linalg import svd

def rigid_transform_3D(Y: np.ndarray, X: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
    """
    已知点对一一对应时求解3D刚性变换
    Args:
        Y: 重建点云，形状为(N, 3)
        X: 真值点云，形状为(N, 3)，点顺序与Y一一对应
    Returns:
        R: 旋转矩阵(3,3)，t: 平移向量(3,1)
        变换公式：Y对齐后 = (R @ Y.T + t).T
    """
    assert Y.shape == X.shape, "两组点云的点数量必须一致"
    N = Y.shape[0]
    # 计算质心
    mu_y = np.mean(Y, axis=0)
    mu_x = np.mean(X, axis=0)
    # 去中心化
    Y_prime = Y - mu_y
    X_prime = X - mu_x
    # 计算协方差矩阵
    H = Y_prime.T @ X_prime
    U, S, Vt = svd(H)
    # 计算旋转矩阵
    R = Vt.T @ U.T
    # 手性校验
    if np.linalg.det(R) < 0:
        Vt[-1, :] *= -1
        R = Vt.T @ U.T
    # 计算平移向量
    t = mu_x.reshape(-1, 1) - R @ mu_y.reshape(-1, 1)
    return R, t

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