将多传感器模态和深度学习集成到同时定位和mapping（SLAM）系统中是当前研究的重要领域。多模态是在具有挑战性的环境中实现鲁棒性和具有不同传感器设置的异构多机器人系统的互操作性的一块垫脚石。借助maplab 2.0，这个多功能的开源平台，可帮助开发、测试新模块和功能，并将其集成到一个成熟的SLAM系统中。广泛的实验表明maplab 2.0的精度可与HILTI 2021基准测试的最新技术相媲美。此外，本文通过三个用例展示了系统的灵活性：
i）大规模(∼10公里）多机器人多会话（23个任务）mapping，ii）非视觉地标的集成，以及iii）将基于语义对象的循环闭合模块纳入mapping框架，代码链接：https://github.com/ethz-asl/maplab
另外，清华和轻舟智航提出了首篇端到端视觉语义定位算法BEV-Locator，感兴趣的小伙伴可以看下历史文章。
领域背景
SLAM是各种机器人应用的重要组件，如自动驾驶、移动操纵和增强/混合现实。在这些应用中，机器人平台需要了解周围环境及其位置以执行给定任务，无论是自动驾驶到特定目的地还是拾取和运送物体。One step further 是执行长期mapping的能力，这通常需要处理和合并多个mappping工具，从而实现更广泛的不同应用程序和任务。
近年来，针对特定环境或传感器配置成功开发了许多量身定制的SLAM解决方案。然而，在SLAM完全解决或在普遍的操作条件下普遍部署之前，仍存在许多挑战。由于多模态系统能够弥补单个传感器或方法中的弱点，最近在融合多模态方面的努力取得了重大进展。因此，即使在传感器完全或部分故障的情况下，也能在降级环境中实现更稳健的机器人操作。前面已经有结合了许多不同传感器的工作存在[3]–[5]，并取得了显著的性能。然而，与其它开源SLAM框架[8]-[11]一起，这些系统是紧密集成的。更具体地说，它们仅在特定传感器配置下运行，并且基本模块（例如里程计、定位或特征提取）高度纠缠。修改这些模块或加入新功能需要大量的工程工作，增加了科学研究和新产品开发的主要开销。因此，需要能够无缝集成各种传感器设置并利用多种传感器模态的多功能系统。多种模态的灵活支持也是异构多机器人系统的基石，其中不同的机器人可以配备不同的传感器组合（例如由于平台限制）。
maplab2.0为多会话、多机器人和多功能多模态mapping提供了一个开源平台。最初的maplab是一个开源工具箱，专门用于创建和管理视觉惯性地图。借助maplab 2.0，我们通过集成多种新模式（如LiDAR、GPS接收器、车轮编码器、语义对象等），将原始框架扩展到远远超出其初始范围。这些示例为进一步的传感模式提供了易于扩展的模板。maplab2.0还提供了方便集成外部组件的接口，例如添加任意数量的不同视觉特性或循环闭合约束。这些特性使我们的新平台非常适合作为深度学习关键点检测器和loop closure引擎的开发和研究工具，迄今为止，这些引擎大多是单独测试的。此外，由于新的submapping功能，在线协作SLAM现在可以在maplab 2.0中实现，从而可以在线构建、优化和共同定位来自多个来源的一个全局地图。这是通过实现一个新的集中式服务器节点实现的，该节点聚合来自多个机器人的数据，并可以将协作构建的地图发送回机器人以提高性能。在多个实验和数据集中展示了本文系统的功能和性能，为非视觉关键点、深度学习描述符集成和基于语义对象的循环闭包引擎提供了概念验证实现。
论文主要创新
论文主要贡献总结如下：
1.我们提供了一个开源、多模式和多机器人mapping框架，与其它现有方法相比，该框架允许集成和融合数量无与伦比的不同数据；
2.一个在线协作建图系统，利用submapping和中央服务器来编译和分发全局一致、功能丰富的地图；
3.集成任意数量的自定义特征点、描述符和循环loop的接口，在以3D LiDAR关键点和基于语义对象的循环loop为特征的实验中展示了灵活性；
相关paper
mapping可以被定义为创建环境表示的挑战，在过去几十年中，已经看到了大量多样的解决方案，新的传感器和场景推动了重大变化。多模态已经超越了标准传感器融合（即视觉惯性或立体摄像机），包括更复杂的组合，例如激光雷达和语义信息。另一个值得注意的主题是多机器人mapping，其中多个机器人同时探索一个环境，并旨在创建一个全局一致的地图。多机器人mapping不同于多会话mapping，多会话mapping涉及以不同的时间间隔收集同一地点的测量值，并启用会话之间的离线操作。虽然可以通过顺序处理数据记录以多会话方式使用多机器人框架，但这是低效的，因为由于缺少地图管理工具，因此每当添加新记录时，都需要重新处理所有以前的数据。表I列出了重要SLAM框架及其特点的比较。
maplab的第一个版本是为视觉惯性系统设计的多会话建图框架。其它可比较的框架是ORB-SLAM3和RTABMap。ORB-SLAM3增加了对IMU和多会话mapping功能的支持。RTAB地图集成了LiDAR或RGB-D相机的视觉和深度测量。RTAB地图的扩展支持多种手工视觉功能和SuperPoint，但不允许轻松集成其它描述符。这两个框架都提供了与maplab类似的地图创建和管理功能，并在mapping过程中添加了在线循环关闭和优化。上述三个框架都是针对特定传感器配置而设计的紧密集成系统。相反，本文框架允许轻松集成maplab的第一个版本：为视觉惯性系统设计的多会话mapping框架。
Kimera是一个多模态mapping框架，它提供了带有语义注释的局部和全局3D网格以及基于视觉惯性SLAM的全局轨迹估计。与maplab 2.0不同，Kimera不具备多会话功能，并且带有语义注释的3D重建不用于提高SLAM估计的准确性。一般来说，语义信息通过成为高级场景理解的催化剂，有可能显著改善建图。然而，先前利用语义进行mapping的工作[17]–[21]主要集中于生成改进的描述符，而不是在完全语义SLAM系统中利用它们。在这项工作中，本文提出利用图像描述符和简单的语义对象表示，这允许我们使用众所周知的相对姿态误差进行优化。Kimera Multi是Kimera的直接扩展，它支持使用完全分布式系统的多机器人场景，但没有改进原始Kimera中的纯视觉惯性SLAM后端。本文的方法使用一个集中的服务器来收集子地图，优化它们，并创建一个全局一致的地图。COVINS也探索了类似的集中设置，然而COVINS仅限于视觉惯性用例，而maplab 2.0可以结合多种传感器模态和配置。类似的还有LAMP 2.0、CVI-SLAM和DOOR-SLAM，它们提供了机器人之间的协作mapping，但是紧密集成的系统，仅限于一种传感器模态，灵活性很小！
尽管存在各种其它SLAM框架，但它们主要集中于特定的传感器或机器人环境配置，对其中任何一种进行更改通常都是困难的或不可能的。根据对所有现有方法的了解，maplab 2.0是最灵活的mapping和定位框架，它不仅支持各种传感器，还可以无缝地适应新的需求。
Maplab2.0框架
maplab 2.0框架的总体结构如图2所示，整个框架可以分为三个主要组件：，mapping节点、mapping服务器和离线控制台界面，本文首先概述maplab 2.0中的底层地图结构，之后将更详细地讨论主要组件。
Map结构
本文将地图表示为一个或多个任务的集合，其中每个任务都基于一个连续的绘图会话。地图的基本结构是由顶点和边组成的因子图，这些顶点和边结合了所有机器人信息和不同任务的测量结果。机器人在某个时间点t的状态被参数化为顶点（6自由度（DoF）姿态、速度、IMU偏差）。地标也表示为图形中的顶点，其状态定义为3D位置，3D界标可以用作环境中具有3D位置的任何事物的基础表示，例如视觉界标、3D界标，甚至语义对象。
1） 约束：顶点通过不同类型的边连接，这些边基于观察（例如，关键点、imu测量值和循环闭合）对其状态变量施加约束。IMU边包含连接顶点之间的预集成IMU测量，因此仅连接时间顺序的顶点。相对姿态约束边在两个顶点之间施加刚性的6DoF变换，并用于表示相对运动（即里程计）或跨越较大时间间隙或任务的loop closure。边缘被分配一个协方差来量化测量噪声，其通常被设置为一个预定义的常数值。协方差可用于建模传感器可观察到的运动程度，例如，车轮里程计对沿z轴的运动以及俯仰和侧倾具有无限协方差。我们认为循环闭合边是相对姿态约束边的一种特殊情况。为了提高鲁棒性并考虑异常值，可以将环路loop边缘作为可切换约束。如果边与其它约束冲突过多，优化器可以丢弃图中的边。最后，边缘将界标连接到观测到的姿态，并基于估计和观测到的界标位置之间的差异施加误差。在优化过程中，还可以直接对选定顶点的内部状态施加约束。例如，绝对约束以给定的不确定性强制顶点上的全局3D位置，并允许我们集成GPS测量值或绝对基准标记观测值。此外，修复某些状态可以灵活选择要优化的问题部分。
2） 地标：maplab核心的视觉建图模块仍然是maplab2.0的一部分。它包括基于ORB的特征检测，以及BRISK或FREAK的二进制描述符。基于描述符匹配建立连续帧之间的特征对应关系，其中为了鲁棒性，匹配窗口受到集成陀螺仪测量的限制。然后将这些要素轨迹三角化为三维地标，全局定位和loop closure是通过获取单个帧并使用特征描述符建立一组2D-3D匹配来完成的。随后将对匹配项进行共视性检查，以过滤异常值。利用RANSAC方案内的P3P算法，使用剩余匹配来获得关于地图的参考帧的变换。然后可以将该变换作为循环closure边添加到因子图中。本文还提供了一种替代方法，通过合并共视界标并最小化其重投影误差来合并循环closure。这种方法省去了调整显式循环closure边缘协方差的困难，但对因子图施加了更软的约束。
Mapping node
映射节点在每个机器人上运行，并使用外部输入源和原始传感器数据以多模态因子图的形式创建mapping。在地图构建期间使用6DoF里程计输入来初始化基础因子图的机器人姿态顶点。Mapping节点与里程计方法无关，具有简单的界面，因此可以在各种机器人和传感器设置中轻松使用。这与maplab形成对比，maplab只有内置视觉惯性估计器ROVIOLI可用，而maplab 2.0甚至不需要IMU。然而，如果IMU可用，则将惯性约束添加到地图中，并且状态估计器还可以可选地计算初始IMU偏差的估计。然后可以使用这些偏差估计来改进全局地图优化问题的初始化，这有利于其收敛速度和精度。如果IMU存在但未被状态估计器使用，则也可以单独进行偏差估计[30]–[32]。对原始maplab框架也进行了实质性更改，以便可以使用自定义内部组件或易于配置的外部接口处理和集成其他传感器模态。最值得注意的是，maplab 2.0可以在运行时合并任意数量的不同3D地标类型。此外，现在可以无缝地添加相对约束（例如里程计或外部环路闭合）和绝对6 DoF约束（例如GPS或基准标记）
原始相机图像或LiDAR点云可以作为资源附加到地图上，稍后的模块可以随时使用这些资源，例如，计算额外的loop closure或检测对象。然后，可以将包含所有约束的结果mapping传递到mapping服务器以进行联机处理，或者将其存储并加载，以便稍后在控制台中进行脱机处理！
Mapping Server
mapping服务器是maplab 2.0的新添加，支持协作和在线mapping。该方法作为获胜团队（CERBERUS）多机器人地图系统的一部分成功应用于DARPA地下挑战。服务器节点可以在专用机器或与mapping节点并行的机器人之一上运行。mapping节点以规则的间隔将其mapping划分为块，称为submapping。子地图立即被发送到mapping服务器，在那里它们被预处理并连接到来自同一机器人的相应的先前发送的子地图。拆分时，通过将每个子贴图的最后一个顶点复制到下一个子贴图中来进行记账，这也避免了边和特征轨迹中的不连续性。并行地，服务器连续循环将来自不同机器人的地图闭合为全局一致的地图。值得注意的是，服务器和控制台共享相同的代码库，因此任何新功能都可以轻松地集成到其中。
1） submap预处理：传入的子贴图不会直接合并，而是先单独处理，以确保局部精度。具体而言，在每个机器人的子地图上执行一组可配置的操作，其中包括局部地图优化（对所有传感器数据和约束进行全束调整）、特征质量评估和地图内循环闭合（可视和LiDAR取决于可用内容）。由于子地图彼此独立地处理，因此mapping服务器可以高效地同时处理多个地图。在完成预处理步骤后，每个完成的子地图都连接到来自同一机器人的前一个子地图
2） 多机器人处理：mapping服务器在全局多机器人地图上连续运行，并执行第二组可配置操作（循环闭合、特征质量评估、束调整、可视化、绝对约束异常值剔除等）。这里， loop closure算法（视觉或激光雷达）尝试将所有不同的机器人放置在同一参考系中，并纠正漂移。与预处理步骤相反，多机器人地图上的操作总是在全局范围内执行，例如，在机器人内和机器人间方法中检测 loop closure，并且在所有机器人地图上联合进行全局优化。协作构建的全局地图也可以传回机器人，以提高其机载估计的准确性。环境意识的提高不仅有助于定位精度，也有助于其他任务，如全局路径规划。
Offline Console
离线控制台是从maplab移植过来的，旧工具适应了传感器和模态的新功能。有一些工具可用于进一步处理mapping，例如批处理优化、合并不同会话的mapping、异常值拒绝、关键帧、mapping稀疏化[34]等。使用LiDAR的loop closure现在也可以通过包括ICP和G-ICP的实现的新模块实现，但不限于这些，并且可以容易地扩展。由配准模块计算的变换被添加为具有可切换约束的环路闭合边（参见第III-A节）。对于每个传感器和方法组合，使用预定义的固定协方差，该协方差分别为每个平移和旋转分量设置。基于传感器噪声和配准方法的精度根据经验选择这些值。也可以使用集成的Voxblox插件进行密集重建。控制台另外提供了用于资源管理（操纵或可视化所附的点云、图像和语义测量）或导出地图数据（姿态，IMU偏差、界标等）。最后，控制台通过插件实现了简单的扩展，这些插件可以离线运行代码，并且独立于地图构建过程。例如使用插件来实现上面提到的LiDAR配准模块和语义loop closure模块（参见第IV-D节）。
使用案例
论文进行了几次实验，以彻底评估提出的框架，并证明其易于使用和高度灵活性。具体来说，介绍了四个数据集的结果，以展示maplab 2.0的新特性和功能。首先在公共HILTI SLAM 2021数据集上验证了提出的框架的性能和准确性，并将其与最新方法进行了比较。接下来，将展示提出的框架在现实世界中的适用性，并展示大规模多机器人多会话功能。然后，通过结合从投影点云检测到的3D激光雷达特征，展示了landmarks系统的多功能性。最后，展示了自定义室内数据集上的语义loop closure模块，所有数据集都是通过硬件时间同步传感器设置收集的。
验证和比较
本文使用HILTI SLAM挑战2021数据集将提出的框架与最先进的方法进行比较。该数据集包括12个记录，涵盖室内办公环境和具有挑战性的室外建筑工地。在实验中，排除了三个太小且不存在有趣挑战的序列。其余九个序列中的三对（工地、地下室和校园）在同一环境中拍摄，可以共同定位以提高精度。对于maplab 2.0，可以使用数据集中提供的五个摄像头、ADIS IMU和OS0-64激光雷达。作者展示了maplab 2.0使用三种不同的里程计来源的三种用例：ROVIO、OKVIS和FASTLIO2。除了标准BRISK描述符，使用第III-A2节中的外部接口，还包括SuperGlue、跟踪的SuperPoint特征，以及LK跟踪的SIFT特征。为了减小地图大小并加快描述符搜索，使用主成分分析（PCA）将SuperPoint和SIFT特征从256个浮点压缩到32个。使用所有可用特征计算全局环路闭合，并合并匹配的界标。论文还使用LiDAR配准模块中的ICP（见第III-D节）对姿态进行局部改进。loop closure边缘的协方差是根据经验预定义的。Office Mitte序列的可视化如图3a-c所示，可以观察到，与ORB相比，SuperPoint功能如何更好地遵循建筑结构！
表II还显示了单独的里程计源和其他SLAM基线（LVI-SAM、ORB-SLAM3、RTAB Map和maplab）的性能。Maplab和Maplab 2.0是唯一能够使用所有五个相机进行loop closure的方法。对于ROVIO和OKVIS，只使用正面摄像头或stereo对进行里程测量。在所有使用视觉maplab 2.0的方法中，maplab的性能明显优于基线，FAST-LIO2仅使用LiDAR惯性，是最好的基线，甚至优于LVI-SAM，LVI-SAM是基于相同原理的LiDAR惯导融合愿景。然而，也可以采用性能最佳的方法作为里程计，并进一步改进结果，特别是在FAST-LIO2上显著改善停车顺序。作者还提出了ROVIO和SIFT的融合，展示了maplab 2.0的多功能性，可快速增量改进，独立于更好的深度学习视觉特征。所有方法的计时也在配备Intel i7-8700和Nvidia RTX 2080 GPU的机器上显示。
大规模多机器人多会话mapping
论文通过在瑞士的大型设施中部署提出的框架，证明了其对复杂现实场景的适用性。环境特点是类似城市的街道，有建筑物，环境恶劣，如坍塌的建筑物和狭窄的空间。在这项实验中，用一台手持设备记录了23次个人跑步，该手持设备配有五个摄像头和一台Ouster OS0-128，其中包含超过两个小时的数据，距离约10公里，以及多次室内-室外转换。每次跑步都使用OKVIS进行里程测量。前五张地图用于使用mapping服务器构建全局多机器人地图，其余的地图使用控制台中的多会话mapping进行合并。所有任务之间的一致性通过使用全局视觉回路闭合以及RTK GPS的附加绝对姿态约束（如果可用）来实现。此外，通过执行任务内和任务间激光雷达配准，对单个轨迹进行了改进。
图4显示了最终的多机器人地图，为了定量评估多机器人服务器，在公共EuRoC基准上进行了测试。由于控制台和服务器使用相同的底层mapping框架，排除了操作排序等次要细节，因此预期的准确性是相同的。使用mapping服务器、ROVIO和BRISK在一个多机器人实验中同时运行所有11个序列。然后，通过使用mapping节点和控制台顺序处理每个任务，然后将它们合并在一起，来重复实验。这两种场景的平均RMSE APE均为0.043 m，但并行化映射服务器只需3分钟27秒即可完成所有任务，而顺序多会话工作流只需35分钟56秒。对于这两种情况，计时包括里程计、优化和地图合并！
投影激光雷达图像的视觉跟踪
为了展示maplab 2.0中地标系统的灵活性，论文集成了3D激光雷达关键点。从Streiff等人的工作中获得灵感，[45]并将LiDAR点云投影到2D平面上。作者使用对数尺度归一化LiDAR范围和强度值，并使用Mertens融合合并两个通道[46]，使用相邻值绘制坏LiDAR返回的缺失像素。图5中显示了生成的2D投影的示例图像，以及来自相同视角的相机图像，显示了环境。然后，将LiDAR图像视为相机图像，并使用SuperPoint和SuperGlue获得点特征和轨迹，如图5所示。
由于对于每个特征观测，都有LiDAR的深度信息，因此可以更有效地初始化和循环闭合这些3D LiDAR地标，如第III-A2节所述。最后使用这些LiDAR关键点绘制出HILTI 2021数据集中的一个序列，并在图3d中可视化生成的地图。与视觉关键点相比，LiDAR标志更准确地映射到结构上，从墙壁的直线度可以看出。然而，它们也受到环境中缺失点或移动物体在LiDAR图像中产生的噪声引起的异常值的影响。
基于语义的mapping
本节展示了maplab 2.0的可扩展性和模块化设计，通过用语义信息扩充地图并说明其在现实世界场景中的潜在应用。最初，使用Mask R-CNN在图像中检测语义对象，对于每一次检测，使用NetVLAD提取掩码实例分割的描述符。代替内置跟踪器，所有检测到的目标都使用Deep SORT进行跟踪，Deep SORT使用外观项扩展了典型的空间数据关联度量，可以直接利用先前提取的对象描述符。与视觉地标类似，语义对象是maplab 2.0地图中的3D地标，但具有关联的类标签，可用于例如语义循环闭合检测。
最后，通过直接比较同一类的对象描述符来找到候选语义loop closure。首先，应用唯一的可见性过滤器，即在单个图像中观察到的两个地标不能匹配。在几何验证候选和聚类共可见地标之后，使用通过Horn方法获得的两个3D地标簇之间的相对坐标变换，在最接近两个匹配地标簇的两个机器人顶点之间构建6-DoF约束。最后，使用Manoj等人提出的方法计算相应因子图约束的协方差。本文使用RGB惯性传感器在办公室环境中收集了一个具有多个对象的室内数据集。论文观察了两次办公桌上的物体，同时留出了一些时间来积累漂移（见图6a和6c）。图6b显示了语义标志聚类和检测到的loop closure候选。在将语义对象的loop closure边缘添加到全因子图之后，漂移显著减少，图6c中可以看到改进的图！
参考
[1] maplab 2.0 – A Modular and Multi-Modal Mapping Framework.
原文首发微信公众号【自动驾驶之心】：一个专注自动驾驶与AI的社区（https://mp.weixin.qq.com/s/NK-0tfm_5KxmOfFHpK5mBA）
