介绍

Real‐time appearance‐based mapping (RTAB‐Map)是一种基于外观的闭环检测方法，具有良好的内存管理，以满足处理大场景和在线长周期管理要求。RTAB‐Map集成了视觉和激光雷达SLAM方案，并支持目前绝大多数的传感器。

简介

SLAM 强调在建图与定位的精确性与实时性，在电脑性能有限的情况下，往往这两个要求是相互矛盾的，我们需要在程序中充分的考虑如何平衡它们，取得最佳的实践效果。一般SLAM的需求为：

• 实时处理：随着地图的不断增长，需要更多时间来处理闭环检测、图优化和地图组合等，所以将前端输出频率设置为可接受的最大延迟，能够避免其他的模块出现处理滞后的问题。
• 低漂移的里程计：虽然闭环检测可以纠正大部分里程计漂移，但在探索新区域或在缺乏辨别力环境中难以正确定位。保证里程计漂移最小化，以便在定位之前仍然可以进行准确的自主导航。当环境中有足够的特征时，使用相机和激光雷达等传感器估计里程表可以非常准确，但是，如果环境中的跟踪特征不再可见，则仅使用一种传感方式可能会出现问题并且容易出现定位失败。混合使用多传感器（例如，车轮编码器、IMU）和外部感受器传感器可以增加对里程计估计的鲁棒性。
• 鲁棒定位：SLAM 方法必须能够识别何时重新访问过去的位置（回环检测）以纠正地图。动态环境、光照变化、几何变化，甚至重复的环境都可能导致不正确的定位或定位失败，因此，该方法应该对以上问题具有鲁棒性。
• 地图的生成：目前机器人导航方法都是基于栅格地图，对于静态的大场景地图，有效的地图管理是做地图会话，在定位时进行调用与内存管理，来减少地图维护时间。
• 多地图：当机器人不知道其与先前创建的地图的相对位置或丢失定位，从而无法规划到先前访问过的位置的路径。为了避免机器人从零重新启动映射建图或将自己定位在先前构建的地图中，多会话映射允许在启动时先使用初始化新地图，当遇到先前访问的位置时，再将两张地图融合。

ROS 上已有的 SLAM 方法

激光雷达方案：

• GMapping(2007) 和 TinySLAM(2010)：使用粒子过滤器估计机器人轨迹，只要有足够的估计粒子和实际位置误差对应于输入里程计的协方差，粒子滤波器就会收敛到能够很好地代表环境的解，特别是对于存在闭环时。GMapping 是 ROS 的默认 SLAM 方法，已被广泛用于从 2D 激光扫描中导出环境的 2D 栅格地图。创建地图后，它可以与自适应蒙特卡洛定位一起用于定位和自主导航。
• Hector SLAM(2011)：可以从具有低计算资源的 2D 激光雷达快速创建 2D 栅格地图，并且具有非常低的定位漂移，还可以使用 IMU 等外部传感器来估计机器人的 3D 位置。然而，Hector SLAM 并不完全是一种完整的 SLAM 方法，因为它没有回环检测，因此在回到之前的定位时无法纠正地图。
• Karto SLAM(2010)、Lago SLAM(2012) 和 Cartographer(2016)：都是基于图优化的方案，可以生成 2D 栅格地图。Google Cartographer 支持 3D 激光雷达，从而提供 3D 点云输出。在映射建图时，它们会创建由图中的约束链接的子图。当检测到闭环时，重新优化子图的位置以纠正由传感器噪声和扫描匹配精度引入的错误。与 Hector SLAM 不同，可以提供外部里程计以在几何复杂度较低的环境中获得更稳健的扫描匹配。
• SegMatch(2016)： 是一种基于 3D 激光雷达的闭环检测方法，也可以用作基于 3D 激光雷达的图优化 SLAM。通过匹配从激光点云创建的 3D 片段（例如，车辆、建筑物或树木的一部分）来检测回环。

视觉方案：
对于机器人导航而言，真实的尺度比例是必要的，因此我们会使用立体相机或RGBD相机，而避免使用单目相机，尽管它也能实现三维重建与定位（可以有，但没必要）。

• maplab(2018) 和 VINS-Mono(2017)：视觉惯性图优化SLAM系统，仅使用 IMU 和摄像头，它们就可以提供用于定位的视觉地图。maplab 工作流程分两步完成： 在开环阶段仅使用视觉惯性里程计记录数据；然后离线完成地图管理（即回环检测、图优化、多会话和密集地图重建）。然后可以在定位模式下使用地图。相比之下，VINS-Mono 的地图管理是在线完成的。对于导航，提供了图形处理单元 (GPU) 上计算的局部截断符号距离场 (TSDF) 体积图，用于避障和路径规划。但是难以对大规模的场景进行重建。
• ORB-SLAM2(2017) 和 S‐PTAM(2017)：两种基于特征的视觉 SLAM 方法可以与立体相机一起使用，ORB-SLAM2还可以使用 RGB-D 相机。且都是基于图优化的 SLAM 方法，当使用 DBoW2 检测到闭环时 ，地图使用 BA 进行优化。闭环后的图优化在单独的线程中完成，以避免影响相机跟踪帧率性能。闭环检测和图优化处理时间随着地图的增长而增加，使闭环检测发生延迟。它们仅保持稀疏地图，并没有栅格地图和稠密点云的输出。
• DVO-SLAM(2013) 、RGBiD-SLAM(2016) 和 MPR-SLAM(2017)：不是使用局部视觉特征来估计运动，而是使用 RGB-D 图像的所有像素的光度差和深度差。它们可以生成环境的密集点云。MPR 也可以与激光雷达一起使用，但它只是一种里程计方法。DVO-SLAM 缺乏独立于姿态估计的闭环检测方法，这使得它不太适合大规模建图。
• ElasticFusion(2016)、Kintinuous(2015)、BundleFusion(2017) 和 InfiniTAM(2016): 基于 RGB-D 相机的 TSDF 模型。可以实时重建基于面元的地图，但需要一台配备最新 Nvidia GPU 的强大计算机。对于 ElasticFusion，虽然能够针对小型环境实时处理相机帧，但每帧的处理时间会根据地图中面元的数量而增加。对于 BundleFusion，闭环检测的全局密集优化时间根据环境的大小而增加。InfiniTAM 似乎可以更快地关闭环路，尽管环路闭合检测和校正的处理时间仍会随着环境的大小而增加。虽然是开源的，但这些算法不支持 ROS，因为它们依赖于 GPU 上的映射和跟踪之间极其快速和紧密的耦合。

以上视觉 SLAM 方法都假设相机永远不会被遮挡，或者图像总是有足够的视觉特征来跟踪。这种假设实际上不能在自主机器人上得到满足，例如被过路人挡住，或者对没有视觉特征的表面（例如，白墙）时。以下视觉 SLAM 方法旨在对这些事件更加稳健：

• MCPTAM(2015)：使用多个摄像头来增加系统的视野。如果可以通过至少一个摄像头感知视觉特征就能够跟踪位置。
• RGBDSLAMv2(2014)：可以使用外部里程计作为运动估计，可以生成 3D 栅格地图和环境的密集点云。ROS 包如robot_localization可用于对多个里程计源进行传感器融合（使用扩展的卡尔曼滤波器），以获得更稳健的里程计。

RTAB-MAP 详解

RTAB-Map 是一种基于图优化的 SLAM，以 rtabmap_ros 包集成到 ROS 中。里程计是 RTAB-Map 的外部输入，这意味着也可以使用任何类型的里程表来完成，来满足给定应用程序和机器人的要求。实时地图是带有一系列节点和边的图结构。在外部传感器时间戳同步后，短期记忆 (STM) 模块创建一个节点，用于记忆里程计姿态、传感器的原始数据以及对下一个模块有用的附加信息（例如，用于回环检测和接近检测的视觉词，以及用于全局地图组合的局部栅格地图）。根据从节点创建的数据应相互重叠的数量，来固定速率“Rtabmap/DetectionRate”（毫秒为单位创建节点）。例如，如果机器人移动速度快，传感器范围小，则应提高检测率，以确保连续节点的数据重叠，但是将其设置得太高会不必要地增加内存使用量和计算时间。图中的边包含两个节点之间的刚性变换。存在三种边：Neighbor (临边)、Loop Closure(闭环边) 和 Proximity(邻近边)。Neighbor 通过在STM中里程计的连续节点添加，Loop Closure 和 Proximity 边分别通过闭环检测和邻近检测添加。所有的边都将作为图的约束。当向图中添加新的闭环或邻近边时，图优化将计算的误差传播到整个图，以减少里程计漂移。随着图的优化，OctoMap、点云和 2D 栅格网格可以进行组装并输出发布到外部模块。

RTAB-Map 的内存管理在图管理模块上运行，用于限制图的大小，以便在大场景中实现长期在线 SLAM。在没有内存管理的情况下，随着图的增长，诸如 Loop Closure 和 Proximity Detection、Graph Optimization 和 Global Map Assembling 等模块的处理时间最终可能会超过实时约束，即处理时间可能会大于节点获取周期时间。在没有内存管理的情况下，随着图的增长，诸如 Loop Closure 和 Proximity Detection、Graph Optimization 和 Global Map Assembling 等模块的处理时间最终可能会超过实时约束，即处理时间可能会大于节点获取周期时间。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，以进行更多的闭环和邻近检测。

里程计节点

里程计节点可以实现任何类型的里程计方法，与所使用的传感器无关，它应该向 RTAB-Map 提供目前估计的机器人姿态，带有相应tf变换（例如，/ odom -> /base_link）的 Odometry 消息。对于视觉里程计，RTAB-Map 实现了两种标准里程计方法称为帧到图 (F2M) 和帧到帧 (F2F)。这些方法之间的主要区别在于，F2F 将新帧注册到最后一个关键帧，而 F2M 将新帧注册到从过去关键帧创建的本地特征图。对于激光雷达，称为扫描到地图 (S2M) 和扫描到扫描 (S2S)，

视觉里程计

可以使用 RGB-D 或立体摄像机作为输入。tf需要知道相机在机器人上的放置位置，以便输出里程计可以转换为机器人基础框架（例如，/base_link）。

• 特征检测：获取到新的一帧后，检测GFTT特征，也支持OpenCV中其他的特征点，但选择 GFTT 是为了简化参数调整并在不同的图像尺寸和光强度下获得统一检测的特征。对于立体图像，深度信息是通过光流使用迭代 LucasKanade 方法计算的，以得出左右图像之间每个特征的视差。对于 RGB-D 图像，深度图像用作 GFTT 的掩码，以避免提取无效深度的特征。
• 特征匹配：对于 F2M，匹配是通过最近邻搜索和最近邻距离比 (NNDR) 计算的，使用 BRIEF 描述子将提取的特征与特征图中的特征进行对比。Feature Map 包含 3D 特征和最后一个关键帧的描述子。对于 F2F，光流直接在 GFTT 特征点上完成，无需提取描述子，提供与关键帧的更快特征对应。
• 运动预测：运动模型用于根据先前的运动变换预测关键帧（F2F）或特征图（F2M）的特征在当前帧中的位置。这限制了特征匹配的搜索窗口以提供更好的匹配，特别是在具有动态对象和重复纹理的环境中。
• 运动估计：计算特征对应关系时，OpenCV 的 PnP RANSAC 实现用于计算当前帧对应于关键帧 (F2F) 或特征图 (F2M) 中的特征的变换。
• 局部 BA：对特征图中所有关键帧的特征 (F2M) 或仅对最后一个关键帧 (F2F) 的特征进行优化得到运动变换结果。
• 姿态更新：使用运动估计的变换，然后更新输出里程计以及 tf 变换。使用3D 特征对应之间的中值绝对偏差 (MAD) 方法计算协方差。
• 关键帧和特征图更新：如果在运动估计期间计算的内点数低于固定阈值“Odom/KeyFrameThr”，则更新关键帧或特征图。对于 F2F，关键帧简单地被当前帧替换。对于 F2M，通过添加新帧中的不匹配特征并更新由局部 BA 模块优化的匹配特征的位置来更新特征图。特征图有一个固定的最大特征临时保存（因此最大的关键帧）。当特征图的大小超过固定阈值“OdomF2M/MaxSize”时，与当前帧不匹配的最旧特征被删除。如果关键帧在特征图中不再具有特征，则将其丢弃。

由于某些原因相机的当前运动与预测的运动非常不同，则可能找不到有效的变换，因此需要再次匹配特征。对于 F2M，将当前帧中的特征与 Feature Map 中的所有特征进行比较，然后得到一个变换矩阵。对于 F2F，为了更加鲁棒，使用 NNDR 进行特征匹配而不是光流，因此必须提取 BRIEF 描述符。如果仍然无法计算运动变换，则认为里程计丢失，并且在没有运动预测的情况下比较下一帧。输出里程计姿势设置为空值，具有非常高的方差（即 9999）。订阅此视觉里程计节点的模块可以知道何时无法计算里程计。
可以看到，里程计是独立于映射过程，可以利用其它的里程计方案，RTabMap 集成了FOVIS、Viso2 、DVO 、OKVIS、ORB-SLAM2 、ORB-SLAM2 。FOVIS、Viso2、DVO、OKVIS 和 MSCKF 是仅视觉或视觉惯性里程计的方法，通过将它们的里程计输出连接到 RTAB-Map，它们可以直接集成。ORB-SLAM2 是一个完整的 SLAM 方法，因此为了集成到 RTAB-Map 中，ORB-SLAM2 内部的闭环检测被禁用。ORB-SLAM2 的局部束调整仍然有效，这使得修改后的模块类似于 F2M。最大的区别在于提取的特征类型（ORB) 以及它们如何匹配在一起（直接描述符比较而不是 NNDR），由于 ORB-SLAM2 的设计没有被设计为删除或忘记其地图中的特征，因此在删除特征时不会释放内存，这会导致内存泄漏。

激光雷达里程计

当机器人在扫描过程中移动时，激光扫描可能会出现一些运动失真。这里假设在将扫描馈送到 RTAB-Map 之前纠正这种失真。请注意，如果激光扫描仪的旋转频率相对于机器人速度较高，则激光扫描的运动失真会非常低，因此可以忽略校正，而不会显着损失配准精度。

• 点云过滤：输入点云进行下采样并计算法线。tf 用于将点云转换为机器人基础框架，以便相应地计算里程计（例如，/base_link）
• ICP 注册：ICP 将新点云注册到点云图 (S2M) 或最后一个关键帧 (S2S)，点云图是由过去的关键帧组装而成的云。可以使用点对点 (P2P) 或点对平面 (P2N) 对应来完成注册。
• 运动预测：由于 ICP 正在处理未知的对应关系，因此该模块需要在估计变换之前进行有效的运动预测，无论是来自先前的注册还是来自外部里程计方法（例如，车轮里程计）。
• 姿势更新：成功 ICP 注册后，里程计姿势会被更新。当使用外部里程计时，tf 输出是外部里程计 tf 的校正，因此两个变换可以在同一个tf树中（即 /odom_icp ->/odom -> /base_link）。与视觉里程计一样，协方差是使用 3D 点对应之间的 MAD 方法。
• 关键帧和点云图更新：如果对应比率低于固定阈值“Odom/ScanKeyFrameThr”，则新帧成为 S2S 的关键帧。对于 S2M，在将新点云集成到点云地图之前需要完成一个额外的步骤。从新点云中减去地图（使用最大半径“OdomF2M/ScanSubtractRadius”），然后将剩余点添加到点云地图中。当点云地图达到固定的最大阈值“OdomF2M/ScanMaxSize”时，最旧的点将被删除。

如果 ICP 注册失败，则会丢失里程计。与视觉里程计相比，当运动预测为空时，激光雷达里程计无法从丢失中恢复，以避免出现较大的里程计误差。然后必须重置激光雷达里程计。然而，只要激光雷达能够感知环境结构，机器人就很少会迷路。请注意，如果使用外部里程计，运动预测仍然会给出有效的估计，因此如果机器人返回丢失跟踪的位置，ICP 配准可以从丢失中恢复。

同步

RTAB-Map 有多种输入主题（例如，RGB-D 图像、立体图像、里程计、2D 点云、3D 点云和用户数据），可根据当前的传感器使用。使rtabmap ROS 节点工作所需的最少主题是具有里程计的已注册 RGB-D 图像或已校准立体图像，通过主题或tf（例如/odom -> /base_link）提供。RTAB-Map 还支持多 RGB-D 相机，只要它们具有相同的图像尺寸。
由于传感器并不总是以相同的速率和相同的准确时间发布数据，因此良好的同步对于避免数据的错误注册很重要。ROS 提供了两种同步方式：精确和近似。精确同步要求输入主题具有完全相同的时间戳，即来自同一传感器的主题（例如，立体相机的左右图像）。近似同步比较传入主题的时间戳，并尝试以最小延迟误差同步所有主题。它用于来自不同传感器的主题。

短期记忆 STM

当在 STM 中创建新节点时，根据深度图像、激光扫描或点云计算局部栅格地图。机器人框架中引用了一个局部栅格地图，它包含固定网格单元大小“Grid/CellSize”的空、地面和障碍部分。局部栅格地图的总大小由传感器的范围和视野定义。这些局部栅格地图用于生成全局栅格地图，方法是使用地图的图中的姿势在地图参考中变换它们。虽然预先计算局部栅格地图需要为每个节点提供更多内存，但它大大减少了优化图结构后全局栅格地图的重新生成时间。
根据传感器的类型与设置不同栅格地图可以分为：2D 栅格地图和 3D 栅格地图。2D 局部栅格地图比 3D栅格地图需要更少的内存。但是，局部 2D 栅格地图不能用于生成 3D 全局栅格地图，而局部 3D 栅格地图可用于生成 2D 和 3D 全局栅格地图。

• 2D Ray Tracing：对于激光测距仪的每条光线，在网格上追踪一条线以填充传感器和光线撞击的障碍物之间的空单元格。假设光线平行于地面。这种方法可以非常快速地生成 2D 局部栅格地图，并且默认为基于 2D 激光雷达的映射完成。
• Depth Image to Point Cloud：输入深度图像（或立体图像的视差图像）根据传感器框架和相机校准投影在 3D 空间中。
• Filtering and Ground Segmentation：点云由体素网格过滤器下采样，体素大小等于固定网格单元大小。然后从点云中分割地平面：计算点云的法线；在固定的最大角度“Grid/MaxGroundAngle”内，所有法线平行于轴（向上）的点都被称为地面，其他点是障碍物。
• Projection：如果“Grid/3D”为假，则将 3D 地面和障碍物点云投影到地平面上。
• 3D Ray Tracing：从机器人参考中的单个本地栅格地图创建 OctoMap。OctoMap 进行 3D 光线追踪并检测相机和占用单元之间的空单元。OctoMap 被转换到具有空、地面和障碍物单元的本地栅格地图格式。

闭环和邻近检测

闭环检测是使用词袋方法完成的。基本上，当创建一个新节点时，STM 从 RGB 图像中提取视觉特征点，并将它们量化为一个增量的视觉词汇表。特征可以是 OpenCV 中包含的任何类型，例如加速鲁棒特征 (SURF)、尺度不变特征变换 (SIFT)、ORB 或 BRIEF。当使用视觉里程计 F2F 或 F2M 时，可以重用已经为里程计提取的特征来进行闭环检测。由于闭环检测不需要像里程计那么多的特征点来检测闭环，因此只有响应最高的里程计特征的子集（“Kp/MaxFeatures”的最大值）被量化为视觉词汇表。
邻近检测用于通过激光扫描（如果可用）定位靠近当前位置的节点。例如，通过邻近检测，可以在沿不同方向返回同一条走廊时进行定位，在此期间相机无法用于查找回环。与复杂性取决于 WM 大小的闭环检测相比，邻近检测的复杂性受限于靠近的节点。这些节点在图中必须靠近，即它们与最新节点之间的链接数应小于固定阈值“RGBD/ProximityMaxGraphDepth”。当里程计漂移过大距离时，机器人可能会移动到与当前实际位置不同的先前映射区域，因此调整此阈值大小，使得邻近检测不与来自先前映射区域的节点进行比较，以避免无效的邻近检测。如果里程计没有漂移太多或地图更新率较高，则可以将阈值设置得更高，否则应降低。

图优化

当检测到闭环或邻近时或由于内存管理而检索或转移节点时，应用图优化方法以最小化地图中的错误。RTAB-Map 集成了三种图优化方法：基于树的网络优化器 (TORO) 、图优化的通用框架 (g2o)和 GTSAM。g2o 和 GTSAM 比 TORO 收敛得更快，但是当多个独立的图必须合并在一起时，多会话映射的鲁棒性较差。TORO 对估计不佳的里程计协方差也不太敏感。然而，对于单张地图，基于经验数据，g2o 和 GTSAM 优化质量优于 TORO，特别是对于六自由度地图。GTSAM 对多会话的鲁棒性略强于 g2o，因此它是现在 RTAB-Map 中默认使用的策略。

视觉闭环检测在非常相似的地方可能会触发无效的闭环检测，这会给地图增加错误。为了检测无效的闭环或邻近检测，RTAB-Map 使用了一个新参数。如果优化后图中链接的变换变化大于其平移方差的因子“RGBD/OptimizeMaxError”，则拒绝新节点添加的所有闭环和邻近链接，保持优化后的图就好像没有发生闭环一样。

全局地图拼接

可以看到在节点中保存 3D 本地栅格地图提供了最大的灵活性，因为它可用于生成所有类型的地图。使用地图的图结构，将每个局部栅格地图转换为其对应的位姿。当向地图添加新节点时，新的局部栅格地图与全局栅格地图相结合，清除并添加障碍物。当发生闭环时，应根据地图的图中所有节点的所有新优化姿势重新拼接全局地图。这个过程是必需的，以便可以重新包含在闭环之前被错误清除的障碍物。