多传感器融合SLAM|maplab2.0：一个模块化和多模态建图框架（上）

• ​论文链接：​​https://arxiv.org/pdf/2212.00654.pdf​​
• GitHub链接：https://github.com/ethz-asl/maplab

摘要

将多种传感器模态和深度学习集成到同时定位和建图（SLAM）系统中是当前研究中非常重要的领域。多模态是面向具有挑战性环境中实现鲁棒性和配备不同传感器装置的异构多机器人系统互操作性的一块基石。通过maplab2.0，我们提供一个多功能的开源平台，它有助于开发、测试并且集成新的模块和功能到一个完全成熟的SLAM系统中。通过广泛的实验，结果表明maplab2.0的精度在HILTI 2021基准上与最先进的系统相媲美。此外，本文通过三个使用案例展示系统的灵活性：i）大规模（~10km）多机器人多段场景（23个任务）建图；ii）集成非视觉路标；iii）将基于语义对象的回环模块合并到建图框架中。

代码已开源：https://github.com/ethz-asl/maplab。

介绍

同时定位和建图（SLAM）是各种机器人应用的重要组件，例如自动驾驶、移动操作和增强/混合现实。在这些应用中，机器人平台需要知道周围环境和它的位置以执行给定的任务，例如自动地驾驶到特定目的地或者拾取和运送物体。更进一步是执行长期建图的能力，这通常需要工具来处理和融合多个地图，从而实现更广泛的多种应用和任务。

近年来，针对特定环境或者传感器配置已经成功地开发了很多专门的SLAM解决方案。然而，很多挑战仍然存在，直到SLAM被完全解决或者在普遍的操作条件下都能被通用地部署。最近融合多模态的研究工作已经取得显著的进展，因为多模态系统能够补偿单个传感器或者方法的缺陷。因此，在退化环境甚至全部或者部分传感器失效的情况下能够实现更为鲁棒的机器人操作。结合很多不同传感器的工作已经实现了非凡的性能。然而，与其它的开源SLAM框架相比，这些系统是紧耦合集成的。更具体而言，它们仅在特定传感器配置下运行，并且基础的模块（例如里程计、定位或者特征提取）是高度耦合的。因此修改这些模块或者包含新的功能需要大量的工程工作，这增加了科学研究和新产品开发的主要经费。因此，能够无缝集成各种传感器配置并且利用多传感器模态的多功能系统是值得的。多模态的灵活支持也是异构多机器人系统的基石，其中不同的机器人能够装配不同的传感器组合（例如由于平台约束）。

maplab2.0为多段场景、多机器人和多功能多模态建图提供一个开源平台。原始​​maplab​​是专门用于创建和管理视觉-惯性地图的开源工具箱。借助maplab2.0，我们通过集成多种新模态（例如激光雷达、GPS接收器、车轮编码器、语义对象等）将原始的框架扩展至远远超过其初始范围。这些示例为容易扩展到进一步的传感器模态提供了模板。maplab2.0还提供了容易集成外部组件的接口，例如添加任意数量的不同视觉特征或者回环约束。这些特性使得我们的新平台非常适合作为一个开发和研究工具用于基于深度学习的关键点检测器和回环引擎，迄今为止，它们大都是单独测试的。此外，由于新的子建图功能，在线协作SLAM现在能够在maplab2.0中实现，从而能够从多个来源实现全局地图的在线构建、优化和协同定位。这能够由我们对一个新的集中式服务器节点的实现来完成，该服务器节点整合来自多个机器人的数据，并且能够将协作构建的地图发送回机器人以提高性能。本文在多个实验和数据集上展现我们系统的能力和性能，为非视觉关键点、基于深度学习的描述子集成和基于语义对象的回环引擎提供概念性实现验证。

本文的贡献被总结如下：

1）我们提供一种开源的、多模态和多机器人建图框架，与其它现有的方向相比，它允许集成和融合大量的不同数据。

2）一个在线协作建图系统，它利用子地图和中央服务器来编译和分发全局一致且特征丰富的地图。

3）为任意数量的自定义特征点、描述子和回环集成接口。我们在以3D激光雷达关键点和基于语义对象的回环为特点的实验中展示它们的灵活性。

相关工作

建图能够被定义为创建环境表示的挑战，并且在过去几十年中，已经涌现大量不同范围的解决方案，它们具有由新传感器和场景驱动的重大改变。多模态已经超越了标准的传感器融合（即视觉-惯性或者双目相机），其包含更复杂的组合，例如激光雷达和语义信息。另一个值得注意的主题是多机器人建图，其中多个机器人同时探索环境并且其目标为创建一个全局一致的地图。多机器人建图与多段场景建图不同，多段场景建图涉及在不同时间间隔采集相同位置的测量数据，并且允许地图段之间的离线操作。尽管能够通过顺序处理数据记录以多段的方式使用多机器人框架，但是这是低效的，因为由于缺少地图管理工具导致任何时刻加入新数据记录时，都需要重新处理所有的先前数据。重要的SLAM框架和它们特点的比较如表格I所示。

maplab的首个版本是为视觉-惯性系统设计的多段建图框架。其它相媲美的框架为ORB-SLAM3和RTAB-Map。ORB-SLAM3是其前身ORB-SLAM2的扩展，它添加对于IMU和多段建图功能的支持。RTAB-Map集成来自激光雷达或者RGB-D相机的视觉和深度测量数据。RTAB-Map的扩展版本支持不同手工视觉特征和SuperPoint，但是不允许轻易地集成其它的描述子。这两种框架提供与maplab相似的地图创建和管理功能，其在建图过程中加入在线回环和优化。所有之前提供的三种框架都是针对特定传感器配置设计的紧耦合集成系统。相反，本文系统maplab2.0允许容易地集成不同的传感器配置、视觉特征并且支持任意的里程计输入，这促进其在异构机器人上的使用，并且提供一种新级别的灵活性。

Kimera是一个多模态建图框架，它提供带有语义标注的局部和全局三维网格以及基于视觉-惯性SLAM估计的全局轨迹。与maplab2.0不同，Kimera不具有多段建图功能，并且带有语义标注的三维重建不用于提高SLAM估计的精度。一般而言，语义信息通过成为高级场景理解的催化剂，具有显著改善建图的可能性。然而，之前利用语义信息建图的工作主要着重于生成改进的描述子，而不是在全语义SLAM系统中利用它们。在本工作中，我们提出利用图像描述子和简单的语义物体表示，这允许我们使用相对位姿误差进行优化。

Kimera-Multi是Kimera的一种直接的扩展，它使用完全分布式系统进行多机器人场景建图，但是没有对原始Kimera中纯视觉-惯性SLAM后端进行改进。本文方法使用一种集中式服务器取代它来采集子地图、优化它们并且创建一个全局一致的地图。COVINS也探索了一种类似的集中式设置。然而，COVINS受限于视觉-惯性使用案例，而maplab2.0能够包含多种传感器模态和配置。类似的方法还有LAMP2.0、CVI-SLAM和DOOR-SLAM，它们提供机器人间的协作建图，但是它们是紧耦合集成系统，受限于单传感器模态并且具有很小的灵活性。

尽管存在各种不同的SLAM框架，但是它们主要着重于特定传感器或者机器人-环境配置，并且对其中一种更改通常是困难或者不可能的。据我们所知，在所有现有的方法中，maplab2.0是最灵活的建图和定位框架，它不仅支持各种传感器，还能无缝地对接新需求。

maplab2.0框架

maplab2.0框架的通用结构如图2所示。整个框架被划分为三个主要的组件：建图节点、建图服务器和离线控制台交互界面。本文首先概述maplab2.0中底层地图结构，然后更详细地讨论这些主要的组件。

A.地图结构

本文将地图表示为一个或者多个任务的集合，其中每个任务是基于单个连续的建图段。地图的底层结构是一个由顶点和边组成的因子图，它包含不同任务间的所有机器人信息和测量数据。在特定时刻t处的机器人状态被参数化为一个顶点（6自由度（DoF）位姿、速度和IMU偏置）。路标也被表示为图中的顶点，它的状态被定义为三维位置。三维路标能够被用作环境中具有三维位置的任务事物的底层表示，例如视觉路标、三维路标或者甚至语义物体。

1）约束：顶点通过不同类型的边连接，这些边基于观测（例如关键点、IMU测量和回环）在状态变量上施加约束。IMU边包含连接顶点之间的预积分IMU测量，并且因此仅连接时间上顺序的顶点。相对位姿约束边在两个顶点之间施加一个刚体6自由度变换，并且被用于表示相对运动（即里程计）或者跨越较大时间间隔或任务的回环。边被赋予一个协方差来量化测量噪声，这通常被设置为一个预先定义好的常值。协方差能够被用于建模一个传感器所观测到的运动程度，例如车轮里程计对于沿z轴运动、俯仰角以及滚转角具有无限的协方差。我们认为回环边是相对位姿约束边的一种特殊情况。为了增加鲁棒性并且考虑异常值处理，回环边被作为可切换约束包含。然后，如果这些回环边与其它约束冲突过多，优化器则将这些边从图中删除。最后，边连接了路标和观测到它的位姿，并且基于估计和观测路标位置的差异来施加误差约束。

在优化过程中，约束能够被直接施加在被选择顶点的内部状态上。例如，绝对约束以给定的不确定性在顶点上强制施加一个全局三维位置，并且允许我们集成GPS测量或者绝对的基准标记观测。此外，固定特定的状态能够灵活地选择需要优化的问题部分。

2）路标：处于maplab核心的视觉建图模块仍然是maplab2.0的一部分，它包括基于ORB的特征检测以及来自BRISK或者FREAK的二进制描述子。连续帧间的特征对应关系基于描述子匹配来建立，其中为了鲁棒性，匹配窗口受到经过积分的陀螺仪测量数据的限制。然后，这些特征跟踪被三角化为三维路标。

全局定位和回环通过采用单个帧并且使用特征描述子建立一组2D-3D匹配来完成。然后，共视检查被应用于匹配对来滤除异常值。接着，通过RANSAC策略内的P3P算法，剩余的匹配对被用于获取相对于地图参考帧的变换。该变换作为回环边被添加到因子图中。我们还提供另一种方法，它通过合并共视路标并且最小化它们的重投影误差来包含回环。这种方法省去调整明确的回环边协方差的困难，但是在因子图中强制施加更软的约束。

在maplab2.0中，我们增加了在地图中同时包含任意数量不同类型特征的可能性。为了获取连续帧间特征跟踪，用户能够使用被包含的Lucas-Kanade跟踪器的通用实现，或者他们自己提供跟踪信息。此外，本文扩展了匹配引擎以支持浮点类型的描述子，从而使得回环能够使用最新开发的描述子。在maplab中，二进制描述子使用倒排多索引进行匹配，而浮点型描述子使用近似最近邻快速库（FLANN）进行匹配。接着，为了实现前面描述的回环，匹配对被类似地处理。出于调整目的，不同的特征类型能够用分离的参数集合来说明质量和行为中的差异。

maplab2.0还能够处理具有三维观测的路标。例如，这些可能来自RGB-D相机，其中视觉特征还具有关联的深度，或者来自直接从三维点云中检测到的特征。显著的差异为，这些路标的位置不是使用多视图几何进行三角化获得的，而是通过平均三维测量来获取的。类似地，位姿图误差项不是基于重投影误差，而是基于被观测到的三维位置和路标三维位置之间的欧式距离。另一个显著差异为回环被作为3D到3D的RANSAC匹配问题，而不需要P3P算法。

B.建图节点

建图节点在每个机器人上运行，并且使用外部的输入来源和原始的传感器数据以多模态因子图的形式创建地图。在地图构建过程中使用6自由度的里程计输入来初始化底层因子图的机器人位姿顶点。建图节点与里程计方法无关，并且具有简单的接口，因此使得它在各种机器人和传感器配置下易于使用。这与​​maplab​​形成对比，在maplab中仅有内置的视觉-惯性估计器ROVIOLI可用，而maplab2.0甚至不需要IMU。然而，如果IMU可用，则惯性约束被加入到地图中，并且状态估计器还能够可选地计算IMU偏置的初始估计值。接着，这些偏置估计值能够被用于改进全局地图优化问题的初始化，这有益于它的收敛速度和精度。如果IMU存在但是没有被状态估计器使用，则偏置估计能够单独完成。

对原始的maplab框架也进行实质地改变，以便于能够使用自定义的内部组件或者易于配置的外部接口来处理和集成其它的传感器模态。最值得注意之处为，maplab2.0能够在运行时包含任意数量的不同三维路标类型。此外，现在能够无缝地添加相对约束（例如里程计或者外部回环）和绝对的6自由度约束（例如GPS或者基准标记）。

原始相机图像或者激光雷达点云能够被作为资源附加到地图中，以供之后的模块在任何时候使用，例如，计算额外的回环或者检测物体。然后，带有所有被包含约束的生成地图被传给建图服务器用于在线处理或者存储，并且被加载用于之后控制台中的离线处理。

C.建图服务器

建图服务器是maplab2.0的新增加项，它支持协作和在线建图。该方法作为获胜团队（CERBERUS）的多机器人建图系统的一部分被成功地部署于DARPA地下挑战赛中。服务器节点能够在专用机器或者与建图节点并行的机器人之一上运行。建图节点以均匀的间隔将它们的地图划分成块，称为子地图。子地图被立刻发送给建图服务器，其中它们被预处理并且被连接到之前由相同机器人传送的对应子地图。当拆分地图时，通过将每个子地图的最后一个顶点复制到下一个子地图中来完成bookkeeping。这也避免了边和特征跟踪的不连续。并行地，服务器将来自不同机器人的地图连续地回环成一个全局一致的地图。值得注意的是，服务器和控制台共享相同的代码库，因此任何新功能能够容易地被集成到其中任何一个中。

1）子地图预处理：传入的子地图没有直接合并，而是首先单个处理以确保局部精度。具体而言，在每个机器人的子图上执行一组可配置的操作，其中包括局部地图优化（在所有传感器数据和约束上进行全部BA）、特征质量评估和内部地图回环（视觉和激光雷达依赖于可用的数据）。由于子地图被彼此独立地处理，所以建图服务器能够高效地同时处理多个地图。在完成预处理步骤之后，每个完成的子图被连接到来自相同机器人的先前子图上。

2）多机器人处理：建图服务器在全局多机器人地图上连续地运行，并且执行第二组可配置操作（回环、特征质量评估、BA、可视化、绝对约束异常值剔除等）。这里，回环算法（视觉或者激光雷达）尝试将所有不同的机器人放置到相同的参考系下，并且纠正漂移。与预处理步骤相反，在多机器人地图上的操作总是在全局范围内执行，例如通过机器人内和机器人间的方法检测回环并且在所有机器人地图上联合地完成全局优化。协作构建的全局地图还能够被传回到机器人以提高机器人载体上估计的精度。对于环境理解的提高不仅有益于定位精度，还有益于其它任务，例如全局路径规划。

D.离线控制台

离线控制台是从​​maplab​​中移植过来的，其中旧工具适配关于传感器和模态的新特性。存在工具进一步处理地图，例如批量优化、从不同段合并地图、异常值剔除、关键帧、地图稀疏化等。使用激光雷达进行回环目前也能够通过包含ICP和G-ICP实现的新模块成为可能，但是不限于这些，并且能够容易地进行扩展。由配准模块计算的变换被作为具有可切换约束的回环边进行添加。对于每种传感器和方法的组合，我们使用一种预先定义好的固定的协方差，它分别为每个平移和旋转分量设置。协方差的值基于传感器噪声和配准方法的精度进行经验性地选择。稠密重建也能够通过集成的Voxblox插件来完成。控制台另外提供工具用于资源管理（操作或者可视化附加的点云、图像和语义测量）或者导出地图数据（位姿、IMU偏置、路标等）。

最后，控制台通过插件实现容易的扩展，它能够离线运行代码，并且与地图构建过程无关。例如，我们使用插件来实现上述的激光雷达配准模块和语义回环模块。

文章转载自公众号：自动驾驶专栏