RTAB-Map：用于大规模场景的开源激光雷达和视觉SLAM库

自2013年RTAB-Map作为开源库发布以来，RTAB-Map 最初是作为一种基于外观的回环闭合检测方法，并配有内存管理功能，以应对大规模和长期在线操作。随后它逐步发展为在各种机器人和移动平台上实现同时定位与建图（SLAM）,由于每个应用都有其自身在传感器、处理能力和移动方式方面的约束，这就引发了一个问题，即在成本、准确性、计算能力和集成便利性方面，哪种 SLAM 方法是最合适的。由于大多数 SLAM 方法要么基于视觉，要么基于激光雷达，因此很难进行比较。因此我们决定扩展 RTAB-Map 以支持视觉和激光雷达 SLAM，在一个包中提供了一个工具，使用户能够在不同的机器人和传感器上实现和比较各种 3D 和 2D 解决方案。本文介绍了这个扩展版本的 RTAB-Map，并展示了其在比较大量流行的现实世界数据集（如 KITTI、EuRoC、TUM RGB-D、PR2 机器人上的 MIT Stata Center）时的应用，从实际角度概述了视觉和激光雷达 SLAM 配置在自主导航应用中的优缺点。

RTAB-Map 逐渐发展成为一个跨平台独立的 C++ 库和一个 ROS 包，由以下实际需求驱动：

• 在线处理：SLAM 模块的输出应在接收传感器数据后保持最大延迟限制。对于基于图的 SLAM，随着地图的增长，需要更多的处理时间来检测回环闭合、优化图和组装地图。此外，与控制、导航、避障、用户交互、物体识别等其他处理模块的集成也可能限制可用于 SLAM 的 CPU 时间。因此，限制计算负载的可能性有助于避免其他模块的滞后问题，甚至可能是防止不安全情况的必要条件。

• 稳健且低漂移的里程计：尽管回环闭合检测可以校正大多数里程计漂移，但在现实场景中，机器人往往无法在地图上正确定位自己，要么是因为正在探索新区域，要么是环境中缺乏辨别特征。在此期间，应尽量减少里程计漂移，以便在定位之前仍能实现准确的自主导航，避免错误地覆盖已建图区域（例如，在房间入口处错误地添加障碍物，使其成为封闭区域）。使用相机和激光雷达等外感传感器估计里程计在环境中有足够特征时可能非常准确，但仅使用一种传感方式可能存在问题，并且如果环境中的特征不再可见，可能会导致定位失败。使用内感（例如，轮式编码器、惯性测量单元（IMU））和外感传感器的组合将提高里程计估计的稳健性。

• 稳健的定位：SLAM 方法必须能够识别何时重新访问过去的位置（用于回环闭合检测）以校正地图。动态环境、照明变化、几何变化甚至重复环境都可能导致定位错误或定位失败，因此该方法应对误报具有稳健性。

• 实用的地图生成和利用：大多数流行的导航方法基于占据网格，因此开发能够提供 3D 或 2D 占据网格的 SLAM 方法便于集成。此外，当环境大多是静态时，进行一次建图会话然后切换到定位更为实用，从而设置内存使用并节省地图管理时间。

• 多会话建图（即被绑架的机器人问题或初始状态问题）：启动时，机器人不知道其相对于先前创建的地图的相对位置，因此无法规划到先前访问过的位置的路径。为了避免机器人重新开始建图过程或在开始建图之前在先前构建的地图中进行定位，多会话建图允许 SLAM 方法在启动时用自己的参考系初始化新地图，当遇到先前访问过的位置时，可以计算两张地图之间的变换。这带来了避免重新建图整个环境的优点，当只需重新建图小部分或添加新区域时尤其如此。

因此，既然 RTAB-Map 发展到可以处理这些实际需求，我们决定进一步扩展 RTAB-Map 的功能，以比较用于自主机器人导航的视觉和激光雷达 SLAM 配置。RTAB-Map 作为具有内存管理的回环闭合方法，其核心独立于使用的里程计方法，这意味着它可以使用视觉里程计、激光雷达里程计甚至仅使用轮子里程计进行处理。这意味着 RTAB-Map 可用于实现视觉 SLAM 方法、激光雷达 SLAM 方法或两者的混合，从而可以在真实机器人上比较不同的传感器配置。本文描述了扩展版本的 RTAB-Map 并展示了其用于比较最先进的视觉和激光雷达 SLAM 方法的应用，从而概述了两种范式在自主导航中的实际局限性。

表1总结了与ROS兼容的开源SLAM方法及其输入和输出。激光雷达3D类别包括所有点云类型，包括从RGB-D摄像头的深度图像中提取的点云。Odom指的是可以用来帮助SLAM方法计算运动估计的里程计输入。3D占用栅格地图指的是OctoMap。

RTAB-Map 是一种基于图的 SLAM 方法，自2013年起作为rtabmap_ros包集成在ROS中。图1展示了其主要的ROS节点rtabmap。里程计是RTAB-Map的外部输入，这意味着SLAM也可以使用任何类型的里程计，以适应特定应用和机器人的需求。地图的结构是由节点和链接构成的图。在传感器同步后，短期记忆（STM）模块会创建一个节点，记录里程计位姿、传感器的原始数据以及对下一个模块有用的附加信息（例如，用于回环闭合和邻近检测的视觉词汇，以及用于全局地图组装的局部占用栅格）。节点按照固定的速率“Rtabmap/DetectionRate”创建，该速率以毫秒为单位设置，以确保节点创建的数据相互重叠。例如，如果机器人移动得快且传感器范围较小，应提高检测速率以确保连续节点的数据重叠，但设置过高会不必要地增加内存使用和计算时间。一个链接包含两个节点之间的刚性变换。链接有三种类型：邻居链接、回环闭合链接和邻近链接。邻居链接在STM中添加在连续节点之间，并带有里程计变换。回环闭合链接和邻近链接分别通过回环闭合检测或邻近检测添加。所有链接都作为图优化的约束使用。当有新的回环闭合或邻近链接添加到图中时，图优化会将计算误差传播到整个图，以减少里程计漂移。在图优化后，OctoMap、点云和2D占用栅格输出可以组装并发布到外部模块。通过tf/map→/odom还可以获得里程计校正，以推导出地图框架中的机器人定位。

RTAB-Map 的内存管理方法运行在图管理模块之上。它用于限制图的大小，从而在大环境中实现长期在线 SLAM。没有内存管理时，随着图的增长，像回环闭合和邻近检测、图优化和全局地图组装等模块的处理时间最终可能超过实时约束，即处理时间可能大于节点获取的周期时间。基本上，RTAB-Map 的内存分为工作内存（WM）和长期记忆（LTM）。当一个节点被转移到 LTM 时，它不再可用于 WM 内的模块。当 RTAB-Map 的更新时间超过固定时间阈值“Rtabmap/TimeThr”时，WM 中的一些节点会被转移到 LTM 以限制 WM 的大小并减少更新时间。类似于固定时间阈值，还有一个内存阈值“Rtabmap/MemoryThr”可以用来设置 WM 可以容纳的最大节点数。为了确定要转移到 LTM 的节点，权重机制使用一些启发式方法识别出比其他节点更重要的位置，例如观察一个位置的时间越长，它就越重要，因此应该保留在 WM 中。为此，在创建新节点时，STM 将节点的权重初始化为 0，并将其与图中的最后一个节点进行视觉比较（导出对应视觉词汇的百分比）。如果它们相似（对应视觉词汇的百分比超过相似性阈值“Mem/RehearsalSimilarity”），新节点的权重将增加一个加上最后一个节点的权重。最后一个节点的权重将重置为 0，如果机器人没有移动，则最后一个节点将被丢弃以避免无意义地增加图的大小。当达到时间或内存阈值时，最旧的最小权重节点将首先转移到 LTM。当与 WM 中的位置发生回环闭合时，该位置的邻居节点可以从 LTM 重新带回 WM 以进行更多的回环闭合和邻近检测。当机器人在先前访问的区域移动时，它可以逐步记住过去的位置，以扩展当前组装的地图并进行定位。

图1：所需的输入包括：用于定义传感器相对于机器人基座位置的TF；任何来源的里程计（可以是3DoF或6DoF）；一种相机输入（单个或多个RGB-D图像，或立体图像）及其对应的校准消息。可选输入包括来自2D激光雷达的激光扫描或来自3D激光雷达的点云。这些输入的所有消息随后被同步并传递给图-SLAM算法。输出包括：包含最新添加节点及压缩传感器数据和图的地图数据；不含任何数据的地图图；通过TF发布的里程计校正；可选的OctoMap（3D占用栅格）；可选的密集点云；可选的2D占用栅格。

里程计节点

里程计节点可以实现任何类型的里程计方法，从简单的轮式编码器和IMU衍生的里程计到使用相机和激光雷达的更复杂的里程计。无论使用哪种传感器，它都应至少向RTAB-Map提供截至目前估计的机器人位姿，以Odometry消息的形式，并包含相应的TF变换（例如，/odom → /base_link）。当机器人上没有已经可用的内感里程计或其精度不足时，必须使用视觉或激光雷达里程计。对于视觉里程计，RTAB-Map实现了两种标准的里程计方法，分别称为帧到地图（F2M）和帧到帧（F2F）。这两种方法的主要区别在于F2F将新帧与最后一个关键帧进行配准，而F2M则将新帧与由过去关键帧创建的特征局部地图进行配准。这两种方法也用于激光雷达，分别称为扫描到地图（S2M）和扫描到扫描（S2S），其思想与F2M和F2F相同，但使用点云代替3D视觉特征。以下部分将展示当选择其中一种视觉或激光雷达里程计方法时，里程计节点是如何实现的。

图2：RGB-D里程计和立体里程计ROS节点的框图。TF定义了相机相对于机器人基座的位置，并作为输出发布机器人的基座里程计变换。对于RGB-D相机和立体相机，管道是相同的，区别在于后者需要计算立体对应关系以确定检测到的特征的深度。可以使用两种里程计方法：绿色表示的帧到帧（F2F）方法，和红色表示的帧到地图（F2M）方法。

图3：ICP里程计ROS节点的框图。TF定义了激光雷达相对于机器人基座的位置，并作为输出发布机器人的基座里程计变换。可以使用两种里程计方法：绿色表示的扫描到扫描（S2S）方法，和红色表示的扫描到地图（S2M）方法。这些方法还可以选择使用恒速模型（粉色）或其他来源的里程计（蓝色）进行运动预测。对于后者，输入里程计的校正通过TF发布。

同步

RTAB-Map具有多种输入主题（例如RGB-D图像、立体图像、里程计、2D激光扫描、3D点云和用户数据），根据可用传感器可以选择使用。使rtabmap ROS节点正常工作所需的最低主题是通过主题或tf（例如，/odom→/base link）提供的配准RGB-D或校准立体图像与里程计。RTAB-Map还支持多个RGB-D摄像头，只要它们具有相同的图像尺寸。需要准确的传感器tf（例如，/base link→/camera link）。图4和图5展示了两个视觉SLAM示例及其对应的tf树。RTAB-Map的视觉里程计节点可以被任何其他里程计方法替换（例如，轮式里程计、其他视觉里程计软件包、激光雷达里程计等）。

图 4：使用RGB-D摄像头（如Xbox 360 Kinect）的视觉SLAM。rgbd odometry ROS节点用于计算rtabmap ROS节点的里程计。右侧显示了该传感器配置的标准TF树（通过虚线连接到相应的发布ROS节点）。

图 5：使用立体摄像头（如BumbleBee2）的视觉SLAM。stereo odometry ROS节点用于计算rtabmap ROS节点的里程计。RTAB-Map的ROS节点需要经过矫正的立体图像，因此使用标准的stereo image proc ROS节点进行矫正。右侧显示了该传感器配置的标准TF树（通过虚线连接到相应的发布ROS节点）。

STM（Short-Term Memory）

当STM创建新节点时，除了基本信息外，还会根据输入的深度图像、激光扫描或点云生成局部占用栅格。对于立体图像，使用块匹配算法计算密集的视差图，再转换为点云形式。局部占用栅格以机器人坐标系为基准，包括固定网格单元大小的空、地面和障碍单元。这些局部栅格通过地图图形的姿态变换生成全局占用栅格，从而大大减少了全局占用栅格重新生成所需的时间。

根据配置参数“Grid/FromDepth”和“Grid/3D”，局部占用栅格的生成方式有所不同，结果可以是2D或3D。例如，如果“Grid/FromDepth”为false，并且订阅了激光扫描主题，则会生成一个局部2D占用栅格。相比于3D占用栅格，2D占用栅格需要更少的内存，因为不需要保存额外的维度信息（例如z轴），同时能够有效减少重叠障碍物的处理复杂度。

该系统使用了多种技术来生成局部占用栅格，包括2D光线追踪、深度图像到点云的转换、点云的过滤和地面分割以及OctoMap的3D光线追踪。这些技术确保了对不同类型传感器数据的高效处理和精确建图，为SLAM（Simultaneous Localization and Mapping）系统提供了可靠的环境感知能力。

图 6：RGB-D摄像头（如Xbox One Kinect）、激光扫描（URG-04LX）和里程计的同步示例。在此示例中，里程计通过轮式编码器计算。摄像头消息通过rgbd sync ROS节点同步，然后再与其他传感器的结果RGB-D图像消息进行同步（这些传感器可能具有不同的发布速率）。右侧展示了该传感器配置的TF树示例（通过虚线连接到相应的发布ROS节点）。

图7: STM的局部占据网格创建。根据参数（由椭圆表示）和可选的激光扫描和点云输入的可用性（由菱形表示），局部占据网格可以是2D或3D。

闭环检测和邻近检测

闭环检测和邻近检测是RTAB-Map中的关键模块，用于增强SLAM系统的位置估计和地图更新能力。

闭环检测： 闭环检测利用基于词袋（bag-of-words）的方法，在创建新节点时从RGB图像中提取视觉特征，并将其量化到增量视觉词汇表中。特征可以是OpenCV中的各种类型，如SURF、SIFT、ORB或BRIEF。当使用F2F或F2M的视觉里程计时，可以重复使用为里程计提取的特征，避免重复提取相同的特征。闭环检测不需要像里程计那样多的特征来检测闭环，因此仅量化最高响应的一小部分（最多“Kp/MaxFeatures”个）里程计特征到视觉词汇表中。节点与STM中的节点进行比较以检测闭环，STM包含了最近添加到地图中的节点。为了计算新节点与所有STM中节点之间的可能性，使用Tf-IDF方法更新贝叶斯过滤器，估计闭环假设。当一个闭环假设达到固定阈值“Rtabmap/LoopThr”时，即检测到闭环，并计算变换。如果变换被接受，新的连接将被添加到图中。当可用激光扫描或点云时，连接的变换将使用与激光里程计相同的ICP配准方法进行精化。

邻近检测： 用于使用激光扫描定位接近当前位置的节点。例如，在回溯同一走廊的不同方向时，可以使用邻近检测进行定位，而相机无法找到闭环。与闭环检测的复杂性依赖于STM的大小不同，邻近检测的复杂性仅限于靠近机器人的节点。这些节点在图中应该是接近的，即它们与最新节点之间的链接数应小于固定阈值“RGBD/ProximityMaxGraphDepth”。当里程计漂移超过一定距离时，机器人可能移动到先前建图的区域，而不是当前的真实位置，因此使用该阈值，邻近检测不会与先前建图区域的节点进行比较，以避免无效的邻近检测。

图优化

当检测到环路闭合或邻近检测，或者由于内存管理而检索或转移一些节点时，将应用图优化方法来最小化地图中的错误。RTAB-Map集成了三种图优化方法：TORO，g2o 和GTSAM。g2o和GTSAM比TORO收敛更快，但在多会话建图（需要合并多个独立图形）时更不稳健。TORO对于误差估计较差的里程计协方差也不太敏感。然而，根据实证数据，在单一地图中，g2o和GTSAM的优化质量比TORO更好，特别是对于6DoF地图而言。在多会话建图方面，GTSAM比g2o稍微更为稳健，因此现在RTAB-Map默认使用GTSAM策略，与我们以前使用TORO的工作相反。

视觉环路闭合检测并非完全无误，非常相似的地点可能会触发无效的环路闭合检测，这会导致地图中增加错误而非减少错误。为了检测无效的环路闭合或邻近检测，RTAB-Map现在引入了一个新参数。如果一个链路在优化后的图中变化超过其平移方差的“RGBD/OptimizeMaxError”倍数，那么新节点添加的所有环路闭合和邻近链接将被拒绝，保持优化后的图，就好像没有发生环路闭合一样。

图 8: 全局地图组装。根据地图图中创建的局部地图类型（见图7），可用的输出全局地图将有所不同。只有3D局部占据栅格可以用来生成3D占据栅格（OctoMap）及其在2D中的投影。

全局地图输出

图8展示了可以从图7中的局部占据栅格组装而成的全局地图输出。将3D局部占据栅格保存在节点中提供了最大的灵活性，因为它们可以用来生成所有类型的地图。然而，如果只需要一个2D全局占据栅格地图，那么在节点中保存已投影的局部栅格可以节省内存（每个点只需两个数字而不是三个）和时间（点已经投影到2D时，组装局部地图时）。使用地图的图结构，每个局部占据栅格被转换成其相应的姿态。当向地图添加新节点时，新的局部占据栅格与全局占据栅格结合，清除和添加障碍物。当发生环路闭合时，应根据地图图中所有节点的新优化姿态重新组装全局地图。这个过程是必需的，以便在环路闭合之前错误清除的障碍物可以重新包含进来。点云输出将所有局部地图的点汇总，并以标准的sensor_msgs/PointCloud2 ROS格式发布。使用体素网格过滤来合并重叠的表面。生成的点云是可视化和调试的便捷格式，并便于与第三方应用集成。

图9: 使用RTAB-Map进行的轨迹估计，顶部是使用立体视觉里程计F2M，底部是使用激光雷达里程计S2S，对比KITTI数据集中的真实轨迹。RTAB-Map估计的姿态（蓝色）与地面真实轨迹（黑色）之间的误差显示为红色。

图10: 使用RTAB-Map进行的轨迹估计，使用RGB-D视觉里程计F2M（蓝色）与TUM数据集中的地面真实轨迹（黑色）进行对比。RTAB-Map估计的姿态与地面真实轨迹之间的误差显示为红色。

图11: 使用RTAB-Map进行的轨迹估计，顶部是使用立体视觉里程计F2M，底部是使用视觉惯性里程计OKVIS，对比EuRoC数据集中的真实轨迹。RTAB-Map估计的姿态（蓝色）与地面真实轨迹（黑色）之间的误差显示为红色。

图12: 使用RTAB-Map进行的轨迹估计（蓝色），对比Stata Center数据集中的地面真实轨迹（黑色）。左侧和右侧分别使用立体摄像头（顶部）或长距离激光雷达（底部）。RTAB-Map估计的姿态与地面真实轨迹之间的误差显示为红色。

图13: RTAB-Map的WheelIMU→S2M与其他基于激光雷达的SLAM方法的比较。

图14: 本地占用格网示例。对于摄像头，分割示例对应于没有光线追踪的3D本地占用格网，其他示例是没有或带有2D光线追踪的2D本地占用格网。障碍单元显示为红色，空单元和地面单元显示为绿色。黑色网格仅作为视觉参考，单元格大小为1米。

图15: 带有光线追踪示例的3D本地占用格网地图。

图16: 2D占用格网地图示例

图17: 使用RGB-D相机创建的OctoMap示例。a) 和 b) 分别展示深度为16和14的OctoMap

图. 18: 在MIT Stata Center序列的组合会话中，每个模块在rtabmap ROS节点内不带（a）和带（b）内存管理所需的处理时间，地图更新速率为2 Hz。

图19: 使用MIT Stata Center序列的组合会话创建的全局地图示例，分别展示了有（a,b）和没有（c）内存管理的情况。在（a）中，展示了在特定时间使用WM中可用节点在线创建的五个最大地图示例，蓝色三角形表示该时间点的机器人姿态。每个会话期间的全局轨迹分别显示为紫色和橙色。邻近、环路闭合和邻近链接分别显示为蓝色、红色和黄色。地面真实轨迹显示为绿色路径，在（b）和（c）中几乎看不见，因为与蓝线重叠

本文介绍了RTAB-Map的扩展版本，该版本完全集成了ROS，用于处理机器人的tf，同步RGB-D、立体视觉、激光扫描和点云话题，并能够为所有传感器生成占用格网。因此，RTAB-Map现在是一个多用途的基于图的SLAM方法，可以供新手SLAM用户直接使用，并用于具有不同传感器配置和处理能力的机器人平台的原型开发。它可以用于在数据集上比较性能并进行在线评估。用于SLAM的传感器，无论其成本低廉还是昂贵，都有限制，这些限制会影响定位精度、地图质量和计算资源。本文通过对视觉和激光雷达基础SLAM配置进行有意义的比较，展示了RTAB-Map的灵活性，从而分析哪种机器人传感器配置最适合室内自主导航。RTAB-Map作为一个开源库已经面向社区发布。目前，RTAB-Map是最受欢迎的ROS包之一，社区活跃使用，特别是用于RGB-D和立体视觉摄像头的低成本SLAM。我们的目标是继续集成新的里程计方法，缺乏适当的ROS集成，以便比较移动机器人平台自主导航的SLAM配置。