Kabsch算法

Posted on In , Views: Disqus:

Kabsch 算法

Kabsch 算法是一种用于求解两组三维点（或向量）之间最佳旋转矩阵的经典方法，广泛应用于计算机视觉、机器人学和分子动力学等领域。在你的问题中，Kabsch 算法被用来求解旋转矩阵 \(R\)，使得摄像头的角速度 \(\text{gyros}_{\text{cam}}\) 和 IMU 的角速度 \(\text{gyros}_{\text{imu}}\) 之间的均方误差

\[\|\text{gyros}_{\text{cam}} - R \cdot \text{gyros}_{\text{imu}}\|_{2}^{2}\]

最小化。以下是对 Kabsch 算法数学原理的详细解析。

1. 问题定义

假设我们有两组三维向量：

  • \(A = \{\mathbf{a}_{i}\}_{i=1}^{N}\)，表示 IMU 的角速度 \(\text{gyros}_{\text{imu}}\)，每个 \(\mathbf{a}_{i} \in \mathbb{R}^{3}\)。
  • \(B = \{\mathbf{b}_{i}\}_{i=1}^{N}\)，表示摄像头的角速度 \(\text{gyros}_{\text{cam}}\)，每个 \(\mathbf{b}_{i} \in \mathbb{R}^{3}\)。

目标是找到一个旋转矩阵 \(R \in \mathrm{SO}(3)\)（即满足 \(R^{T}R = I\) 且 \(\det(R) = 1\)），使得以下误差最小化：

\[E = \sum_{i=1}^{N} \|\mathbf{b}_{i} - R\mathbf{a}_{i}\|_{2}^{2}\]

这里，\(R\mathbf{a}_{i}\) 表示将 IMU 角速度 \(\mathbf{a}_{i}\) 旋转到摄像头坐标系后的向量。

2. 数学推导

2.1 中心化数据（在角速度标定中通常省略）

在点云配准中常需要中心化，但在角速度对齐问题中不需要，直接使用原始数据即可。

2.2 目标函数展开

\[ \begin{align*} E &= \sum_{i=1}^{N} \|\mathbf{b}_{i} - R\mathbf{a}_{i}\|_{2}^{2} \\ &= \sum_{i=1}^{N} \left( \mathbf{b}_{i}^{T}\mathbf{b}_{i} - 2\mathbf{b}_{i}^{T}R\mathbf{a}_{i} + \mathbf{a}_{i}^{T}R^{T}R\mathbf{a}_{i} \right) \end{align*} \]

因为 \(R^{T}R = I\)，上式简化为

\[E = \sum_{i=1}^{N}\mathbf{b}_{i}^{T}\mathbf{b}_{i} + \sum_{i=1}^{N}\mathbf{a}_{i}^{T}\mathbf{a}_{i} - 2\sum_{i=1}^{N}\mathbf{b}_{i}^{T}R\mathbf{a}_{i}\]

前两项与 \(R\) 无关，故最小化 \(E\) 等价于最大化

\[\sum_{i=1}^{N} \mathbf{b}_{i}^{T} R \mathbf{a}_{i}\]

2.3 协方差矩阵

定义 \(3\times N\) 矩阵 \(A\) 和 \(B\)（列分别为 \(\mathbf{a}_{i}\)、\(\mathbf{b}_{i}\)），则

\[\sum_{i=1}^{N} \mathbf{b}_{i}^{T} R \mathbf{a}_{i} = \operatorname{trace}(B^{T} R A)\]

令协方差矩阵

\[H = AB^{T} = \sum_{i=1}^{N} \mathbf{a}_{i} \mathbf{b}_{i}^{T} \in \mathbb{R}^{3\times 3}\]

目标变为最大化 \(\operatorname{trace}(R H)\)。

2.4 奇异值分解（SVD）

对 \(H\) 进行 SVD：

\[H = U \Sigma V^{T}\]

其中 \(\Sigma = \operatorname{diag}(\sigma_{1}, \sigma_{2}, \sigma_{3})\)，且

\[\sigma_{1} \geq \sigma_{2} \geq \sigma_{3} \geq 0\]

则

\[\operatorname{trace}(R H) = \operatorname{trace}(R U \Sigma V^{T}) = \operatorname{trace}(\Sigma V^{T} R U)\]

令 \(S = V^{T} R U\)（\(S\) 为正交矩阵），最大化 \(\operatorname{trace}(\Sigma S)\) 的最优解为 \(S = I\)，此时

\[\operatorname{trace}(\Sigma S) = \sigma_{1} + \sigma_{2} + \sigma_{3}\]

因此

\[V^{T} R U = I \quad \Rightarrow \quad R = V U^{T}\]

2.5 修正行列式（确保 \(\det(R) = +1\)）

计算 \(R = V U^{T}\) 后，若 \(\det(R) = -1\)（反射），则修正为：

\[V' = V \cdot \operatorname{diag}(1, 1, -1)\]

\[R = V' U^{T}\]

此时 \(\det(R) = +1\)，为合法旋转矩阵。

3. 算法步骤总结

  1. 构造协方差矩阵
    \[H = \sum_{i=1}^{N} \mathbf{a}_{i} \mathbf{b}_{i}^{T}\]

  2. 对 \(H\) 进行 SVD
    \[H = U \Sigma V^{T}\]

  3. 计算 \(R = V U^{T}\)

  4. 若 \(\det(R) < 0\)，则 \(V \leftarrow V \cdot \operatorname{diag}(1,1,-1)\)，重新计算 \(R\)

4. 在你的代码中的实现（C++/Eigen）

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
// 2. 构建协方差矩阵 H = sum(P_imu * P_cam^T)
Eigen::Matrix3d H = Eigen::Matrix3d::Zero();
for (size_t i = 0; i < gyros_cam.size(); ++i) {
    // P_cam 是 gyros_cam, P_imu 是 gyros_imu_synced
    H += gyros_imu_synced[i] * gyros_cam[i].transpose();
}

// 3. 对 H 进行 SVD 分解
Eigen::JacobiSVD<Eigen::Matrix3d> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
const Eigen::Matrix3d& U = svd.matrixU();
const Eigen::Matrix3d& V = svd.matrixV();

// 4. 计算旋转矩阵 R = V * U^T
Eigen::Matrix3d R = V * U.transpose();

// 确保 R 是一个纯旋转矩阵 (det(R) = +1)
if (R.determinant() < 0) {
    std::cout << "检测到反射，正在修正..." << std::endl;
    Eigen::Matrix3d V_prime = V;
    V_prime.col(2) *= -1; // 翻转最后一列的符号
    R = V_prime * U.transpose();
}

// --- 标定结束，输出结果 ---
std::cout << "\n--- 标定结果 ---" << std::endl;
std::cout << "计算出的旋转矩阵 R (IMU -> Camera):" << std::endl;
std::cout << R << std::endl;

// (可选) 验证误差
double total_error = 0.0;
for (size_t i = 0; i < gyros_cam.size(); ++i) {
    Eigen::Vector3d error_vec = gyros_cam[i] - R * gyros_imu_synced[i];
    total_error += error_vec.squaredNorm();
}
double mean_squared_error = total_error / gyros_cam.size();
std::cout << "\n标定后的均方误差 (MSE): " << mean_squared_error << std::endl;

5. 数学性质与局限性

5.1 性质
  • 唯一性：如果数据点 $ {i} $ 和 $ {i} $ 线性无关（即 $ H $ 的秩为 3），Kabsch 算法给出的 $ R $ 是唯一的全局最优解。
  • 效率：SVD 分解的计算复杂度为 $ O(n) $，其中 $ n $ 是数据点数量，适合实时应用。
  • 几何意义：Kabsch 算法本质上是找到一个旋转，将一组向量尽可能对齐到另一组向量。
5.2 局限性
  • 噪声敏感性：如果 $ {} $ 或 $ {} $ 包含大量噪声，Kabsch 算法可能不够鲁棒，需要预处理数据（如滤波）。
  • 时间同步：算法假设两组向量是一一对应的。如果时间戳未对齐，需先进行插值。
  • 退化情况：如果 $ {i} $ 或 $ {i} $ 线性相关（例如所有向量共线），SVD 分解可能不稳定，导致 $ R $ 不唯一。

7. 总结

Kabsch 算法通过 SVD 分解协方差矩阵 $ H = A B^T $，高效地求解最佳旋转矩阵 $ R $，其核心数学原理是最大化 $ (R H) $。在你的应用中，它将 IMU 角速度旋转到摄像头坐标系，适用于时间同步后的角速度数据。对于噪声较大或复杂约束的情况，可以结合非线性优化进一步提高精度。