🐋摘要

我们为实时度量-语义视觉-惯性同步定位和建图（SLAM） 提供了一个开源C++库。该库超越了现有的视觉和视觉惯性 SLAM 库（例如ORB-SLAM、VINS-Mono、OKVIS、ROVIO），实现了 3D网格重建和语义标记。Kimera在设计时考虑了模块化，并有四个关键组件：

1. 用于快速准确状态估计的视觉惯性里程计（VIO）模块，
2. 用于全局轨迹估计的强大姿态图优化器，
3. 用于快速网格重建的轻量级3D网格划分器模块，
4. 以及用于快速网格重建的密集3D度量语义重建模块。

这些模块可以单独运行或组合运行，因此Kimera可以很容易地回退到最先进的VIO或完整的SLAM系统。Kimera在CPU上实时运行，并从语义标记的图像中生成3D度量语义网格，这可以通过现代深度学习方法获得。我们希望Kimera提供的灵活性，计算效率，健壮性和准确性将为未来的度量语义SLAM和感知研究奠定坚实的基础，并将允许多个领域（例如，VIO，SLAM，3D重建，分割）的研究人员对自己的工作进行基准测试和原型设计，而不必从头开始。

🌳一、介绍

度量语义理解是同时估计场景的3D几何图形并将语义标签附加到对象和结构（例如，表格，墙壁）的能力。几何信息对于机器人安全导航和操纵物体至关重要，而语义信息为机器人理解和执行人类指令（例如：“给我一杯咖啡”，“从红门退出”）并为人类提供易于理解的环境模型提供了理想的抽象水平。

尽管在几何重建（例如，SLAM [1]，运动结构[2]和多视图立体[3]）和基于深度学习的语义分割（例如，[4]-[10]）方面取得了前所未有的进展，但这两个领域的研究传统上都是孤立进行的。然而，最近人们对这些领域的交叉点的研究和应用越来越感兴趣[1]，[11]-[15]。

这种日益增长的兴趣促使我们创建并发布了Kimera，这是一个用于度量语义定位和建图的库，它将几何和语义理解的最新技术结合到现代感知库中。与针对视觉惯性里程计（VIO）和SLAM的相关努力相反，我们将视觉惯性SLAM，网格重建和语义理解相结合。我们的努力还在几个方面补充了度量和语义理解之间边界的方法。首先，虽然现有的努力集中在RGB-D传感上，但Kimera使用视觉（RGB）和惯性传感，这在更广泛的（室内和室外）环境中效果很好。其次，虽然相关作品[16]–[18]需要GPU进行3D建图，但我们提供了一个快速，轻量级和可扩展的基于CPU的解决方案。最后，我们专注于鲁棒性：我们包括最先进的异常值拒绝方法，以确保Kimera在各种场景中稳健地执行，并且参数调整最少，从真实的基准测试数据集[19]到照片级真实模拟[20]，[21]。

🌾二、相关工作

（mono就是单目相机、stereo是双目相机，lidar是激光雷达，IMU是惯性测量单元，RGB-D是深度相机）

我们请读者参考表 I，以便与现有的 VIO 和视觉 SLAM 系统进行视觉比较，并参考 [1] 以获取有关 SLAM 的更广泛评论。虽然度量语义理解[11]和[33]的早期工作是为离线处理而设计的，但近年来，由SLAM++[16]等开创性工作引发，人们对实时度量语义映射的兴趣激增。这些作品中的大多数（i）依赖于RGB-D相机，（ii）使用GPU处理，（iii）替代跟踪和建图（表I中的“交替”），以及（iv）使用基于体素的（例如，截断的有符号距离函数，TSDF）表面或对象表示。例子包括SemanticFusion[17]，郑等人的方法[15]，泰特诺等人[34]，以及李等人[35]，Fusion++[36]，Mask-fusion[29]，Co-fusion[37]和MID-Fusion[38]。最近的工作研究了基于CPU的方法，例如，Wald等人[39]，PanopticFusion[40]和Voxblox ++ [14];这些也依赖于 RGB-D 传感。一组稀疏的贡献涉及其他传感模式，包括单目摄像头（例如，CNN-SLAM [41]，VSO [42]， VITAMIN-E [43]，XIVO [32]）和lidar（例如，SemanticKitti [44]，SegMap [31]）。XIVO [32] 和Voxblox++ [14] 最接近我们的提案。XIVO [32]是一种基于EKF的视觉惯性方法，可生成基于对象的地图。Voxblox++ [14] 依靠 RGB-D 传感、车轮里程测量和使用 maplab [26] 的预构建地图来获得视觉惯性姿态估计值。与这些工作相反，Kimera（i）提供了基于高精度实时优化的VIO，（ii）使用强大而通用的姿势图优化器，以及（iii）提供轻量级网格重建。

🌟贡献

我们发布了Kimera，这是一个开源C++库，它使用视觉惯性传感来估计机器人的状态，并构建一个轻量级的度量语义网格环境模型。Kimera这个名字源于我们图书馆的混合性质，它统一了各个研究领域的最新工作，包括VIO，姿势图优化（PGO），网格重建和3D语义分割。Kimera包括四个关键模块：

• Kimera-VIO：用于快速准确的IMU速率状态估计的VIO模块。基美拉-VIO的核心是采用基于GTSAM的VIO方法[45]，使用IMU预集成和无结构视觉因子[27]，并在EuRoC数据集上实现最佳性能[19];

• Kimera-RPGO：一种稳健（鲁棒）的位姿图优化（RPGO）方法，利用现代技术进行异常值拒绝[46]。Kimera-RPGO增加了一个健壮性层，避免了由于感知混叠而导致的SLAM故障，并使用户免于耗时的参数调整;

• Kimera-Mesher：一种计算快速每帧和多帧正则化3D网格以支持避障的模块。该网格划分器建立在作者和其他小组先前的算法的基础上 [43]， [47]–[49];

• Kimera-Semantics：一个语义模块，它使用体积法[28]构建一个更慢但更准确的全局3D网格，并使用2D像素语义分割对3D网格进行语义注释。

Kimera既可以使用离线数据集，也可以使用机器人操作系统（ROS）在线工作[50]。它在CPU上实时运行，并提供有用的调试和可视化工具。此外，它是模块化的，允许替换每个模块或单独执行它们。例如，它可以回退到VIO解决方案，或者如果语义标签不可用，它可以简单地估计几何网格。

💐三、Kimera

图2显示了Kimera的建筑。Kimera 将立体帧和高速率惯性测量作为输入并返回 （i） 以 IMU 速率计算的高度准确的状态估计值，（ii） 全局一致的轨迹估计值，以及 （iii） 环境的多个网格，包括快速局部网格和全局语义注释网格。Kimera高度并行化，并使用四个线程来容纳不同速率的输入和输出（例如，IMU，帧，关键帧）。在这里，我们按线程描述体系结构，而每个模块的描述将在以下各节中给出。

• 第一个线程包括Kimera-VIO前端（第II-A节），它获取立体图像和IMU数据，并输出特征轨迹和预集成的IMU测量。前端还发布 IMU 速率状态估计值。
• 第二个线程包括 （i） 返回优化状态估计值的 Kimera-VIO 后端，以及 （ii） Kimera网格（第 II-C 节），用于计算每帧和多帧 3D 网格的低延迟（< 20 毫秒）。这两个线程允许创建图 2（b） 中的每帧网格（也可以带有图 2（c）中的语义标签），以及图 2（d） 中的多帧网格。最后两个线程以较慢的速率运行，旨在支持低频功能，例如路径规划。
• 第三个线程包括 Kimera-RPGO（第 II-B 节），这是一个健壮的 PGO 实现，可检测循环闭包、拒绝异常值并估计全局一致的轨迹（图 2（a））。
• 最后一个线程包括Kimera语义（第 II-D 节），它使用密集的立体和 2D 语义标签，使用 Kimera-VIO 的姿势估计来获得精细的度量语义网格。

🎷A. Kimera-VIO：视觉惯性里程计模块

Kimera-VIO 实现了 [27] 中介绍的基于关键帧的最大后验视觉惯性估计器。在我们的实现中，估计器可以执行完全平滑或固定滞后平滑，具体取决于指定的时间范围;我们通常使用后者来限制估计时间。我们还扩展了 [27] 以处理单目和立体声帧。Kimera-VIO包括一个（视觉和惯性）前端，负责处理原始传感器数据，以及一个后端，它融合处理的测量结果以获得传感器状态的估计值（即，姿态，速度和传感器偏差）。

1) VIO Front-end

我们的IMU前端执行流形预集成[27]，从原始IMU数据中获得两个连续关键帧之间相对状态的紧凑预集成测量值。视觉前端检测Shi-Tomasi角[51]，使用Lukas-Kanade跟踪器[52]跨帧跟踪它们，找到左右立体声匹配，并执行几何验证。我们使用 5 点 RANSAC [53] 执行单声道（透明）验证，并使用 3 点 RANSAC [54] 进行立体声验证;该代码还提供了使用IMU旋转的选项，并分别使用2点[55]和1点RANSAC执行单声道和立体声验证。特征检测、立体匹配和几何验证在每个关键帧执行，而我们只跟踪中间帧的特征。

2) VIO Back-end

在每个关键帧上，预集成的IMU和视觉测量被添加到构成我们的VIO后端的固定滞后平滑器（因子图）中。我们使用预集成的IMU模型和[27]的无结构视觉模型。因子图在 GTSAM [57] 中使用 iSAM2 [56] 求解。在每次 iSAM2 迭代中，无结构视觉模型使用 DLT [58] 估计观测到的特征的 3D 位置，并分析地从 VIO 状态 [59] 中消除相应的 3D 点。在消除之前，将去除退化点（即相机后面的点或没有足够的视差进行三角测量的点）和异常值（即具有较大重投影误差的点），从而提供额外的鲁棒性层。最后，脱离平滑地平线的国家被GTSAM边缘化。

🎺B. Kimera-RPGO：稳健的姿势图优化模块

Kimera-RPGO 负责 （i） 检测当前和过去关键帧之间的循环闭包，以及 （ii） 使用可靠的 PGO 计算全局一致的关键帧姿势。

1) Loop Closure Detection

闭环检测依赖于 DBoW2 库 [60]，并使用词袋表示来快速检测推定的环路。对于每个假定的环路闭包，我们使用单目和双目几何验证拒绝异常值环路闭包（如第 II-A 节所述），并将剩余的环路闭包传递给鲁棒 PGO 求解器。请注意，由于感知混叠，生成的循环闭包仍可能包含异常值（例如，建筑物不同楼层的两个相同房间）。

2) Robust PGO

该模块在GTSAM中实现，包括一种现代异常值抑制方法，增量一致性测量集最大化（PCM）[46]，我们将其定制为单个机器人和在线设置。我们分别存储里程线边缘（由Kimera-VIO产生）和环闭包（由环闭合检测产生）;每次执行 PGO 时，我们首先使用 PCM 的修改版本选择最大的一致循环闭包集，然后在姿势图上执行 GTSAM，包括里程法和一致循环闭包。

PCM专为多机器人案例而设计，仅检查机器人间循环闭合是否一致。我们开发了一个C++的PCM实现，该实现（i）在环闭合上添加了里程测量一致性检查，并且（ii）增量更新了一致测量集以实现在线操作。里程测量检查验证每个环闭包（例如，l1在图2（a））中与里程计一致（图中为红色）：在没有噪声的情况下，沿着由里程计形成的循环和环l的姿态1必须组成标识。与 PCM 一样，我们将沿周期累积的误差与使用卡方检验的测量噪声不一致的异常值循环标记为异常值循环。如果在当前时间检测到的环 t 通过了里程检查，我们将测试它是否与 [46] 中的先前循环闭包成对一致（例如，检查循环 l1和 l2图2（a）中的图2（a）彼此一致）。当 PCM [46] 构建邻接矩阵 A ∈ R 时长×升从头开始跟踪成对一致的循环（其中L是检测到的循环闭包的数量），我们通过增量构建矩阵A来实现在线操作。每次检测到新循环时，我们都会向矩阵 A 中添加一行和列，并且仅针对以前的循环测试新循环。最后，我们使用 [61] 的快速最大集团实现来计算最大的一致循环闭包集。一组一致的测量值被添加到姿态图中（与里程法一起），并使用 Gauss-Newton进行优化。

🎸C. Kimera-Mesher: 3D Mesh Reconstruction

Kimera-Mesher 可以快速生成两种类型的 3D 网格：（i） 每帧 3D 网格，以及 （ii） 跨越 VIO 固定滞后平滑器中的关键帧的多帧 3D 网格。

1) Per-frame mesh

与 [47] 中一样，我们首先对当前关键帧中成功跟踪的 2D 特征（由 VIO 前端生成）执行 2D Delaunay 三角测量。然后，我们使用 VIO 后端的 3D 点估计值，反向投影 2D Delaunay 三角测量以生成 3D 网格（图 2（b））。虽然每帧网格旨在提供低延迟障碍物检测，但我们还提供了通过测量2D标签的网格对3D网格进行语义标记（图2（c））。

2) Multi-frame mesh

多帧网格将 VIO 后退地平线上收集的每帧网格融合成单个网格（图 2（d））。每帧和多帧 3D 网格都编码为顶点位置列表，以及顶点 ID 的三元组列表，用于描述三角形面。假设我们在时间 t − 1 时已经有了一个多帧网格，对于我们生成的每个新的每帧 3D 网格（在时间 t 处），我们循环遍历其顶点和三元组，并添加位于每帧网格中但在多帧网格中缺失的顶点和三元组。然后，我们循环遍历多帧网格顶点，并根据最新的 VIO 后端估计值更新其 3D 位置。最后，我们删除了与在VIO时间范围之外观察到的旧特征相对应的顶点和三元组。结果是一个最新的 3D 网格，该网格跨越当前 VIO 时间范围内的关键帧。如果在网格中检测到平面，则会将规则因子 [47] 添加到 VIO 后端，从而导致 VIO 和网格正则化之间的紧密耦合，有关更多详细信息，请参阅 [47]。

🎻D. Kimera-Semantics: Metric-Semantic Segmentation

我们将 [28] 中引入的BA技术改编为 （i） 构建精确的全局 3D 网格（覆盖整个轨迹），以及 （ii） 对网格进行语义注释。

1) Global mesh

我们的实现建立在Voxblox [28]的基础上，并使用基于体素（TSDF）的模型来过滤掉噪声并提取全局网格。在每个关键帧中，我们使用密集立体声（半全局匹配[62]）从当前立体声对中获取3D点云。然后，我们使用 Voxblox [28] 应用捆绑的光线投射，使用 [28] 中讨论的“快速”选项。在每个关键帧上重复此过程并生成一个 TSFD，使用行进立方体 [63] 从中提取网格。

2) Semantic annotation

Kimera-Semantics使用2D语义标记图像（在每个关键帧处生成）对全局网格进行语义注释;2D语义标签可以使用现成的工具获得像素级2D语义分割，例如深度神经网络[7]–[9]，[64]–[69]或经典的基于MRF的方法[70]。为此，在BA过程中，我们还传播语义标签。使用2D语义分割，我们将标签附加到由密集立体产生的每个3D点上。然后，对于BA中的每个光线束，我们从BA中观察到的标签的频率构建标签概率向量。然后，我们仅在TSDF截断距离（即靠近表面）内沿射线传播此信息以进行备用计算。换句话说，我们节省了更新“空”标签的概率的计算工作。在沿光线遍历体素时，我们使用贝叶斯更新来更新每个体素的标签概率，类似于 [17]。捆绑语义光线投射后，每个体素都有一个标签概率向量，我们从中提取最可能的标签。度量语义网格最终使用行进立方体 [63] 提取。生成的网格比 II-C 部分的多帧网格精确得多，但计算速度较慢（≈ 0.1 秒，请参见第 III-D 节）。

🎹E. Debugging Tools

虽然我们出于篇幅原因限制了讨论，但值得一提的是，Kimera 还提供了一套开源评估工具，用于 VIO、SLAM 和度量语义重建的调试、可视化和基准测试。Kimera包括一个持续集成服务器（Jenkins），它断言代码的质量（编译，单元测试），但也使用evo [71]自动评估EuRoC数据集上的Kimera-VIO和Kimera-RPGO。此外，我们还提供 Jupyter 笔记本来可视化中间 VIO 统计数据（例如，特征轨迹的质量、IMU 预集成错误），以及使用 Open3D [72] 自动评估 3D 重建的质量。

✨四、实验评估

🐰A. 位姿估计性能

表 II 使用 [77] 中的独立报告值和 [24] 中的独立报告值，将Kimera-VIO 的绝对平移误差 （ATE） 与最先进的开源 VIO 管道进行比较：OKVIS [73]、MSCKF [74]、ROVIO [75]、VINS-单声道 [24] 和 SVO-GTSAM [76].请注意，这些算法使用单目相机，而我们使用双目相机。在评估误差之前，我们使用 SE（3） 变换来对齐估计轨迹和真实轨迹。使用 Sim（3） 对齐， 如 [77] 所示， 会给 Kimera 带来更小的误差： 我们更喜欢 SE（3） 对齐， 因为它更适合 VIO， 由于 IMU，比例尺是可观测到的。我们根据这些技术是否使用固定滞后平滑、完全平滑和循环闭包来对它们进行分组。Kimera-VIO 和Kimera-RPGO 在整个频谱中实现了最佳性能。

此外，Kimera-RPGO确保了强大的性能，并且对循环闭包参数调整不太敏感。表III显示了DBoW2中使用的不同环路闭合阈值α有和没有异常值抑制（PCM）的PGO精度。较小的α值会导致更多的循环闭包检测，但这些检测不太保守（更多异常值）。表三表明，通过使用PCM，Kimera-RPGO对α的选择相当不敏感。表II中的结果使用α = 0.001。

🐷B. 几何重建

我们使用EuRoC V1和V2数据集中提供的地面实况点云来评估Kimera生成的3D网格的质量。我们使用 [78， Sec. 4.3] 中的准确性和完整性指标根据实况评估每个网格：（i） 我们通过对均匀密度为 10 的网格进行采样来计算点云3点/米2，（ii）我们使用CloudCompare [80]用ICP[79]记录估计云和地面真实云，并且（iii）我们评估从真实点云到估计点云中其最近邻居的平均距离（精度），反之亦然（完整性）。图3（a）显示了估计的点云（对应于V1_01上的Kimera-Semantics的全球网格），由到地面实况云中最近点的距离（精度）进行颜色编码;图3（b）显示了真实点云，用颜色编码到估计云中最近点的距离（完整性）。


表四提供了Kimera网格生成的快速多帧网格与Kimera语义通过 TSDF 生成的慢速网格之间的定量比较。为了从Kimera网格中获取完整的网格，我们设置了一个大的VIO视界（即，我们执行完全平滑）。如图3（a）所示，来自Kimera-Semantics的全局网格非常准确，数据集的平均误差为0.35 − 0.48m。Kimera-Mesher 产生的网格噪声更大（误差增加高达 24%），但计算时间缩短了两个数量级（参见第 III-D 节）。

🐣C. 语义重建

为了评估来自Kimera-Semantics的度量语义重建的准确性，我们使用麻省理工学院林肯实验室提供的基于Unity的照片级真实感模拟器，该模拟器为场景的几何和语义提供传感器流（在ROS中）和地面实况，并且具有类似于[20]，[21]的接口。为了避免将结果偏向于特定的2D语义分割方法，我们使用基本事实2D语义分割，并向读者推荐[70]以获取潜在的替代方案。

Kimera-Semantics根据VIO姿态估计值构建了3D网格，并使用密集立体和BA的组合。我们通过运行三个不同的实验来评估这些组件中每个组件的影响。首先，我们使用具有真实值 （GT） 姿势和真实深度图（在模拟中可用）的 Kimera-Semantics 来评估由于BA而导致的初始性能损失。其次，我们使用Kimera-VIO的姿势估计。最后，我们使用完整的Kimera语义管道，包括密集立体声。为了分析语义性能，我们计算了平均交并比（mIoU）[13]，以及正确标记的点（Acc）的整体部分[81]。我们还报告了ATE，以将结果与Kimera-VIO产生的漂移相关联。最后，我们评估了度量重建，将估计的网格与地面实况进行注册，并计算了点的 RMSE，如第 III-B 节所示。

表V总结了我们的发现，并表明BA导致几何（3D网格上的<8cm误差）和语义（精度>94%）的性能略有下降。使用Kimera-VIO也会导致性能损失可以忽略不计，因为我们的VIO漂移很小（<0.2%，32米长的轨迹为4厘米）。当然，性能下降最大的是由于使用了密集的立体声。密集立体声[62]很难解析无纹理区域（如墙壁）的深度，这些区域在模拟场景中很常见。图 4 显示了使用Kimera-VIO 和地面实况深度运行Kimera语义时的混淆矩阵（图 4（a）），与使用密集立体（图 4（b））的比较。混淆矩阵中的较大值显示在“墙/架”和“地板/墙”之间。这正是密集立体声遭受最大影响的地方。无纹理的墙壁难以重建，并且靠近架子和地板，导致几何和语义错误增加。

🦄D. 时间安排

图5报告了Kimera模块的时序性能。IMU前端需要大约40μ秒进行预集成，因此可以以IMU速率（>200Hz）生成状态估计值。视觉前端模块显示双峰分布，因为对于每个帧，我们只需执行特征跟踪（平均需要4.5ms），而在关键帧速率下，我们执行特征检测，立体匹配和几何验证，这些组合平均需要45ms。Kimera网格能够在不到5ms的时间内生成每帧3D网格，而构建多帧网格平均需要15ms。后端在不到40ms的时间内求解因子图优化。Kimera-RPGO和Kimera-Semantics在较慢的线程上运行，因为它们的输出不需要时间关键型操作（例如，控制，避障）。在我们在EuRoC上的实验中，Kimera-RPGO平均需要55毫秒，但一般来说，它的运行时间取决于姿势图的大小。最后，Kimera-Semantics（为清楚起见，图中未报告）平均需要0.1秒来更新每个关键帧的全局度量语义网格，融合720×480密集深度的图像，就像我们的模拟器生成的图像一样。

🏅五、结论

Kimera是一个用于度量语义 SLAM 的开源C++库。它包括视觉惯性里程测量、稳健的姿态图优化、网格重建和 3D 语义标记的最新实现。它在CPU上实时运行，并提供一套持续集成和基准测试工具。我们希望Kimera能够为未来机器人感知研究提供坚实的基础，并为社区的研究人员提供易于使用的基础设施。