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. 算法步骤总结
构造协方差矩阵
\[H = \sum_{i=1}^{N} \mathbf{a}_{i}
\mathbf{b}_{i}^{T}\]
对 \(H\) 进行 SVD
\[H = U \Sigma V^{T}\]
计算 \(R = V U^{T}\)
若 \(\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
|
Eigen::Matrix3d H = Eigen::Matrix3d::Zero();
for (size_t i = 0; i < gyros_cam.size(); ++i) {
H += gyros_imu_synced[i] * gyros_cam[i].transpose();
}
Eigen::JacobiSVD<Eigen::Matrix3d> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
const Eigen::Matrix3d& U = svd.matrixU();
const Eigen::Matrix3d& V = svd.matrixV();
Eigen::Matrix3d R = V * U.transpose();
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
角速度旋转到摄像头坐标系，适用于时间同步后的角速度数据。对于噪声较大或复杂约束的情况，可以结合非线性优化进一步提高精度。
