# Kimera2：在现实世界中鲁棒且精准的度量语义SLAM系统 Marcus Abate $^{1}$、Yun Chang $^{1}$、Nathan Hughes $^{1}$ 和 Luca Carlone $^{1}$ 信息与决策系统实验室（LIDS）美国马萨诸塞州剑桥市麻省理工学院 {mabate,yunchang,lcarlone}@mit.edu 摘要。我们针对开源度量语义视觉惯性SLAM库Kimera进行了多项改进。具体而言，我们优化了为Kimera提供支持的视觉惯性里程计流程Kimera-VIO，使其具备更优的特征跟踪能力、更高效的关键帧选择机制，以及支持多种输入模态（如单目、双目、RGB-D图像，以及轮式里程计数据）。此外，Kimera的位姿图优化后端Kimera-RPGO和Kimera-PGMO也得到了更新，可支持现代离群点剔除方法——尤其是渐变非凸性算法——从而提升对虚假回环的鲁棒性。我们在各类仿真及真实机器人平台上对这些新特性进行了广泛测试，包括无人机、四足机器人、轮式机器人以及仿真自动驾驶汽车。我们将Kimera与几款先进的视觉惯性SLAM流程进行了对比，分析了新版Kimera的优势与不足。新增功能已作为开源项目发布在上。关键词：SLAM、定位、建图、视觉惯性导航。 ![Image](images/1d81b6605c9039e8ad2a32f41c75cce64554fa3516fbdf918bc2dfc4e5ce19a1.jpg)

(a)

![Image](images/8e0f9f1a16b80e34dc58d4480252a17e2554d9d431e6d395a5619feda79138be.jpg)

(b)

(c)

![Image](images/ce59ecf478dbae01bdde49e5822b4feeaa410ea0aca67be6497242de33012374.jpg) ![Image](images/5592bd9987d2a13c1467772bd54408637bb1cac73f6b6c4dcc80779adf8ee79f.jpg)

(d)

![Image](images/87deb995cf0414966ea8e63700750e886469437d6e771401c2b3b6b8f19f76bf.jpg)

(e)

Fig. 1: Overview of some of the platforms and datasets used in the experimental evaluation of Kimera. (a) Clearpath Jackal Robot (left), Unitree A1 quadruped (right). (b) Handheld Jetson-based sensor rig, not discussed in this paper, but evaluated in [^1]. (c) uHumans2 simulator office scene. (d) CarSim simulator scene. (e) Self-driving car, not discussed in this paper, but evaluated in [^2].

本研究部分由ARL DCIST CRA W911NF-17-2-0181项目、MathWorks公司以及亚马逊研究奖资助。 --- ## 1 引言 Kimera [^3] [^1]是一款开源的度量语义视觉惯性SLAM库，采用宽松的BSD许可证发布，供科研界和工业界使用。自2019年首次发布以来，Kimera已被应用于多个学术和工业项目中。视觉惯性（VI）SLAM流程的最终用户可能有不同的系统需求，但普遍希望获得快速的（在线）性能，以及精准且鲁棒的状态估计与建图能力。为满足这些性能目标，Kimera中实现了多项新特性，以提升VIO跟踪性能、鲁棒的位姿图优化能力以及语义建图能力。此外，Kimera-Multi [^4] [^5]和Hydra [^6]对Kimera-VIO的跟踪功能进行了优化，分别将其作为多机器人建图和3D场景图创建的基准VI-SLAM流程。在本文中，我们介绍了对Kimera开源版本所做的几项改进，这些改进将作为各Kimera软件包的版本更新向公众发布，其中包括Kimera-VIO、Kimera-Semantics、Kimera-RPGO和Kimera-PGMO等关键模块。我们对2019年Kimera发布后新增的若干特性开展了消融研究，并与现有技术水平（尤其是ORB-SLAM3和Vins-Fusion）进行了对比。我们评估了Kimera的优势、不足以及其在不同平台上的适用性。实验所用数据来自真实平台和仿真环境（见图1）。本文的结构如下：第2节综述相关研究；第3节详细阐述即将接受评估的Kimera新增特性；第4节介绍消融测试和与现有技术的对比结果，并讨论实验结果；最后，第5节总结全文，并提出未来的研究与开发方向。 ## 2 相关工作视觉惯性SLAM。目前已有多种成熟的开源视觉惯性SLAM算法，可应用于研究和工业领域。Kimera [^1] [^3]、Vins-Fusion [^7]、ORB-SLAM3 [^8]以及Open-Vins [^9]是一些用于机器人领域的最新软件包。它们均利用现代传感系统，尤其是配备惯性测量单元（IMU）的立体相机或RGB-D相机，通常能够在复杂的视觉环境中实现亚米级的轨迹估计和长距离3D建图精度。虽然这些算法流程都能进行实时轨迹估计，但据我们所知，Kimera仍是唯一一款既能执行在线视觉惯性SLAM，又能实现实时度量语义稠密3D重建的开源算法流程。感兴趣的读者可在我们之前的研究中看到关于视觉惯性SLAM领域的更深入综述 [^1]。度量语义SLAM。自Kimera发布以来，我们见证了机器人空间感知领域的诸多新研究方法。Hughes等人 [^10] 基于Kimera开发了用于3D场景图构建的实时方法。Jatavallabhula等人 [^11] 提出了ConceptFusion系统，该系统能生成环境的开放集稠密地图，可通过文本、图像、音频和点击操作进行查询。作者后续还扩展了该研究，在ConceptGraphs中创建了3D场景图 [^12]。其他利用场景图表示的最新研究还包括Tourani等人 [^13] 的工作，他们通过将基准标记融入场景图的拓扑结构中来优化状态估计。其他系统则利用了大规模、互联网级别的 --- 训练模型，包括大型语言模型（LLM）。Kassab等人 [^14] 将大型语言模型与视觉惯性观测（VIO）结合使用，以收集关于场景的语义信息，并利用CLIP特征执行回环闭合。更广泛地说，大型语言模型已被应用于各类感知任务，从图像分割到3D地图搜索 [^15] [^16]。读者可参考 [^10] 以获取更详尽的综述。 ## 3 Kimera2：系统改进 ### 3.1 Kimera-VIO前端 Kimera-VIO的前端是一个初始数据处理模块，用于为视觉惯性观测后端的优化准备原始传感器数据。该前端具有足够的灵活性，可适配多种传感器输入。在Kimera-VIO的最初版本中 [^3] [^1]，前端是针对假设已同步的立体相机和IMU设计的。Kimera从设计之初就具备高度模块化且便于开发的特点，以支持未来的功能扩展。这使我们能够为单目-IMU输入、配备IMU的RGB-D相机以及外部（如轮式）里程计添加相应的实现，这些成果随后也向社区发布了。下文我们将详细介绍Kimera-VIO前端模块中新增功能的一部分。外部里程计。各种机器人平台能够提供不同的里程计估计值。例如，搭载激光雷达传感器的平台可提供激光雷达里程计；轮式平台则便于获取轮式里程计数据；某些摄像头（如RealSense T265）也能提供现成的里程计估计值。无需实现特定传感器的前端模块，就能将这些不同来源的数据与Kimera-VIO的估计值进行融合，从而快速优化状态估计，且不会拖慢Kimera的姿态估计线程。因此，我们可选择将外部里程计作为VIO关键帧之间的相对位姿，在前端的一个独立子模块中加以处理。这些相对位姿会与视觉特征及预积分惯性测量单元数据一同被传递给VIO后端。随后，VIO后端会结合视觉特征（双目、单目或RGB-D）、惯性测量单元数据以及里程计测量值（在GTSAM中建模为BetweenFactor），计算出基于滚动时域的更精准的里程计估计值。特征分箱与非极大值抑制。针对视觉输入，我们做了两项小型改进，以提升图像处理和关键点跟踪的效率。特征分箱功能允许用户提供图像的抽象像素掩码，用以指定哪些区域需纳入特征检测范围、哪些区域可忽略；同时还能确保未 masked 区域内的特征检测更加均匀。这一功能在图像部分区域无法使用时尤为实用；例如，若自动驾驶汽车上搭载鱼眼摄像头，车架可能会持续出现在摄像头视野中，进而可能影响VIO性能。此外，我们还实现了多种形式的非极大值抑制机制[^17]，可帮助前端高效剔除不必要的特征轨迹，从而优化特征跟踪效果，并将计算资源集中用于最有潜力的特征上。关键帧逻辑。一旦识别出关键帧，上一个关键帧与当前关键帧之间的所有前端测量数据（包括视觉特征、预积分惯性测量单元数据及其他可选数据）都会被发送至后端，以纳入VIO固定滞后平滑器中。通过将优化范围限定为仅关键帧，我们便能在不拖慢优化线程的前提下，在因子图中纳入更多视觉测量数据。 --- Kimera此前选择关键帧的逻辑是基于用户设定的参数，该参数决定了关键帧之间的时间间隔。但我们发现，对于汽车等车辆而言，其运动轨迹中常会出现几乎静止或完全无移动的长时段。在此类情况下，按固定频率选择关键帧并触发后端优化并无必要，因为姿态并未较上一关键帧发生显著偏移，反而可能导致累积误差增大甚至系统失效。出于这一原因，我们修改了关键帧选择逻辑：要么在上一个关键帧之后经过最长规定时间后选择新关键帧，要么当关键帧之间存在足够大的差异（体现为特征的光流差异）时再选择新关键帧。后一种条件能促使前端仅在机器人发生移动后才选取关键帧，从而节省计算资源。另外，由于后端因子图是基于滚动时域运作的（即属于固定滞后平滑器），仅在机器人移动后才选择关键帧，可防止Kimera“遗忘”机器人静止前的整个近期运动轨迹，避免系统陷入退化状态。更新后的关键帧逻辑通常能减小因子图规模，同时仍能保留足够的历史信息，以确保在长时间近乎静止的状态下仍能维持跟踪能力。 ### 3.2 Kimera-RPGO与Kimera-PGMO的GNC功能 Kimera-VIO的后端会基于前端收集到的各类测量数据，在滑动时间窗口内构建并优化因子图，以此估算机器人的里程计信息。随后，这一里程计估算结果会与闭环检测信息相结合，从而计算出全局一致的轨迹估算值。具体而言，Kimera-LCD（闭环检测模块）会利用视觉词袋方法，结合后端里程计数据以及每个关键帧对应的前端数据（如图像）来识别闭环[^18]。之后，里程计因子和闭环因子会被加入到一个独立的位姿图中，再通过Kimera-RPGO（鲁棒位姿图优化算法）进行优化[^3] [^1]。过去，RPGO依赖成对一致性测量集最大化算法的增量版本来排除虚假的闭环检测结果[^19]。这一机制能够剔除不良的闭环候选结果，而使用视觉词袋方法时，在环境外观相似的场景中这类不良候选结果较为常见。我们在[^1]中指出，采用PCM算法的Kimera-RPGO在全局位姿估算方面取得了显著提升。不过此后，新的异常值排除方法逐渐在该领域崭露头角。Yang等人[^20]证明，渐进非凸性算法在位姿图优化中的表现更为出色。该方法在[^21]中的多种应用场景中得到了验证，且与RANSAC和PCM算法进行了对比。由于这与视觉惯性SLAM相关，因此GNC现已被作为位姿图优化的异常值排除选项集成到Kimera-RPGO中。最后，我们在[^1]中介绍了用于同步优化位姿图和稠密度量语义网格的Kimera-PGMO。由于底层优化框架与Kimera-RPGO共享，GNC也可在此处应用，因此我们也对Kimera-PGMO进行了修改，使其能利用GNC实现更精确的网格重建。 ## 4 实验由于Kimera能够轻松适配各种机器人平台，本节我们将展示Kimera在多种数据集上的实验结果，以及 --- 用于验证第3节中讨论的新功能。此外，我们还将其与其他先进的开源视觉惯性SLAM算法进行对比。 ### 4.1 数据集我们选取了范围广泛的数据集来展示实验结果——其中大多数数据集均可公开获取——旨在凸显各功能的具体影响，并证明Kimera作为一款通用性强的视觉惯性SLAM库的灵活性。图1概述了各类数据集。除了下文所述的仿真数据和真实数据外，我们还在EuRoC序列上评估了其性能[^22]。这些序列的数据采集自无人机平台。 A1和Jackal。其中许多数据集是在Kimera-Multi [^4] [^5] [^23]项目中收集的，包括现已公开、在Unitree A1四足机器人以及Clearpath Robotics Jackal轮式机器人上采集的数据集。A1是一款四足机器人，搭载了用于感知的RealSense D455 RGB-D摄像头，还配备了惯性测量单元和外部里程计。Jackal是一款小型四轮地面机器人，配有立体摄像头和惯性测量单元，以及轮式里程计。这些数据集是在麻省理工学院的多个地点记录的，包括室内外场所、地下隧道以及本科生宿舍。标记为“室内”的数据集是指在麻省理工学院校园内的室内环境中用Jackal机器人采集的数据集。标记为“室外”的数据集是指在麻省理工学院校园外的室外环境中采集的Jackal数据集。标记为“混合”的数据集是指机器人会在室内外之间切换的Jackal数据集。我们还收录了一个A1数据集——标记为a1——它是在本科生宿舍楼（Simmons楼）内记录的。 uHumans2。uHumans2数据集是我们早期关于Kimera [^1]研究的成果之一。该仿真环境及数据集均已开源。该智能体配备了一个朝前的立体摄像头和模拟惯性测量单元。仿真环境的类型多样，从小型公寓到大型地下地铁站都有涵盖。 CarSim。标记为carsim的数据集是在TESSE环境 [^3] [^1] [^24]中采集的。不过与uHumans2 [^3] [^1]数据集不同，这些序列是在模拟的室外城市环境中记录的，所用的机器人智能体是一辆汽车。这辆模拟汽车在前、后、左、右四个方向各安装了一台单目摄像头。在这些消融测试中，我们采用单目模式下的Kimera系统，并使用右侧摄像头。更多关于自动驾驶平台的结果可参考 [^2]。 ### 4.2 外部里程计 Jackal机器人配备了轮式里程计。表1展示了来自Jackal的数据集，以及加入轮式里程计后对定位性能的影响。每个数据集进行了三次试验，所报告的指标是所有试验中绝对平移误差（ATE）均方根误差的均值和标准差。在许多情况下，外部里程计能带来优势，尤其是在室外数据集中。不过对于部分室内数据集而言，加入外部里程计因素后误差反而略大（jackal_hybrid_3主要在室内）。这可能是因为这类环境中的视觉特征更容易被追踪，且这些特征大多靠近摄像头，因此采用立体配置能在这些情况下得到更精确的深度和轨迹估计。这样一来，轮式里程计中的任何误差都会产生更大的负面影响，因为视觉因素本身已经能达到很高的精度。 ---

Dataset	Absolute Translation Error RMSE
	Without EO		With EO
	Avg [m]	Std [m]	Avg [m]	Std [m]
jackal_hybrid_0	3.25	0.03	3.21	0.04
jackal_hybrid_1	4.1	0.35	3.73	0.39
jackal_hybrid_2	-	-	8.3	1.09
jackal_hybrid_3	9.67	0.67	11.8	7.02
jackal_indoor_0	9.17	1.64	11.4	1.19
jackal_indoor_1	3.98	2.0	3.98	0.83
jackal_indoor_2	8.67	3.24	6.97	2.05
jackal_indoor_3	7.06	0.88	6.04	0.81
jackal_outdoor_0	10.6	0.85	10.6	1.99
jackal_outdoor_1	15.9	1.02	12.3	1.35
jackal_outdoor_2	17.0	3.99	21.3	2.09

Table 1: VIO accuracy with and without external (wheel) odometry. Datasets come from the KimeraMulti [^4] [^5] [^23] project, and were collected with Jackal robots. The best result for each dataset is highlighted in green. A dash is used to denote that Kimera failed to get a reasonable trajectory for that dataset in the given configuration.

### 4.3 特征分箱我们对KimeraMulti中的A1数据集以及CarSim数据集进行了特征分箱处理；在其他数据集中，由于无需掩码，该操作并无效果。对于A1数据集而言，分箱掩码旨在去除机器人本体上的特征，这些特征会出现在摄像头图像的底部。对于CarSim数据集，为了提升性能，我们屏蔽了通常与天空相关的特征（即画面中央及上方的区域）。表2展示了这项消融实验的结果。在A1数据集中，若没有分箱掩码，Kimera系统将完全无法运行，这一现象在A1记录的其他数据集中也同样存在。对于CarSim数据集，启用分箱处理后，定位误差整体有所改善。对于存在已知不良特征区域的应用场景，这种方法似乎是降低定位误差的有效解决方案。

	Absolute Translation Error RMSE
Dataset	No Binning		Binning
Dataset	Avg [m]	Std [m]	Avg [m]	Std [m]
carsim_1	1.22	0.5	0.65	0.03
carsim_2	0.82	0.43	0.51	0.01
carsim_3	3.67	0.76	2.55	0.52
carsim_4	8.52	3.5	3.22	0.26
a1_simmons_0	-	-	1.74	0.33

Table 2: VIO accuracy with and without feature binning. A dash is used to denote that Kimera failed to get a reasonable trajectory for that dataset in the given configuration.

### 4.4 关键帧逻辑在表3中，我们对Kimera-VIO前端中的max_disparity_since_lkf参数开展了消融实验。该参数决定了选取新关键帧所依据的阈值——也就是特征光流平均范数的阈值。 --- 该数值越高，意味着在选取关键帧并触发后端因子图优化之前，特征在画面中可移动的范围就越大。当该值设为1000时，该系统实际上会被禁用，转而采用旧版Kimera基于时间的逻辑。uHumans2和CarSim序列未被纳入实验，因为在这些序列中机器人大多处于匀速运动状态，该参数几乎不起作用。我们发现，max_disparity_since_lkf的最佳取值通常偏小，这一结果印证了光流差异是识别关键帧的更优方法。在某些情况下，各数据集的最佳结果与最差结果之间的差异相当显著（相差一个数量级）。总体而言，将max_disparity_since_lkf的取值设定在75到100之间，似乎能获得稳定良好的结果。

Dataset	Absolute Translation Error RMSE
	MDSL 25		MDSL 50		MDSL 75		MDSL 100		MDSL 150		MDSL 1000
	Avg [m]	Std [m]	Avg [m]	Std [m]	Avg [m]	Std [m]	Avg [m]	Std [m]	Avg [m]	Std [m]	Avg [m]	Std [m]
jackal_hybrid_0	3.25	0.12	5.57	2.14	3.74	0.81	3.31	0.04	3.29	0.15	3.27	0.09
jackal_hybrid_1	3.25	0.36	2.78	1.48	3.56	0.12	3.41	0.54	3.56	0.66	3.71	0.35
jackal_hybrid_2	11.7	0.43	-	-	9.58	2.62	11.6	1.59	17.1	4.65	-	-
jackal_hybrid_3	70.6	3.62	54.3	25.2	37.7	42.6	9.79	3.27	26.3	16.2	-	-
jackal_indoor_0	1.53	2.63	7.68	0.61	7.99	0.81	7.2	1.76	6.32	1.71	8.56	1.8
jackal_indoor_1	2.73	0.49	2.83	0.52	4.59	1.89	2.67	0.31	3.21	0.69	2.91	0.06
jackal_indoor_2	13.9	8.1	-	-	9.08	1.76	8.58	3.11	9.07	3.05	6.85	1.15
jackal_indoor_3	5.2	2.56	6.36	2.27	4.46	0.24	6.17	2.23	7.38	3.45	4.64	1.38
jackal_outdoor_0	4.64	1.38	5.5	0.68	5.04	0.41	4.23	0.61	6.94	3.65	5.33	0.51
jackal_outdoor_1	9.78	1.36	12.5	1.56	9.35	2.36	12.4	0.61	13.2	1.05	12.4	1.68
jackal_outdoor_2	10.7	2.03	16.2	3.17	15.5	2.48	15.7	0.18	11.9	1.21	16.4	2.95
a1_simmons_0	1.13	2.21	1.66	2.33	0.92	0.64	1.14	4.1	0.99	0.16	1.19	2.10

Table 3: VIO accuracy ablation study on keyframe logic for Jackal and A1 datasets. Dashes are used to denote tracking failures (very high error).

### 4.5 GNC与PCM对比我们将GNC与基于成对一致性最大化（PCM）的基线方法[^19]进行对比，以评估其在鲁棒位姿图优化方面的表现。在所有数据集中，PCM的旋转阈值设为0.01，平移阈值设为0.05。所测试的数据集均存在多个闭环候选点。表4显示，在大多数情况下GNC能显著提升定位性能。实验中包含了KimeraMulti和uHumans2序列。对于A1机器人（a1_simmons_0）而言，唯有借助GNC才能让Kimera正常运行。 ### 4.6 PGMO 我们在[^1]中展示了Kimera-PGMO对网格重建的影响。研究发现，通过联合优化包含闭环的位姿图与网格，我们能够在稠密度量语义网格上闭合闭环，从而获得精度更高的网格。表5对比了Kimera-Semantics与Kimera-PGMO，其中Kimera-Semantics是[^3]中发布的原始版本。由于引入了闭环信息，Kimera-PGMO的网格重建精度更高。 ---

Dataset	Absolute Translation Error RMSE
	PCM		GNC
	Avg [m]	Std [m]	Avg [m]	Std [m]
jackal_hybrid_0	3.28	0.08	3.23	0.03
jackal_hybrid_1	3.79	0.16	3.91	0.33
jackal_hybrid_2	14.6	5.34	12.3	3.78
jackal_hybrid_3	14.7	3.39	11.2	6.62
jackal_indoor_0	9.84	3.03	8.02	1.19
jackal_indoor_1	3.41	0.9	3.99	1.11
jackal_indoor_2	6.39	0.96	6.13	0.73
jackal_indoor_3	7.55	3.28	6.33	2.53
jackal_outdoor_0	12.4	1.75	11.2	0.69
jackal_outdoor_1	13.8	6.52	16.2	0.83
jackal_outdoor_2	16.4	4.06	14.0	4.14
a1_simmons_0	-	-	28.5	2.43
uH2_apartment	0.1	0.6	0.1	0.0
uH2_suburb	-	-	2.47	0.67
uH2_office	0.33	0.4	0.33	0.09
uH2_subway	4.97	0.91	4.11	1.43

表4：在真实数据与模拟数据上，采用PCM和GNC进行闭环外点剔除时的VI-SLAM精度。此前我们的研究[^1]已在uHumans2数据集上完成了相关评估，不过我们在此基于新数据（CarSim和Kimera-Multi数据集）重复了该实验。

Dataset	3D Mesh Geometric Reconstruction RMSE
	Kimera-Semantics		Kimera-PGMO
	Avg [m]	Std [m]	Avg [m]	Std [m]
carsim_1	0.26	0.05	0.22	0.06
carsim_2	0.40	0.1	0.37	0.05
carsim_3	0.32	0.11	0.29	0.07
carsim_4	0.35	0.09	0.34	0.12
jackal_hybrid_0	0.76	0.01	0.67	0.01
jackal_hybrid_1	1.59	0.16	1.42	0.09
jackal_hybrid_2	1.61	0.21	1.61	0.21
jackal_hybrid_3	2.49	0.9	2.48	0.89
jackal_indoor_0	4.73	0.16	4.56	0.19
jackal_indoor_1	2.97	0.25	3.08	0.26
jackal_indoor_2	4.81	0.39	4.15	0.65
jackal_indoor_3	3.34	0.15	3.34	0.13
jackal_outdoor_0	2.51	0.26	2.49	0.25
jackal_outdoor_1	2.5	0.15	2.49	0.15
jackal_outdoor_2	2.19	0.05	2.18	0.06

表5：有无PGMO时的稠密几何地图精度（ATE RMSE）。如需了解更多关于PGMO的结果，感兴趣的读者可参考[^1]。 ### 4.7 竞争对手对比自Kimera首次发布以来，其他VI-SLAM算法流程也相继更新以提升其性能与能力。例如，Vins-Fusion [^7]就是 --- 广受欢迎的Vins-Mono [^25]的后续版本。同样，ORB-SLAM3 [^8]相较于成功的ORB-SLAM2 [^26]也有诸多改进。在本节中，我们将在多个平台上对比Kimera的性能与这些经过最新改进的算法流程。关于与其他开源算法流程的更多对比（如[^9]、[^27]、[^28]），读者可参考[^1]和[^2]。由于ORB-SLAM3仅为SLAM算法流程，我们仅对比开启回环检测功能的Kimera与ORB-SLAM3。由于Vins-Fusion无法支持RGB-D VIO，因此我们省略了使用D455相机的A1数据集的相关结果，该情况在表格中用圆点（◑）标示。此外，由于CarSim数据集中没有双目相机，我们展示了Kimera-VIO在单目模式下的结果，并省略了Vins-Fusion在双目模式下的结果。表6对比了开启回环检测功能前的Kimera-VIO与Vins-Fusion [^7]。在Jackal数据集中，Kimera采用外部里程计进行评估；在CarSim数据集中则采用单目模式评估。对于A1数据集，Kimera则使用RGB-D前端进行评估。总体而言，在大多数情况下Kimera的性能都优于Vins-Fusion，而Vins-Fusion在多个数据集中出现了失效情况（以短横线表示）。唯一的例外是uHumans2数据集，在该数据集中双目模式的Vins-Fusion性能优势显著。uHumans2智能体不会产生会干扰IMU数据的动态变化，这与CarSim数据集不同——CarSim数据集模拟了车辆动态，存在频繁的加速和制动操作。CarSim和uHumans2是在同一仿真环境中开发的，因此这种差异很可能是由场景类型和智能体动态特性导致的。表7对比了开启回环检测功能后的Kimera、Vins-Fusion和ORB-SLAM3。Vins-Fusion和ORB-SLAM3分别在单目及双目/RGB-D模式下进行评估。需要注意的是，由于ORB-SLAM3支持RGB-D惯性VI-SLAM，我们对A1数据集采用了该配置，但ORB-SLAM3在RGB-D或双目模式下均无法保持稳定的跟踪效果。在大多数情况下，Kimera的性能都优于竞争对手；Vins-Fusion在uHumans2数据集上的轨迹误差最小，而ORB-SLAM3在EuRoC数据集上表现较好。在EuRoC评估中，ORB-SLAM3在大多数情况下性能优于Kimera和Vins-Fusion，其中Vins-Fusion的性能也优于Kimera。ORB-SLAM3会频繁进行光束法平差优化，这无疑使其在EuRoC这类较小数据集上的状态估计效果更好。不过，在CarSim或Jackal数据集这类较长的数据集上，这种优势就会减弱。 ## 5 结论在本文中，我们介绍了自2019年首次发布以来对Kimera所做的几项关键改进。具体而言，我们讨论了针对Kimera-VIO前端的修改，以支持更多传感器模式（如单目、双目、RGB-D）、可选的外部里程计数据源、图像特征分箱以及更新的关键帧选择逻辑。我们还讨论了后端的变更，将GNC作为离群值剔除方法纳入其中，以实现更稳健的位姿图优化。我们针对多种数据集进行了大量的消融实验，以评估这些改进对定位误差的影响。此外，我们还展示了Kimera-Semantics在密集3D网格重建方面的改进，并对多个数据集上的Kimera-PGMO性能进行了评估。最后，我们将Kimera与其他开源视觉惯性SLAM算法进行了对比，结果显示其性能表现优异，尤其是在大规模数据集中。 ---

Dataset	VIO Absolute Translation Error RMSE (No Loop Closures)
	Kimera-VIO		Vins-Fusion Mono		Vins-Fusion Stereo
	Avg [m]	Std [m]	Avg [m]	Std [m]	Avg [m]	Std [m]
carsim_1	M 0.65	0.03	-	-	●	●
carsim_2	M 1.82	0.09	-	-	●	●
carsim_3	M 2.81	0.32	-	-	●	●
carsim_4	M 3.24	0.22	-	-	●	●
jackal_hybrid_0	S 3.21	0.03	5.6	0.43	3.75	0.04
jackal_hybrid_1	S 3.73	0.48	9.71	1.1	13.1	15.0
jackal_hybrid_2	S 8.3	1.09	10.57	2.76	-	-
jackal_hybrid_3	S 11.8	7.02	41.42	50.36	47.8	68.7
jackal_indoor_0	S 11.5	1.19	6.96	1.7	7.7	1.67
jackal_indoor_1	S 4.48	0.16	12.0	7.17	3.75	1.69
jackal_indoor_2	S 10.7	2.45	-	-	-	-
jackal_indoor_3	S 6.05	0.81	-	-	-	-
jackal_outdoor_0	S 10.6	2.0	-	-	-	-
jackal_outdoor_1	S 12.3	1.35	-	-	21.6	10.9
jackal_outdoor_2	S 21.3	2.09	-	-	-	-
a1_simmons_0	D 0.92	0.64	-	-	●	●
uH2_apartment	S 0.11	0.0	0.03	0.1	0.01	0.01
uH2_suburb	S 2.25	0.13	1.51	0.07	0.23	0.03
uH2_office	S 0.34	0.4	0.23	0.03	0.05	0.01
uH2_subway	S 4.11	2.78	0.28	0.01	0.16	0.01
MH_01	S 0.10	0.03	0.18	0.0	0.26	0.0
MH_02	S 0.10	0.01	0.05	0.06	0.2	0.0
MH_03	S 0.16	0.01	-	-	0.3	0.0
MH_04	S 0.21	3.2	0.2	0.0	0.42	8.41
MH_05	S 0.15	0.02	0.3	0.01	0.31	0.0
V1_01	S 0.05	0.02	0.06	0.03	0.11	6.57
V1_02	S 0.04	0.02	0.27	0.01	0.1	0.00
V1_03	S 0.10	0.02	0.17	0.01	0.09	0.02
V2_01	S 0.06	0.09	0.09	0.1	0.14	3.95
V2_02	S 0.07	0.34	-	-	0.12	0.0
V2_03	S 0.19	0.0	0.16	0.01	0.33	0.0

表6：Kimera与Vins-Fusion的视觉惯性定位精度对比。无法保持跟踪的数据集用破折号标示。圆点（●）表示该算法无法在该数据集上运行（例如不支持RGB-D），或者该数据集不包含相关传感器（例如CarSim没有双目摄像头）。Kimera所使用的传感器模式标注在第一列数据的左侧。S表示双目，M表示单目，D表示RGB-D。 ## 参考文献 [^1]: A. Rosinol, A. Violette, M. Abate, N. Hughes, Y. Chang, J. Shi, A. Gupta, and L. Carlone, "Kimera: from SLAM to spatial perception with 3D dynamic scene graphs," Intl. J. of Robotics Research, vol. 40, no. 12–14, pp. 1510–1546, 2021, arXiv preprint: 2101.06894, (pdf). [^2]: M. Abate, A. Schwartz, X. Wong, W. Luo, R. Littman, M. Klinger, L. Kuhnert, D. Blue, and L. Carlone, “Multi-camera visual-inertial simultaneous localization and mapping for autonomous valet parking,” in Intl. Sym. on Experimental Robotics (ISER), 2023, (pdf), (video). [^3]: A. Rosinol, M. Abate, Y. Chang, and L. Carlone, “Kimera: an open-source library for real-time metric-semantic localization and mapping,” in IEEE Intl. Conf. on Robotics and Automation (ICRA), 2020, arXiv preprint: 1910.02490, (video), (code), (pdf). [^4]: Y. Chang, Y. Tian, J. How, and L. Carlone, “Kimera-Multi: a system for distributed multi-robot metric-semantic simultaneous localization and mapping,” in IEEE Intl. Conf. on Robotics and Automation (ICRA), 2021, arXiv preprint: 2011.04087, (pdf). ## [^5]: Y. Tian, Y. Chang, F. H. Arias, C. Nieto-Granda, J. How, and L. Carlone, "Kimera-Multi: Robust, distributed, dense metric-semantic slam for multi-robot systems," IEEE Trans. Robotics, 2022, accepted, arXiv preprint: 2106.14386, (pdf) ---

Kimera2: Enhancing Kimera

Dataset	VI-SLAM Absolute Translation Error RMSE (With Loop Closures)
	Kimera		Vins	Mono	Vins	Stereo	ORB3	Mono	ORB3	S/D
	Avg [m]	Std [m]	Avg [m]	Std [m]	Avg [m]	Std [m]	Avg [m]	Std [m]	Avg [m]	Std [m]
carsim_2	M 0.51	0.01	-	-	●	●	22.6	4.23	●	●
carsim_3	M 2.55	0.52	-	-	●	●	61.2	20.5	●	●
carsim_4	M 3.22	0.26	-	-	●	●	15.3	6.33	●	●
jackal_hybrid_0	S 3.21	0.04	5.12	1.37	3.85	0.05	25.0	21.6	-	-
jackal_hybrid_1	S 3.73	0.39	6.15	5.39	10.1	14.8	44.2	9.08	-	-
jackal_hybrid_2	S 8.30	1.09	4.47	1.39	-	-	-	-	-	-
jackal_hybrid_3	S 11.8	7.02	42.4	52.4	48.1	69.3	-	-	-	-
jackal_indoor_0	S 11.4	1.19	6.14	0.91	8.01	1.89	-	-	11.9	5.03
jackal_indoor_1	S 3.98	0.83	11.8	8.08	3.54	1.61	16.5	6.07	5.86	5.64
jackal_indoor_2	S 6.97	2.05	-	-	-	-	72.4	0.31	15.2	5.49
jackal_indoor_3	S 6.04	0.81	-	-	-	-	-	-	23.3	14.9
jackal_outdoor_0	S 10.6	1.99	-	-	20.3	17.3	-	-	19.8	24.8
jackal_outdoor_1	S 12.3	1.35	-	-	4.53	3.46	88.9	10.8	15.7	9.38
jackal_outdoor_2	S 21.3	2.09	5.94	2.97	25.3	10.8	-	-	18.1	17.3
a1_simmons_0	D 0.92	0.64	-	-	●	●	-	-	-	-
uH2_apartment	S 0.06	0.0	0.02	0.01	0.01	0.0	0.12	0.53	0.02	0.42
uH2_suburb	S 0.69	0.13	0.61	0.4	0.14	0.02	32.6	0.4	-	-
uH2_office	S 0.11	0.18	0.02	0.01	0.02	0.0	0.69	0.4	0.04	0.05
uH2_subway	S 0.41	2.78	0.02	0.04	0.01	0.0	0.72	0.32	0.16	0.21
MH_01	S 0.09	0.01	0.07	0.01	0.17	0.0	0.05	0.0	0.09	0.1
MH_02	S 0.11	0.05	0.02	0.02	0.15	0.01	0.06	0.0	0.05	0.0
MH_03	S 0.12	0.01	0.04	0.05	0.14	0.0	0.09	0.02	0.06	0.0
MH_04	S 0.15	3.2	0.12	0.01	0.27	0.0	0.08	0.02	0.09	0.02
MH_05	S 0.15	0.02	0.13	0.0	0.3	0.02	0.12	0.01	0.08	0.02
V1_01	S 0.06	0.5	0.04	0.0	0.08	0.0	0.06	0.0	0.12	0.01
V1_02	S 0.04	0.02	0.17	0.0	0.07	0.0	0.12	0.0	0.06	0.0
V1_03	S 0.10	0.02	0.1	0.01	0.1	0.02	0.08	0.0	0.12	0.0
V2_01	S 0.05	0.09	0.08	0.0	0.11	0.04	0.05	0.01	0.08	0.0
V2_02	S 0.06	0.34	-	-	0.09	0.0	0.09	0.01	0.05	0.01
V2_03	S 0.19	0.0	0.09	0.0	0.1	0.02	0.14	0.03	0.09	0.01

Table 7: VI-SLAM localization accuracy for Kimera compared to Vins-Fusion and ORB-SLAM3. Loop closures are included for all pipelines represented here. Datasets that failed to maintain tracking are noted with dashes. A dot (●) denotes that either the pipeline was unable to run on that dataset (e.g., no support for RGB-D) or the dataset did not contain relevant sensors (e.g., CarSim does not have stereo cameras). The modality used by Kimera is denoted to the left of the first data column. S = stereo, M = monocular, D = RGB-D.

[^6]: N. Hughes, Y. Chang, and L. Carlone, “Hydra: a real-time spatial perception engine for 3D scene graph construction and optimization,” in Robotics: Science and Systems (RSS), 2022, (pdf). [^7]: T. Qin, J. Pan, S. Cao, and S. Shen, "A general optimization-based framework for local odometry estimation with multiple sensors," arXiv preprint: 1901.03638, 2019. [^8]: C. Campos, R. Elvira, J. J. G. Rodríguez, J. M. Montiel, and J. D. Tardós, “ORB-SLAM3: An accurate open-source library for visual, visual-inertial, and multimap SLAM,” IEEE Trans. Robotics, 2021. [^9]: P. Geneva, K. Eckenhoff, W. Lee, Y. Yang, and G. Huang, “OpenVINS: A research platform for visual-inertial estimation,” in IEEE Intl. Conf. on Robotics and Automation (ICRA), 2020, pp. 4666–4672. [^10]: N. Hughes, Y. Chang, S. Hu, R. Talak, R. Abdulhai, J. Strader, and L. Carlone, “Foundations of spatial perception for robotics: Hierarchical representations and real-time systems,” arXiv preprint: 2305.07154, 2023, (pdf), (video). ## [^11]: K. M. Jatavallabhula, A. Kuwajerwala, Q. Gu, M. Omama, T. Chen, S. Li, G. Iyer, S. Saryazdi, N. Keetha, A. Tewari, J. Tenenbaum, C. M. de Melo, M. Krishna, L. Paull, F. Shkurti, and A. Torralba, “ConceptFusion: Open-set multimodal 3D mapping,” arXiv:2302.07241, 2023 --- [^12]: Q. Gu, A. Kuwajerwala, S. Morin, K. Jatavallabhula, B. Sen, A. Agarwal, C. Rivera, W. Paul, K. Ellis, R. Chellappa, C. Gan, C. de Melo, J. Tenenbaum, A. Torralba, F. Shkurti, and L. Paull, “Conceptgraphs: Open-vocabulary 3d scene graphs for perception and planning,” arXiv, 2023. [^13]: A. Tourani, H. Bavle, J. L. Sanchez-Lopez, D. I. Avsar, R. M. Salinas, and H. Voos, "Vision-based situational graphs generating optimizable 3d scene representations," 2023. [^14]: C. Kassab, M. Mattamala, L. Zhang, and M. Fallon, "Language-extended indoor slam (lexis): A versatile system for real-time visual scene understanding," 2023. [^15]: J. Kerr, C. Kim, K. Goldberg, A. Kanazawa, and M. Tancik, "LERF: Language embedded radiance fields," in iccv, 2023. [^16]: N. M. M. Shafiullah, C. Paxton, L. Pinto, S. Chintala, and A. Szlam, "Clip-fields: Weakly supervised semantic fields for robotic memory," 2023. 17. O. Bailo, F. Rameau, K. Joo, J. Park, O. Bogdan和I. Kweon，“针对空间关键点分布均匀情况下的高效自适应非极大值抑制算法”，《IEEE模式分析与机器智能汇刊》，第106卷，第53–60页，2018年。[在线] 可访问： [^18]: D. Gálvez-López and J. D. Tardós, “Bags of binary words for fast place recognition in image sequences,” IEEE Transactions on Robotics, vol. 28, no. 5, pp. 1188–1197, October 2012. [^19]: J. G. Mangelson, D. Dominic, R. M. Eustice, and R. Vasudevan, “Pairwise consistent measurement set maximization for robust multi-robot map merging,” in IEEE Intl. Conf. on Robotics and Automation (ICRA), 2018, pp. 2916–2923. [^20]: H. Yang, P. Antonante, V. Tzoumas, and L. Carlone, “Graduated non-convexity for robust spatial perception: From non-minimal solvers to global outlier rejection,” IEEE Robotics and Automation Letters (RA-L), vol. 5, no. 2, pp. 1127–1134, 2020, arXiv preprint:1909.08605 (with supplemental material), (pdf). [^21]: P. Antonante, V. Tzoumas, H. Yang, and L. Carlone, “Outlier-robust estimation: Hardness, minimally tuned algorithms, and applications,” IEEE Trans. Robotics, vol. 38, no. 1, pp. 281–301, 2021, (pdf). [^22]: M. Burri, J. Nikolic, P. Gohl, T. Schneider, J. Rehder, S. Omari, M. Achtelik, and R. Siegwart, “The EuRoC micro aerial vehicle datasets,” Intl. J. of Robotics Research, 2016. [^23]: Y. Tian, Y. Chang, L. Quang, A. Schang, C. Nieto-Granda, J. How, and L. Carlone, "Resilient and distributed multi-robot visual SLAM: Datasets, experiments, and lessons learned," in IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems (IROS), 2023, (pdf), (video), (code), (web). 24. A. Rosinol, A. Gupta, M. Abate, J. Shi和L. Carlone，“3D动态场景图：包含地点、物体和人类的可操作空间感知”，发表于2020年机器人科学与系统会议（RSS），(pdf)，(media)，(video)。[在线] 可访问： [^25]: T. Qin, P. Li, and S. Shen, “Vins-mono: A robust and versatile monocular visual-inertial state estimator,” IEEE Transactions on Robotics, vol. 34, no. 4, pp. 1004–1020, 2018. [^26]: R. Mur-Artal and J. D. Tardós, “ORB-SLAM2: an open-source SLAM system for monocular, stereo and RGB-D cameras,” IEEE Trans. Robotics, vol. 33, no. 5, pp. 1255–1262, 2017. [^27]: S. Leutenegger, P. Furgale, V. Rabaud, M. Chli, K. Konolige, and R. Siegwart, "Keyframe-based visual-inertial slam using nonlinear optimization," in Robotics: Science and Systems (RSS), 2013. [^28]: M. Bloesch, S. Omari, M. Hutter, and R. Siegwart, "Robust visual inertial odometry using a direct EKF-based approach," in IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems (IROS). IEEE, 2015.