Kimera2: 面对真实路况中强大且具有准确尺度的语义SLAM

文章：Kimera2: Robust and Accurate Metric-Semantic SLAM in the Real World

作者：Marcus Abate , Yun Chang , Nathan Hughes , and Luca Carlone

编辑：点云PCL

摘要

本文提出了对Kimera的改进，这是一个开源的具有尺度的语义视觉惯性SLAM库。特别增强了Kimera的视觉惯性里程计KimeraVIO，以支持更好的特征跟踪、更有效的关键帧选择，以及各种输入模态（例如，单目、立体、RGB-D图像以及轮速里程计等）。此外Kimera的位姿图优化后端Kimera-RPGO和Kimera-PGMO已经更新，以支持现代的异常值拒绝方法—具体来说，Graduated-Non-Convexity提高对虚假回环闭合的鲁棒性。这些新功能在各种模拟和真实的机器人平台上进行了广泛评估，包括无人机、四足动物、轮式机器人和模拟自动驾驶汽车。我们与几种最先进的视觉惯性SLAM方法进行了比较，并讨论了Kimera新版本的优势和劣势。新增的功能已经以开源形式发布在 https://github.com/MIT-SPARK/Kimera。

主要贡献

Kimera是一款开源的具有尺度的语义视觉惯性SLAM库，采用BSD许可证发布，供广泛的研究界和工业界使用。自2019年首次发布以来，Kimera已经在多个学术和工业项目中得到应用。视觉惯性（VI）SLAM方案的最终用户可能具有各种系统要求，但通常希望实现快速（在线）性能，以及准确而稳健的状态估计和地图创建。为了实现这些性能目标，在Kimera中实施了几项新功能，以提高VIO跟踪性能、稳健的位姿图优化和语义地图创建。此外，Kimera-Multi 和 Hydra对Kimera-VIO的跟踪进行了改进，分别用作多机器人地图制作和3D场景图创建的基准VI-SLAM方案。

本文讨论了我们对Kimera开源版本进行的一些改进，这些改进将作为Kimera各个模块的版本更新的一部分向公众发布，包括关键模块如Kimera-VIO、Kimera-Semantics、Kimera-RPGO和Kimera-PGMO。展示了自2019年发布以来添加到Kimera的一些功能的消融研究，并与现有技术（特别是ORB-SLAM3和VinsFusion）进行了比较，评估了Kimera的优势和劣势以及它在各种平台上的多功能性。实验在从现实平台和模拟环境中收集的数据集上进行（图1）。

内容概述

Kimera2: 系统改进

Kimera-VIO 的前端充当初始数据处理模块，为在 VIO 后端中进行优化的原始传感器测量做准备。前端足够灵活，可为各种传感器输入实施，在最初的 Kimera-VIO 版本中，前端是为双目立体相机和IMU同步假设的情况下实施的。Kimera从一开始就被设计为高度模块化和开发者友好，以支持将来的扩展。这使我们能够为单目IMU输入、RGB-D相机与IMU以及外部（例如轮子）里程计添加实现。

外部里程计模块

• 不同机器人平台提供了多种替代的里程计估算方式，如机载LiDAR传感器提供LiDAR里程计，轮子平台提供轮速里程计，以及一些摄像头（如RealSense T265）提供现成的里程计估算。
• 将这些多样的输入与Kimera-VIO的估算相结合，无需实施特定于传感器的前端，迅速提高状态估算，同时不减慢Kimera的位姿估计线程。因此，我们将外部里程计作为前端的一个独立子模块来处理，作为VIO关键帧之间的相对姿势。

特征处理和跟踪优化

• 对于视觉输入，实施了两个小的改进，提高了图像处理和关键点跟踪的效率。
• 特征划分允许用户提供图像的抽象像素掩码，更均匀地检测特征，特别适用于某些无法使用的图像部分，如使用鱼眼摄像头的自动驾驶汽车。
• 实施各种非极大值抑制的变体，积极剔除前端的特征轨迹，提高特征跟踪的效果，并将计算集中在最有前途的特征上。

关键帧逻辑改进

• 在识别关键帧时，将所有前端测量发送到后端，包括视觉特征、预积分IMU测量和其他可选数据，以包含在VIO固定滞后平滑器中。
• 通过仅对关键帧进行优化，可以在不减慢优化线程的情况下将更多的视觉测量包含在因子图中。
• 更新关键帧逻辑，基于自上一个关键帧以来的时间或关键帧之间的光流差异选择关键帧，避免了在车辆长时间停滞时不必要的关键帧触发，从而减小因子图大小，同时保留足够信息以在较长时间的运动停滞期间保持跟踪。

GNC for Kimera-RPGO and Kimera-PGMO

Kimera-VIO（Visual-Inertial Odometry）的后端（backend）通过创建和优化因子图，从前端（frontend）收集的各种测量，以估算机器人的里程计。为了提高系统对异常值的鲁棒性，引入了Graduated-Non-Convexity（GNC）作为异常值拒绝方法，用于选择性地拒绝不良测量。具体而言，GNC应用非凸优化策略，识别和拒绝由于误差、噪声或环境变化引起的测量偏差。这一方法相对于传统的异常值拒绝方法在处理复杂场景和大规模数据时表现更为优越。

Kimera-LCD（Loop-Closure Detection）处理后端测程仪与前端数据相结合，识别回环闭合，并使用视觉词袋方法进行全局一致的轨迹估算。为了优化因子图，Kimera-RPGO（Robust Pose Graph Optimization）使用GNC进行选择性的异常值拒绝，提高对于虚假回环闭合的鲁棒性。在整个系统中，这些改进共同提高了全局姿态估计和密集度量语义网格的质量。

在Kimera-PGMO中，与Kimera-RPGO共享底层优化框架，也使用GNC进行更精确的网格重建。这些更新和改进通过实验证明在各种机器人平台和数据集上取得了显著的性能提升。

实验

Kimera作为一种灵活适用于各种机器人平台的VI-SLAM库，在不同数据集上进行了实验。这里展示了Kimera的实验结果，并与其他先进的开源VI-SLAM方法进行了比较。

数据集

在各个数据集上获得了结果，其中大部分是公开可用的，以突显每个功能的具体影响，并验证Kimera作为广泛适用的VI-SLAM库的灵活性。实验使用了多样的数据集，包括EuRoC序列和以下几种：

A1和Jackal：这些数据集主要来源于Kimera-Multi项目，包括Unitree A1四足机器人和Clearpath Robotics Jackal轮式机器人的数据。这些数据集在MIT校园的各种环境中录制，包括室内、室外、地下通道等。

uHumans2：该数据集是Kimera早期工作的一部分，包含了一个仿真环境，代理配备前置立体摄像头和模拟IMU。

CarSim：这些数据集在TESSE环境中收集，模拟了室外城市环境，使用汽车作为机器人代理。实验中使用了Kimera的单目模式。

这些实验结果有助于展示Kimera在不同场景和平台上的性能，并且对其新功能进行了验证。

外部里程计

在Jackal机器人上使用了轮子里程计。表格1汇总了来自Jackal机器人的数据集，展示了包含轮子里程计对于定位性能的影响。每个数据集都进行了三次试验，所报告的指标是绝对平移误差（ATE）在所有试验中的均值和标准差。结果显示，外部里程计在许多情况下提升了定位性能，尤其是在室外数据集中。然而，在某些室内数据集中（例如，jackal_hybrid_3主要在室内），使用外部里程计的误差略高。这可能是因为在这些环境中，视觉特征更容易跟踪，并且由于它们大多靠近摄像头，使用立体配置在这些情况下可以更准确地估计深度和轨迹。因此，轮子里程计的误差对于视觉因素的高准确性产生了不成比例的负面影响。

特征划分

特征划分是在Kimera-Multi的A1数据集和CarSim数据集上进行的一项操作，而在其他数据集中并未使用掩码，因此特征划分对它们没有影响。对于A1数据集，划分掩码的设计旨在移除图像底部可见的机器人主体特征。对于CarSim数据集，通过遮蔽通常与天空相关的特征（位于帧的中心和顶部），启用划分操作有助于提高性能。表2中的结果表明，在A1的情况下，如果没有划分掩码，Kimera将完全失败，这一观察也在其他A1数据集中得到验证。而对于CarSim数据集，启用划分操作后，定位误差在各方面都有所改善。总体而言，特征划分对于降低定位误差在具有已知问题特征区域的应用中是一种有效的解决方案。

选取关键帧逻辑

在表3中，我们对Kimera-VIO前端的max_disparity_since_lkf参数进行了消融研究。该参数确定了在超过新关键帧被选中之前的视差阈值（以特征的光流的平均范数为单位）。数值越大，特征在帧中移动的范围就越大，才会选择新的关键帧并触发后端因子图优化。当设置为1000时，系统基本上被禁用，使用的是Kimera先前版本的基于时间的逻辑。由于uHumans2和CarSim序列中的机器人在大部分序列时间内以恒定速度行驶，所以未包括这两个序列，因此该参数影响较小。我们发现，max_disparity_since_lkf值越小，结果通常越好，这印证了在光流视差中识别关键帧的优越性。在某些情况下，每个数据集的最佳和最差结果之间的差异相当大（达到一个数量级）。总体而言，选择在75-100之间的max_disparity_since_lkf值似乎能够始终获得良好的结果。

GNC vs PCM

对GNC（Graduated-Non-Convexity）和基于成对一致性最大化（PCM）的基线进行比较，用于进行姿势图的鲁棒优化。在所有数据集中，我们将PCM的旋转阈值设定为0.01，平移阈值设定为0.05。在所有调查的数据集中，都存在多个回环闭合的候选项。表4显示，对于大多数情况，GNC显著提高了定位性能。这些序列包括来自KimeraMulti和uHumans2的数据。特别是在A1机器人的情况（a1_simmons_0），GNC是使Kimera正常运行所必需的。

PGMO

在先前的研究中，我们展示了Kimera-PGMO对于网格重建的影响。通过联合优化姿势图、回环闭合和网格，我们实现了在密集度量-语义网格上闭合回环并提高网格精度的效果。表5对比了Kimera-Semantics和Kimera-PGMO，其中Kimera-Semantics是在[3]中发布的初始版本。Kimera-PGMO通过引入回环闭合因子提供了更优越的网格重建精度。尽管这项评估最初是在uHumans2数据集上完成的[1]，但我们在新的数据集（CarSim和Kimera-Multi数据集）上复制了这个实验。

对比

自Kimera首次发布以来，其他VI-SLAM方案也进行了更新，以提高性能和功能。例如，Vins-Fusion是备受欢迎的Vins-Mono的继任者，而ORB-SLAM3则是ORB-SLAM2的改进版本。我们通过在多个平台上对Kimera-VIO进行最新改进的性能比较，与这些方案进行对比。对于与其他开源方案的比较。由于ORB-SLAM3只是一个SLAM方案，我们仅与启用了回环闭合的Kimera中的ORB-SLAM3进行比较。由于Vins-Fusion无法执行RGB-D VIO，我们在使用D455相机的A1数据集上省略了结果。这在表中用点（•）表示。此外，由于CarSim数据集中没有立体摄像头，我们显示了Kimera-VIO在单目模式下的结果，并省略了Vins-Fusion在立体模式下的结果。

表6比较了Kimera-VIO（未使用回环闭合）与Vins-Fusion。对于Jackal数据集，Kimera使用外部里程计进行评估，对于CarSim数据集，则在单目模式下运行。对于A1数据集，Kimera使用RGB-D前端进行评估。总体而言，在大多数情况下，Kimera的性能优于Vins-Fusion，Vins-Fusion在几个数据集中出现失败（以破折号表示）。唯一的例外是uHumans2数据集，在这里Vins-Fusion在立体模式下表现明显更好。uHumans2代理没有导致IMU数据干扰，与CarSim数据集中的车辆动力学和频繁的加速刹车操控不同。CarSim和uHumans2都是在相同的模拟环境中开发的，因此差异可能是由场景类型和代理动态引起的。

表7将Kimera与Vins-Fusion和ORB-SLAM3进行了比较，都使用了回环闭合。Vins-Fusion和ORB-SLAM3在单目和立体/RGBD模式下进行评估。请注意，由于ORB-SLAM3支持RGB-D-Inertial VI-SLAM，在A1数据集中使用了该配置，但ORB-SLAM3无法在RGB-D或立体模式下保持一致的跟踪。总体而言，Kimera在大多数情况下胜过竞争对手；Vins-Fusion在uHumans2数据集中具有最低的轨迹误差，而ORB-SLAM3在EuRoC数据集上表现良好。在EuRoC评估中，ORB-SLAM3在大多数情况下优于Kimera和Vins-Fusion，而Vins-Fusion也在某些方面优于Kimera。然而，在CarSim或Jackal数据集等较长的数据集上，ORB-SLAM3的优势减小。

总结

本文介绍了自2019年首次发布以来Kimera所经历的一些关键改进。具体讨论了对Kimera-VIO前端的修改，以支持额外的传感器模态（例如，单目、立体、RGB-D）、可选的外部里程计、图像特征划分以及更新的关键帧选择逻辑。还讨论了对后端的更改，将GNC引入作为异常值拒绝方法，用于鲁棒的姿势图优化。我们通过在各种数据集上进行的广泛消融研究展示了这些改进对定位误差的影响。此外展示了对Kimera-Semantics的密集3D网格重建的改进，通过在多个数据集上评估Kimera-PGMO。最后展示了Kimera与其他开源VI-SLAM方案相比的性能，特别是在大规模数据集中表现出的有利性。