摘要
Kimera（ICRA 2020，arXiv:1910.02490）是 MIT SPARK 实验室发布的开源 C++ 实时度量-语义 SLAM 库，由 Kimera-VIO 、Kimera-RPGO 、Kimera-Mesher 、Kimera-Semantics 四个模块构成。与 ORB-SLAM、VINS-Mono 等系统的核心区别在于：在 CPU 实时 约束下同时完成高精度视觉惯性状态估计、全局一致轨迹优化、低延迟 3D 网格重建和语义标注。EuRoC 数据集上 Kimera-VIO 固定滞后平滑 ATE RMSE 最低达 0.05 m （V1_1 序列），带闭环的 Kimera-RPGO 在全部测试序列上达到最优或次优，且对闭环检测阈值 α\alphaα 近似不敏感（α\alphaα 从 10 降至 0.001 RMSE 仍稳定在 0.045--0.05 m）。
一、问题背景与动机
现有开源 SLAM 系统存在四个共性局限（论文 Table I 的系统对比可直接说明）：
| 方法 | 传感器 | 后端 | 几何表示 | 语义 |
|---|---|---|---|---|
| ORB-SLAM [22] | 单目 | g2o | 点云 | ✗ |
| VINS-Mono [24] | 单目+IMU | Ceres | 点云 | ✗ |
| ElasticFusion [18] | RGB-D | alternation | surfels | ✗ |
| Voxblox [27] | RGB-D | --- | TSDF | ✗ |
| SLAM++ [16] | RGB-D | alternation | objects | ✓ |
| Kimera | 单目/双目+IMU | GTSAM | mesh/TSDF | ✓ |
核心矛盾：几何重建与语义理解长期割裂，且高质量网格重建普遍依赖 RGB-D 或 GPU。Kimera 的定位是：仅用视觉+IMU、CPU 实时、输出带语义标注的全局一致 3D 网格。
二、整体架构
2.1 四模块并行流水线
Thread-4: Kimera-Semantics
Thread-3: Kimera-Mesher
Thread-2: Kimera-RPGO
Thread-1: Kimera-VIO
输入
3D 位姿+路标
关键帧
位姿
全局轨迹
语义网格
Stereo 双目
IMU
VIO Front-end
Shi-Tomasi + LK追踪
5pt/3pt RANSAC
VIO Back-end
iSAM2 + Smart Factors
固定滞后平滑
Loop Closure Detection
DBoW2 词袋
Robust PGO
PCM 外点剔除
Gauss-Newton
Per-frame Mesh
2D Delaunay → 3D反投影
<20ms
Multi-frame Mesh
VIO窗口内融合
Voxblox TSDF
体素化语义
Bundled Raycasting
Bayesian 标签更新
全局语义 3D Mesh
系统以四线程并行推进，各模块输出速率不同：VIO 前端以 IMU 率（典型 200 Hz）发布状态，后端以关键帧率（~10 Hz）优化，Mesher 每帧生成网格（<20 ms），RPGO 和语义重建以更低频率运行。
三、Kimera-VIO：视觉惯性里程计
3.1 前端：特征检测与跟踪
前端在每个关键帧执行特征检测和立体匹配，在中间帧仅做追踪：
- 角点检测：Shi-Tomasi 角点 + ANMS（自适应非极大值抑制），保证特征空间均匀分布
- 跨帧追踪：Lucas-Kanade 光流追踪
- 立体匹配：左右图像匹配建立 3D 路标初值
- 几何验证：单目用 5-point RANSAC（本质矩阵），双目用 3-point RANSAC，可选 2-point 和 1-point RANSAC
3.2 IMU 前端：流形预积分
IMU 前端基于 Forster et al., TRO 2016 的流形预积分理论，对关键帧间 IMU 数据在 SO(3) 上预积分，得到紧凑的预积分测量量（PIM）：
ΔRij=∏k=ij−1Exp ((ω~k−bkg) δt) \Delta\mathbf{R}{ij} = \prod{k=i}^{j-1} \text{Exp}\!\left((\tilde{\boldsymbol{\omega}}_k - \mathbf{b}_k^g)\,\delta t\right) ΔRij=k=i∏j−1Exp((ω~k−bkg)δt)
Δvij=∑k=ij−1ΔRik (a~k−bka) δt \Delta\mathbf{v}{ij} = \sum{k=i}^{j-1} \Delta\mathbf{R}_{ik}\,(\tilde{\mathbf{a}}_k - \mathbf{b}_k^a)\,\delta t Δvij=k=i∑j−1ΔRik(a~k−bka)δt
Δpij=∑k=ij−1[Δvik δt+12ΔRik (a~k−bka) δt2] \Delta\mathbf{p}{ij} = \sum{k=i}^{j-1}\left[\Delta\mathbf{v}{ik}\,\delta t + \tfrac{1}{2}\Delta\mathbf{R}{ik}\,(\tilde{\mathbf{a}}_k - \mathbf{b}_k^a)\,\delta t^2\right] Δpij=k=i∑j−1[Δvikδt+21ΔRik(a~k−bka)δt2]
其中 ω~k\tilde{\boldsymbol{\omega}}_kω~k、a~k\tilde{\mathbf{a}}_ka~k 为陀螺仪/加速度计原始测量，bkg\mathbf{b}_k^gbkg、bka\mathbf{b}_k^abka 为当前零偏估计，Exp(⋅)\text{Exp}(\cdot)Exp(⋅) 为 SO(3) 指数映射。PIM 一经计算可在零偏更新时以一阶线性化修正，无需重新积分。
3.3 后端：Smart Factors + iSAM2 固定滞后平滑
后端构建 GTSAM 因子图，包含三类因子：
| 因子类型 | 数学约束 | 说明 |
|---|---|---|
| IMU 预积分因子 | (ΔR,Δv,Δp)(\Delta\mathbf{R},\Delta\mathbf{v},\Delta\mathbf{p})(ΔR,Δv,Δp) 残差 | 相邻 KF 间运动约束 |
| 结构无关视觉因子（Smart Factor） | 多帧立体重投影误差（隐式边缘化路标） | 不显式保留 3D 点变量 |
| 平面规则性因子（可选） | 点-平面距离约束 | 结构化场景增强 |
Smart Factor 的关键优势：同一路标的所有观测在因子内部通过 DLT 三角化求解，再将其从状态空间解析消去（Schur 补），因子图规模仅随关键帧数增长，与路标数量解耦。退化情况（无足够视差、相机后方）由因子自动检测并移除，提供额外鲁棒性。
优化器使用 iSAM2 （增量平滑与建图）执行固定滞后平滑（fixed-lag smoothing），超出时间窗口的旧状态通过 GTSAM 边缘化移除，保证计算复杂度有界。
四、Kimera-RPGO：鲁棒位姿图优化
4.1 模块职责
Kimera-RPGO（github.com/MIT-SPARK/Kimera-RPGO）负责：
- 接收 DBoW2 检测到的候选闭环，执行单目/双目几何验证
- 用 PCM（Pairwise Consistent Measurement Set Maximization） 剔除伪闭环外点
- 在一致闭环 + 里程计因子上运行 Gauss-Newton 求解全局一致轨迹
4.2 PCM：成对一致性最大化
PCM 的核心思想：两条闭环约束 l1=(a→b)l_1 = (a \to b)l1=(a→b)、l2=(c→d)l_2 = (c \to d)l2=(c→d) 相互一致，当且仅当它们与里程计路径构成的循环误差在噪声范围内趋近于零。
一致性检验通过以下路径合成计算：
Tcycle=Ta→odom c⋅Tc→l2 d⋅(Ta→l1 b⋅Tb→odom d)−1 \mathbf{T}^{\text{cycle}} = \mathbf{T}{a \xrightarrow{\text{odom}}\! c} \cdot \mathbf{T}{c \xrightarrow{l_2}\! d} \cdot \left(\mathbf{T}{a \xrightarrow{l_1}\! b} \cdot \mathbf{T}{b \xrightarrow{\text{odom}}\! d}\right)^{-1} Tcycle=Taodom c⋅Tcl2 d⋅(Tal1 b⋅Tbodom d)−1
若 Tcycle\mathbf{T}^{\text{cycle}}Tcycle 的 Mahalanobis 范数 ∥Tcycle∥Σ<αlc\|\mathbf{T}^{\text{cycle}}\|{\Sigma} < \alpha{\text{lc}}∥Tcycle∥Σ<αlc，则判定 l1l_1l1 与 l2l_2l2 一致。
增量一致矩阵构建：
系统维护一个 L×LL \times LL×L 的邻接矩阵 A∈RL×L\mathbf{A} \in \mathbb{R}^{L \times L}A∈RL×L（LLL 为已检测闭环数）：
Aij={1if li 与 lj 一致0otherwise \mathbf{A}_{ij} = \begin{cases} 1 & \text{if } l_i \text{ 与 } l_j \text{ 一致} \\ 0 & \text{otherwise} \end{cases} Aij={10if li 与 lj 一致otherwise
每新增一条闭环只需追加一行/列并检验新环与历史环的一致性，避免全量重算。
最大团（Max Clique）提取 ：在 A\mathbf{A}A 上运行快速最大团启发式算法（Pattabiraman et al.），得到最大相互一致闭环子集，作为 PGO 的输入。
通过
失败
候选闭环 l_new
里程计一致性检验
循环误差 < odom_threshold?
与历史闭环成对检验
更新 A 矩阵新行/列
最大团搜索
findMaxCliqueHeu
一致闭环集合
GTSAM Gauss-Newton
全局 PGO
丢弃
4.3 PCM 关键参数（SolverParams.h）
| 参数 | 默认值 | 含义 |
|---|---|---|
odom_threshold |
10.0 | 里程计检验 Mahalanobis 距离上限 |
lc_threshold |
5.0 | 成对闭环检验 Mahalanobis 距离上限 |
odom_trans_threshold |
0.05 m | PCM_Simple 每节点平移漂移上限 |
odom_rot_threshold |
0.005 rad | PCM_Simple 每节点旋转漂移上限 |
gnc_inlier_threshold_ |
0.9 | GNC 内点置信度阈值 |
max_iterations_ |
100 | GNC 最大迭代次数 |
4.4 PCM 对阈值 α\alphaα 的不敏感性（论文 Table III）
| α\alphaα | PGO w/o PCM ATE [m] | Kimera-RPGO ATE [m] |
|---|---|---|
| 10 | 0.05 | 0.05 |
| 1 | 0.45 | 0.05 |
| 0.1 | 1.74 | 0.05 |
| 0.01 | 1.59 | 0.045 |
| 0.001 | 1.59 | 0.049 |
小 α\alphaα 会产生大量误检闭环（感知混叠），无 PCM 时系统直接发散（ATE 1.74 m）；加入 PCM 后 ATE 稳定在 0.045--0.05 m，对 α\alphaα 近似不敏感，大幅降低用户调参负担。
五、Kimera-Mesher：快速 3D 网格重建
Mesher 提供两种网格，以不同延迟满足不同需求：
5.1 逐帧网格（Per-frame Mesh，< 20 ms）
- 对 VIO 前端成功追踪的 2D 特征点执行 2D Delaunay 三角化
- 将三角形的 2D 顶点通过 VIO 后端优化的 3D 点云反投影为 3D 顶点
- 可选：用 2D 语义标签纹理贴图到三角面片，得到局部语义网格
逐帧网格以关键帧率更新，低延迟，适合实时避障。
5.2 多帧网格（Multi-frame Mesh）
将 VIO 固定滞后窗口内所有逐帧网格融合为单一网格：
- 新逐帧网格的顶点/三角形若不在当前多帧网格中则追加
- 随 VIO 优化更新顶点 3D 位置
- 超出时间窗口的旧特征对应顶点/三角形移除
六、Kimera-Semantics：度量-语义重建
Kimera-Semantics 以较低频率（比 Mesher 慢）构建全局语义网格：
关键帧 RGB 图
外部 2D 语义分割
深度神经网络
Dense Stereo 深度图
Voxblox TSDF 体素化
Bundled Raycasting
沿射线 Bayesian 标签更新
Marching Cubes
提取语义网格
Bayesian 标签更新：对每个体素维护类别概率向量，每次射线穿过时按观测频率更新：
p(c∣o1:t)∝p(ot∣c) p(c∣o1:t−1) p(c \mid \mathbf{o}_{1:t}) \propto p(\mathbf{o}t \mid c)\, p(c \mid \mathbf{o}{1:t-1}) p(c∣o1:t)∝p(ot∣c)p(c∣o1:t−1)
最终提取概率最高类别作为该体素语义标签。相比逐帧网格，精度提升约 21--24%（V1 序列，见 Table IV），但耗时约 0.1 s/帧。
七、实验结果
7.1 VIO 位姿估计精度（EuRoC，Table II，ATE RMSE [m]）
| 序列 | OKVIS | ROVIO | VINS-Mono | Kimera-VIO（固定滞后） | SVO-GTSAM（全平滑） | Kimera-RPGO（带闭环） |
|---|---|---|---|---|---|---|
| MH_1 | 0.16 | 0.21 | 0.15 | 0.11 | 0.05 | 0.04 |
| MH_2 | 0.22 | 0.25 | 0.15 | 0.10 | 0.06 | 0.06 |
| MH_3 | 0.24 | 0.25 | 0.22 | 0.16 | 0.12 | 0.11 |
| MH_4 | 0.34 | 0.49 | 0.32 | 0.24 | 0.13 | 0.18 |
| MH_5 | 0.47 | 0.52 | 0.30 | 0.35 | 0.16 | 0.20 |
| V1_1 | 0.09 | 0.10 | 0.08 | 0.08 | 0.06 | 0.05 |
| V1_2 | 0.20 | 0.10 | 0.11 | 0.08 | 0.11 | 0.07 |
| V1_3 | 0.24 | 0.12 | 0.18 | 0.07 | --- | 0.07 |
| V2_1 | 0.13 | 0.12 | 0.08 | 0.08 | 0.07 | 0.07 |
| V2_2 | 0.16 | 0.14 | 0.16 | 0.10 | 0.09 | 0.10 |
| V2_3 | 0.29 | 0.14 | 0.27 | 0.21 | --- | 0.19 |
Kimera-VIO 固定滞后平滑与 OKVIS/ROVIO/VINS-Mono 处于同一量级；Kimera-RPGO 带闭环后在多数序列达到最优，且优势在长序列（MH_4/5、V1_3）更明显。
7.2 网格重建精度（EuRoC V1/V2，Table IV）
| 序列 | Multi-frame RMSE [m] | Global RMSE [m] | 全局提升 [%] |
|---|---|---|---|
| V1_01 | 0.482 | 0.364 | 24 |
| V1_02 | 0.374 | 0.384 | -2 |
| V1_03 | 0.451 | 0.353 | 21 |
| V2_01 | 0.465 | 0.480 | -3 |
| V2_02 | 0.491 | 0.432 | 12 |
| V2_03 | 0.530 | 0.411 | 22 |
全局语义网格（Kimera-Semantics）在大多数序列精度优于多帧网格，但计算耗时约高两个数量级（0.1 s vs. <20 ms）。
小结
Kimera 的三个核心设计决策值得关注：
-
Smart Factors 隐式边缘化路标：后端状态空间仅含位姿/速度/偏置，与路标数量解耦，是 CPU 实时运行的根本原因。其代价是因子内部需做线性化三角化，对非线性场景存在近似误差。
-
PCM 增量一致矩阵 + 最大团 ：从根本上将闭环外点剔除转化为组合优化问题，而非逐一阈值过滤。对 DBoW2 阈值 α\alphaα 的不敏感性（Table III）是 PCM 最直接的工程价值------降低了系统调参成本，也说明大量"误闭环"是可以被 PCM 自动过滤的，鲁棒性来自算法而非精心调参。
-
双层网格策略（逐帧 < 20ms + 全局 ~0.1s）：以不同延迟满足不同下游需求，而非强行统一，是模块化设计的典型范例。
局限性：Kimera-Semantics 依赖外部 2D 语义分割，系统本身不包含语义模型训练；Kimera-RPGO 的最大团算法在极高闭环密度场景（大型多机器人 SLAM）下计算开销可能成为瓶颈；逐帧网格的三角化质量依赖 VIO 特征密度，低纹理区域存在空洞。
对于需要在 CPU 上实现定位 + 全局一致轨迹 + 3D 语义地图的机器人系统，Kimera 提供了目前学术界完成度最高、模块拆分最清晰的开源参考实现。
参考资料
- Rosinol A. et al., Kimera: an Open-Source Library for Real-Time Metric-Semantic Localization and Mapping, ICRA 2020. arXiv:1910.02490
- Forster C. et al., On-Manifold Preintegration Theory for Fast and Accurate Visual-Inertial Navigation, IEEE TRO 2016.
- Mangelson J. et al., Pairwise Consistent Measurement Set Maximization for Robust Multi-Robot Map Merging, ICRA 2018.
- 代码仓库：https://github.com/MIT-SPARK/Kimera | https://github.com/MIT-SPARK/Kimera-RPGO