如何计算两个坐标系间的转换矩阵？

Question

**问题：如何计算两个三维坐标系之间的转换矩阵？** 在计算机图形学、机器人学或SLAM等领域中，常常需要计算两个三维坐标系之间的转换矩阵，该矩阵通常包含旋转和平移信息。假设已知在两个坐标系下同一组点的坐标，如何求解从源坐标系到目标坐标系的刚体变换矩阵？常见方法包括使用SVD分解（如Kabsch算法）求解最优旋转矩阵，并结合质心计算平移部分。该方法广泛应用于点云配准、姿态估计等任务中。

Answer 1

一、问题背景与基本定义

在计算机图形学、机器人学或SLAM等领域中，常常需要计算两个三维坐标系之间的转换矩阵。该矩阵通常包含旋转和平移信息，用于描述一个坐标系中的点如何变换到另一个坐标系下的对应点。

设我们有两个坐标系，分别为源坐标系 S 和目标坐标系 T，已知在两个坐标系下同一组点的坐标，记为：

• 源点集：$ P = \{ p_1, p_2, ..., p_n \} $，其中 $ p_i \in \mathbb{R}^3 $
• 目标点集：$ Q = \{ q_1, q_2, ..., q_n \} $，其中 $ q_i \in \mathbb{R}^3 $

目标是求解一个刚体变换矩阵 T，使得：

$ q_i = R \cdot p_i + t $

其中 R 是一个 3×3 的旋转矩阵，t 是一个三维平移向量。

二、核心步骤与数学原理

求解该问题的经典方法是 Kabsch 算法，其核心思想是通过奇异值分解（SVD）求解最优旋转矩阵，并结合质心计算平移部分。

1. 计算质心：分别计算源点集和目标点集的几何中心（质心）：
   • $ \bar{p} = \frac{1}{n}\sum_{i=1}^{n} p_i $
   • $ \bar{q} = \frac{1}{n}\sum_{i=1}^{n} q_i $
2. 去中心化坐标：将所有点减去各自质心，得到去中心化后的点集：
   • $ p'_i = p_i - \bar{p} $
   • $ q'_i = q_i - \bar{q} $
3. 构造协方差矩阵：计算协方差矩阵 H：

   $ H = \sum_{i=1}^{n} p'_i \cdot q'_i^T $

4. 奇异值分解（SVD）：对 H 进行 SVD 分解：

   $ H = U \Sigma V^T $

5. 计算最优旋转矩阵：

   $ R = V U^T $

   如果 det(R) = -1，则需要翻转最小奇异值对应的列向量以确保旋转为右手系。

6. 计算平移向量：

   $ t = \bar{q} - R \cdot \bar{p} $

三、算法流程图与伪代码

以下是 Kabsch 算法的流程图示意：

graph TD
            A[输入点集 P 和 Q] --> B[计算质心]
            B --> C[去中心化]
            C --> D[构建协方差矩阵 H]
            D --> E[SVD 分解 H = UΣV^T]
            E --> F[计算旋转矩阵 R = VU^T]
            F --> G[调整 R 的行列式]
            G --> H[计算平移向量 t]
            H --> I[输出变换矩阵 T]

伪代码如下：

def compute_transform(P, Q):
    # Step 1: Compute centroids
    centroid_p = np.mean(P, axis=0)
    centroid_q = np.mean(Q, axis=0)

    # Step 2: De-center the points
    P_centered = P - centroid_p
    Q_centered = Q - centroid_q

    # Step 3: Compute covariance matrix
    H = P_centered.T @ Q_centered

    # Step 4: SVD decomposition
    U, S, Vt = np.linalg.svd(H)

    # Step 5: Compute rotation matrix
    R = Vt.T @ U.T
    if np.linalg.det(R) < 0:
        Vt[-1, :] *= -1
        R = Vt.T @ U.T

    # Step 6: Compute translation
    t = centroid_q - R @ centroid_p

    # Step 7: Build transformation matrix
    T = np.eye(4)
    T[:3, :3] = R
    T[:3, 3] = t
    return T
    

四、扩展与应用场景

该方法不仅适用于点云配准，还广泛应用于以下场景：

应用场景	说明
SLAM（同步定位与地图构建）	用于计算机器人当前帧与地图之间的相对变换
姿态估计（PnP问题）	通过2D-3D点匹配求解相机位姿
医学图像配准	将不同模态的图像数据对齐
AR/VR空间定位	实现虚拟物体与现实世界的对齐
机器人抓取与操作	计算物体与机械臂之间的空间变换关系

五、数值稳定性与误差分析

尽管 Kabsch 算法在理论上具有良好的性能，但在实际应用中仍需注意以下几点：

• 点的数量：至少需要3个非共线点才能唯一确定一个刚体变换；点越多，结果越稳定。
• 噪声影响：点集中的噪声可能导致计算结果偏差，可通过 RANSAC 等鲁棒估计方法提升抗干扰能力。
• 尺度一致性：该算法假设点集之间为刚体变换（即无尺度变化），若存在尺度差异需额外估计缩放因子。
• 奇异值处理：当点集共线或共面时，协方差矩阵可能接近奇异，导致 SVD 分解不稳定。