RTAB-Map学习记录(1)--论文阅读

前言

RTAB-Map（全称 Real-Time Appearance-Based Mapping）是一个开源的 RGB-D SLAM框架，主要用于机器人导航、3D 重建和环境建图。这个项目目前还在积极的维护和更新，也可以进行实际环境的部署。所以先学习一下相关的原理和论文，为之后的使用打下基础。

文章目录

前言
1.主要贡献
2.关键内容
- [2.1 里程计](#2.1 里程计)
- - [2.1.1 视觉里程计](#2.1.1 视觉里程计)
  - [2.1.2 激光雷达里程计](#2.1.2 激光雷达里程计)
- [2.2 同步性](#2.2 同步性)
- [2.3 STM](#2.3 STM)
- [2.4 回环检测与优化](#2.4 回环检测与优化)
- [2.5 全局地图组成](#2.5 全局地图组成)

1.主要贡献

首先看一下该方法的主要贡献有哪些，现有一个基本的了解：

在线处理： SLAM模块的输出应该被限制在接收到传感器数据之后的最大延迟。特别是对于基于图的SLAM，随着地图的增长，需要更多的处理时间来回环检测、图优化和构建地图。因此，具有限制计算负载的可能性对于避免与其它模块的滞后问题是有益的，并且甚至对于防止不安全的情况可能是必要的。
稳健且低漂移的里程计： 虽然回环检测可以校正大多数里程计漂移，但在现实世界的场景中，机器人通常不能在地图上正确地定位自己。这要么是因为它正在探索新的区域，要么是因为环境中缺乏区分特征。在这段时间内，里程计漂移应该被最小化，使得精确的自主导航仍然是可能的，直到定位可以发生，以避免不正确的地图区域（例如，在房间的入口处不正确地添加障碍物，例如使其成为封闭区域）。当环境中有足够的特征时，利用诸如相机和激光雷达的外部感受传感器来估计里程可以是非常准确的，但是仅使用一种感测模态可能是有问题的，并且如果它们在环境中的被跟踪特征不再可见，则易于定位失败。本体感受传感器和外部感受传感器的混合将增加对里程估计的鲁棒性。
鲁棒的定位： SLAM方法必须能够识别当它正在重访过去的位置(对于回环检测)来校正地图。动态环境、光照变化、几何变化甚至重复环境都可能导致错误定位或定位失败，因此该方法应该对误报具有鲁棒性。
实际的地图生成和开发： 大多数流行的导航方法都是基于占位栅格的，因此开发可以提供3D或2D占位栅格的SLAM方法是有益的，以便于集成。另外，当环境大多是静态的时候，先做一个映射会话，然后切换到本地化，设置内存使用量，节省地图管理时间更具有实际意义。
多会话建图( a.k.a.绑架机器人问题或初始状态问题)： 当重新开启机器人时，机器人不知道其与先前创建的地图的相对位置，从而无法规划到先前访问过的位置的路径。为了避免机器人在开始建图之前将建图过程重新启动到零或在先前构建的地图中进行定位，多会话建图允许SLAM方法在启动时初始化一个具有自己参考点的新地图，当遇到先前访问过的位置时，可以计算两张地图之间的转换。这带来的好处是当只需要重新建图一小部分或者增加一个新的区域时，就可以避免重新创建整个环境。

2.关键内容

RTAB-Map 是一种基于图的 SLAM 方法，自 2013 年以来已集成到 ROS 中，作为 rtabmap ros 包。下图显示了其主要 ROS 节点，称为 rtabmap。里程计是 RTAB-Map 的外部输入，这意味着 SLAM 可以使用任何类型的里程计，以适应特定应用和机器人。

在传感器同步后，短期记忆 (STM) 模块创建一个节点，用于存储里程计姿态、传感器的原始数据以及对后续模块有用的附加信息。节点以固定速率Rtabmap/DetectionRate创建，根据节点间数据重叠的程度设置该速率。如果机器人移动速度快且传感器范围小，则应增加检测速率以确保连续节点的数据重叠，但设置过高会不必要地增加内存使用和计算时间。当向图中添加新的循环闭合或邻近链接时，图优化会将计算误差传播到整个图中，以减少里程计漂移。在图优化后，可以组成并发布 OctoMap , Point Cloud , 2D Occupancy Grid输出给外部模块。通过 /map→/odom 也提供里程计校正，以推导出机器人在地图坐标系中的定位。

RTAB-Map 的内存管理方法运行在图管理模块之上。它用于限制图的大小，从而在大环境中实现长期在线 SLAM。如果没有内存管理，随着图的增长，循环闭合和邻近检测、图优化以及全局地图组装的处理时间最终可能会超过实时约束，即处理时间可能大于节点采集周期时间。基本上，RTAB-Map 的内存分为工作记忆 (WM) 和长期记忆 (LTM)。当一个节点转移到 LTM 时，它不再对 WM 内的模块可用。当 RTAB-Map 的更新时间超过固定时间阈值Rtabmap/TimeThr时，WM 中的一些节点会转移到 LTM 以限制 WM 的大小并减少更新时间。为了确定哪些节点转移到 LTM，使用权重机制识别比其他位置更重要的位置。为此，在创建新节点时，STM 将节点的权重初始化为 0，并通过视觉比较与图中的最后一个节点比较。如果它们相似，则新节点的权重增加 1 加上最后一个节点的权重。最后一个节点的权重重置为 0，如果机器人未移动，则丢弃最后一个节点以避免无用地增加图的大小。当达到时间或内存阈值时，最旧的最小权重节点首先转移到 LTM。当发生与 WM 中位置的循环闭合时，该位置的邻近节点可以从 LTM 带回 WM 以进行更多循环闭合和邻近检测。随着机器人在先前访问的区域移动，它可以逐步回忆过去的的位置，从而扩展当前组装的地图并使用过去的位置进行定位。

2.1 里程计

里程计节点（Odometry Node）可以实现任意类型的里程计方法，从基于轮式编码器和 IMU 的简单方法，到使用相机和激光雷达的复杂方法均可支持。无论采用哪种传感器，它都必须至少以 Odometry 消息的形式向 RTAB-Map 提供当前已估计的机器人位姿，并同时发布对应的 TF 变换（例如 /odom → /base_link）。

当机器人上原本没有本体感受里程计（proprioceptive odometry），或其精度不足时，就必须使用基于视觉或激光雷达的里程计。对于视觉里程计，RTAB-Map 实现了两种经典方法：

Frame-to-Map (F2M)：帧到地图
Frame-to-Frame (F2F)：帧到帧

两者核心区别在于：F2F 是将当前新帧与上一个关键帧进行配准；而 F2M 是将当前新帧与由历史关键帧构建的局部特征地图进行配准。这两种思路同样被扩展到激光雷达上，分别称为：

Scan-to-Map (S2M)：扫描到地图
Scan-to-Scan (S2S)：扫描到扫描

原理与 F2M/F2F 完全一致，只是将 3D 视觉特征替换为点云。

2.1.1 视觉里程计

图 2 使用两种颜色区分 F2F（绿色）和 F2M（红色），展示了 RTAB-Map 的视觉里程计。该模块支持 RGB-D 或双目相机作为输入。必须通过 TF 提供相机在机器人上的安装位置，才能将输出的里程计变换到机器人基座坐标系（例如 /base_link）。即使相机安装在机器人头部且头部发生转动，只要机器人本体与头部之间的 TF 同步更新，就不会影响机器人基座的里程计。整个处理流程如下：

特征检测：捕获一帧图像后，使用 GoodFeaturesToTrack（简称 GFTT）检测特征，最大特征数量由参数 Vis/MaxFeatures 限定。RTAB-Map 支持 OpenCV 中所有可用的特征类型，但选用 GFTT 是为了简化参数调优，并在不同图像分辨率和光照条件下获得分布均匀的特征。对于双目图像，使用迭代 Lucas--Kanade 光流法计算左右图像间的立体对应关系，得到每个特征的视差。对于 RGB-D 图像，则用深度图作为 GFTT 的掩码，避免提取深度无效的特征。
特征匹配：
F2M： 对提取的特征计算 BRIEF 描述子，与特征地图中已有的描述子进行最近邻搜索，并通过最近邻距离比测试过滤，NNDR 阈值由参数 Vis/CorNNDR 设定。特征地图保存了最近若干关键帧的 3D 特征及其描述子。
F2F： 直接对 GFTT 特征进行光流计算，无需提取描述子，因此能更快地获得与上一个关键帧的特征对应。
运动预测：利用恒速运动模型，根据上一时刻的运动变换预测关键帧（F2F）或特征地图（F2M）中的特征在当前帧中的大致位置，从而大幅缩小特征匹配的搜索窗口。这在存在动态物体或重复纹理的环境中能显著提升匹配质量。
运动估计：得到特征对应后，使用 OpenCV 的PnP RANSAC 实现当前帧相对于关键帧（F2F）或特征地图（F2M）的刚体变换的计算。
局部光束法平差：使用local BA对特征进行优化。在F2M 对特征地图中所有关键帧的特征进行优化，F2F 仅对上一个关键帧的特征进行优化。
位姿更新：根据优化后的变换更新输出的里程计，同时发布 TF 变换 /odom → /base_link。协方差矩阵采用 3D 特征对应间的中值绝对偏差（Median Absolute Deviation, MAD）方法计算。
关键帧与特征地图更新：若运动估计阶段的内点数量低于阈值，则触发更新。
F2F： 直接用当前帧替换上一个关键帧。
F2M： 将当前帧中未匹配的特征加入特征地图，并更新经局部光束法平差优化后的匹配特征位置。

特征地图有固定最大容量，超过时会移除与当前帧未匹配的最旧特征。若某个关键帧在特征地图中已无任何特征，则将其丢弃。

若由于相机运动与预测差异过大，导致运动估计或局部光束法平差后仍无法得到有效变换，则会再次进行特征匹配但不使用运动预测：

F2M： 将当前帧特征与特征地图中所有特征进行匹配，重新计算变换；
F2F： 改为使用带 NNDR 测试的 BRIEF 描述子匹配（而非光流），以提高鲁棒性。

如果仍无法得到有效变换，则认为里程计丢失，后续帧将不再使用运动预测。输出的里程计位姿被置为 null，并赋予极高协方差（9999），让订阅该节点的模块能够明确检测到里程计失效。

注意：由于 RTAB-Map 的里程计模块完全独立于建图过程，为了方便使用和对比，其他视觉里程计方法也被集成进来。所选方法均为开源或提供 API，且可单独作为纯里程计使用。那些难以将里程计与建图过程分离的完整视觉 SLAM 系统则无法集成（因为建图由 RTAB-Map 负责）。

目前已集成以下七种方法：

- FOVIS [Huang et al., 2011]
- Viso2 [Geiger et al., 2011]
- DVO [Kerl et al., 2013]
- OKVIS [Leutenegger et al., 2015]
- ORB-SLAM2[Mur-Artal and Tardós, 2017]
- MSCKF [Sun et al., 2018]
- Google Project Tango

其中 FOVIS、Viso2、DVO、OKVIS 和 MSCKF 均为纯视觉或视觉-惯性里程计，可直接将其里程计输出接入 RTAB-Map。ORB-SLAM2 是完整的 SLAM 系统，集成时关闭了其内部的回环检测，但保留了局部光束法平差，因此功能上接近 F2M。主要区别在于特征类型和匹配方式。同样对特征地图大小进行了限制，以保证恒定计算时间。Google Project Tango 集成时关闭了区域学习功能，直接使用其视觉-惯性里程计输出。

2.1.2 激光雷达里程计

下图为激光雷达里程计的流程框图，同样使用两种颜色区分 S2S（绿色）和 S2M（红色）。此处采用与视觉里程计类似的术语，关键帧指一个点云或激光扫描数据。激光扫描输入为 2D，而点云输入可以是 2D 或 3D。当机器人运动时，激光扫描可能会产生运动畸变，这里假设这些畸变在送入 RTAB-Map 之前已被预先校正。
注意：如果激光雷达旋转频率远高于机器人运动速度，运动畸变会非常小，此时可忽略畸变校正，而不会显著损失配准精度。

处理流程如下：

点云滤波：对输入点云进行降采样并计算法线。通过 TF 将点云变换到机器人基座坐标系（/base_link），以保证里程计在正确的坐标系下计算。
ICP 配准：使用 libpointmatcher 的迭代最近点（ICP）算法，将当前点云与点云地图（S2M）或上一个关键帧（S2S）进行配准。支持两种对应关系：Point-to-Point（P2P）和 Point-to-Plane（P2N）。在人工环境中（存在大量平面结构）通常优先选用 P2N。
运动预测：由于 ICP 无法直接知道特征对应关系，因此在配准前必须提供有效的运动预测。该预测可来自上一次配准结果，或来自外部里程计（如轮式里程计）通过 TF 提供。
在处理前两个帧时，仅使用单位变换（Identity）作为初始预测。若不使用外部里程计，则采用恒速运动模型进行预测。这种方式的问题在于：当环境特征不足（如长走廊）时，机器人运动方向可能缺乏约束，导致里程计严重漂移。此时使用外部里程计作为初值可显著改善效果。
例如，机器人在没有门的狭长走廊中行驶，短程激光雷达只能看到两条平行墙。若机器人沿走廊方向加速或减速，ICP 可以校正航向，但无法检测沿走廊方向的速度变化。在这种情况下，外部里程计可以帮助估计该缺失方向的速度。此外，当当前点云的结构复杂度低于参数时，ICP 仅估计旋转，平移（沿缺失特征的方向）则直接由外部里程计提供。结构复杂度定义：2D 点云为法线 PCA 的第二特征值乘以 2；3D 点云为第三特征值乘以 3。
位姿更新：配准成功后更新里程计位姿。当使用外部里程计时，RTAB-Map 输出的 TF 是对外部里程计的校正（即 /odom_icp → /odom → /base_link），使两者能共存于同一 TF 树中。协方差同样采用 MAD 方法基于点对应关系计算。
关键帧与点云地图更新：若对应点比率低于阈值，则：
S2S： 当前帧直接成为新的关键帧；
S2M： 在加入点云地图前，先从当前点云中减去与地图重叠的部分，再将剩余点加入地图。当点云地图达到最大容量时，移除最旧的点。

如果 ICP 无法找到有效变换，则认为里程计丢失。与视觉里程计不同，激光雷达里程计在运动预测无效时无法自动恢复（为避免产生极大误差），必须进行重置。不过，只要激光雷达能够感知到足够的环境结构，机器人实际丢失里程计的情况非常少见。
注意： 当使用外部里程计时，即使暂时丢失跟踪，若机器人回到之前丢失的位置，运动预测仍能提供合理初值，ICP 有可能重新恢复跟踪。

2.2 同步性

RTAB-Map 支持多种输入话题，包括 RGB-D 图像、双目图像、里程计、2D 激光扫描、3D 点云以及用户自定义数据，可根据实际搭载的传感器灵活选用。
要让 rtabmap ROS 节点正常工作，至少需要已配准的 RGB-D 图像或已标定的双目图像，同时提供里程计（可通过话题发布，或通过 TF 提供，例如 /odom → /base_link）。

下图分别展示了两种视觉 SLAM 配置示例及其对应的 TF 树。RTAB-Map 自带的视觉里程计节点可以被任意其他里程计方案替换（如轮式里程计、其他视觉里程计包、激光雷达里程计等）。图中虚线表示哪个节点负责发布对应的 TF 变换。描述传感器在机器人上安装位置的其他 TF 帧，通常由相机驱动、静态变换发布器（static_transform_publisher）或机器人状态发布器（robot_state_publisher，使用机器人 URDF 模型）负责发布。

订阅基础传感器后，还可以可选地同步以下两种话题：

2D 激光扫描 （如 Hokuyo、SICK 激光雷达）→ 用于生成 2D 占据栅格地图；
3D 点云（如 Velodyne 激光雷达）→ 用于生成 3D 占据栅格地图。

这两类数据也可用于通过 ICP 对图中的边进行进一步优化。由于不同传感器发布数据的频率和时刻通常不完全一致，高质量的同步对避免数据配准错误至关重要。ROS 提供了两种同步策略：
精确同步（Exact Synchronization）： 要求所有输入话题的时间戳完全相同，适用于来自同一传感器的数据（如双目相机的左、右图像）。
近似同步（Approximate Synchronization）： 比较各话题的时间戳，寻找时间差最小的组合，适用于来自不同传感器的数据。

当需要将一部分话题（例如相机相关话题）使用精确同步，而同时与其他传感器使用近似同步时，同步会变得较为复杂。此时可使用 rtabmap_ros/rgbd_sync nodelet，先将相机相关话题（RGB + Depth）同步为单一的 rtabmap_ros/RGBDImage 类型话题，再输入给 rtabmap 节点。

图 6 展示了一个典型的 RGB-D 相机 + 激光雷达同步示例。对于 RGB-D 相机，许多 ROS 驱动包并不能保证 RGB 和深度图像的时间戳完全一致，此时也可使用 rgbd_sync 以近似同步方式工作，按相机本身的帧率同步图像，而不受其他输入（如激光扫描、里程计）频率的影响。

2.3 STM

当在 STM中创建一个新节点时，除了 [Labbé and Michaud, 2017] 中已描述的信息之外，现在还会从深度图像、激光扫描或点云中额外计算一个局部占据栅格。

对于双目图像，先使用块匹配算法计算密集视差图像，再将其转换为点云。局部占据栅格以机器人坐标系为参考，包含空闲（empty）、地面（ground）和障碍物（obstacle） 三种单元格，栅格分辨率固定为参数 Grid/CellSize。

这些局部占据栅格通过使用地图中各节点的位姿，变换到地图坐标系后，即可快速生成全局占据栅格。虽然为每个节点预先计算局部占据栅格会占用更多内存，但当图优化后，全局占据栅格的再生时间会大幅缩短。

根据参数 Grid/FromDepth、Grid/3D 以及实际订阅的输入话题，局部占据栅格的生成方式不同，结果可为 2D 或 3D，如下图所示。例如：

若 Grid/FromDepth 为 false，且 rtabmap 节点订阅了激光扫描话题，则生成局部 2D 占据栅格。2D 局部占据栅格比 3D 占用更少内存（少保存一个维度，例如 z），且重叠的障碍物可合并为单个障碍物单元格。但局部 2D 占据栅格无法用于生成 3D 全局占据栅格；而局部 3D 占据栅格则可同时用于生成 2D 和 3D 全局占据栅格。最终选择取决于应用所需全局地图的类型以及可用计算资源。
注意： 若 Grid/FromDepth 为 false 且未订阅激光扫描和点云话题，则不会计算任何占据栅格。

下图中各矩形框的处理流程说明如下：

2D 射线追踪：对激光测距仪的每条射线，在栅格上进行直线追踪，填充传感器与射线击中障碍物之间的空闲单元格。假设所有射线均平行于地面。该方法生成 2D 局部占据栅格速度极快，是基于 2D 激光雷达建图时的默认方式。
深度图像转点云：将输入深度图像（双目时为视差图像）根据传感器坐标系和相机标定参数投影到 3D 空间，随后将点云变换到机器人基座坐标系。
滤波与地面分割：使用体素栅格滤波器对点云进行降采样，体素大小等于固定的栅格单元大小。随后进行地面分割：计算点云法线，将法线与 z 轴（向上方向）夹角小于参数 Grid/MaxGroundAngle 的所有点标记为地面，其余点标记为障碍物。
投影：若 Grid/3D 为 false，则将 3D 地面点云和障碍物点云投影到地面平面（x-y 平面）。再次应用体素栅格滤波器，将投影到同一单元格的点进行合并。可选择执行 2D 射线追踪来填充障碍物与相机之间的空闲空间。若不使用 2D 射线追踪，且点云中没有被分割为地面的点，则占据栅格中传感器与障碍物之间不会填充空闲单元格。
3D 射线追踪：以机器人坐标系下的单个局部占据栅格为基础创建 OctoMap。OctoMap 执行 3D 射线追踪，检测相机与占据单元格之间的空闲单元格。最后将 OctoMap 转换回局部占据栅格格式，保留空闲、地面和障碍物三种单元格。

2.4 回环检测与优化

回环检测采用词袋模型方法。基本流程为：在创建新节点时，STM 从 RGB 图像中提取视觉特征，并将其量化到增量式视觉词词汇表中。支持 OpenCV 中所有特征类型，例如 SURF、SIFT 、ORB或 BRIEF。

当使用视觉里程计的 F2F 或 F2M 模式时，可以直接复用里程计已提取的特征进行回环检测，从而避免重复提取特征。由于回环检测所需的特征数量少于里程计，为了降低计算量，仅选取响应值最高的子集进行量化，其余特征仍保留在节点中，待后续计算回环变换时使用。

新创建的节点会与 WM 中的所有节点进行比较以检测回环。STM 中保存的是最近加入地图的节点，因此这些节点不参与回环检测（因为它们与当前节点位置极为相似，会严重偏置回环假设）。STM 可视为一个固定大小的缓冲区（由参数 Mem/STMSize 控制），节点在移入 WM 之前会先在此缓冲。

计算新节点与 WM 中所有节点的似然值时，采用 TF-IDF 方法更新贝叶斯滤波器，用于估计回环假设。该滤波器判断新节点是来自之前访问过的位置还是全新位置。当某个回环假设的似然值达到固定阈值 Rtabmap/LoopThr 时，即认为检测到回环，随后计算两节点间的变换。

回环变换的计算采用与视觉里程计完全相同的运动估计方法。若变换被接受，则在地图图中添加一条新边。当存在激光扫描或点云数据时，还会使用与激光雷达里程计相同的 ICP 配准方法对该边的变换进行进一步优化。

邻近检测（Proximity Detection）用于在激光扫描可用时对当前机器人位置附近的节点进行定位。例如，当机器人沿同一走廊反向行驶时，相机可能无法检测到回环，但通过邻近检测仍能实现精确定位。

与回环检测（复杂度随 WM 大小线性增长）不同，邻近检测的复杂度被严格限制在机器人附近的节点。这些节点在图中必须与最新节点相邻，即两者之间相连的边数不得超过参数设定的阈值。当里程计在大距离上发生漂移时，机器人可能进入之前已建图但与真实位置不符的区域；此时该阈值可防止与远处旧节点进行无效比较，从而避免错误的邻近检测。若里程计漂移较小或地图更新频率较高，可适当增大该阈值，否则应调小。

当检测到回环或邻近检测，或因内存管理而对节点进行检索/转移时，会执行图优化算法以最小化地图中的累积误差。RTAB-Map 集成了三种图优化方法：TORO、g2o 和 GTSAM。

其中 g2o 和 GTSAM 的收敛速度明显快于 TORO，但在多会话建图（需合并多个独立图）场景下鲁棒性较差。TORO 对里程计协方差估计不佳的情况更不敏感。

不过，对于单地图场景，根据实际测试数据，g2o 和 GTSAM 的优化质量优于 TORO，尤其在 6DoF 地图中表现更佳。GTSAM 在多会话场景下的鲁棒性略优于 g2o，因此目前已成为 RTAB-Map 的默认优化策略（而之前的工作均使用 TORO）。

视觉回环检测并非完全可靠，高度相似的场景可能引发无效回环检测，反而会向地图引入更多误差。为此，RTAB-Map 引入了一个新参数用于剔除无效的回环或邻近检测：

若优化后图中某条边的变换变化量超过其平移方差的 RGBD/OptimizeMaxError 倍，则将该新节点所添加的所有回环检测边和邻近检测边全部拒绝，优化后的图将保持为"未发生回环"的状态。

2.5 全局地图组成

下图展示了可由之前局部占据栅格组装而成的各种全局地图输出。

在节点中保存3D 局部占据栅格能提供最高的灵活性，因为它们可用于生成所有类型的全局地图。然而，若应用仅需2D 全局占据栅格地图，则在节点中保存已投影的 2D 局部栅格可显著节省内存（每个点只需保存 2 个数值而非 3 个）和计算时间（点已预先投影到 2D 平面），在组装全局地图时优势明显。
利用地图图（map's graph），每个局部占据栅格都会根据其对应节点的位姿变换到全局坐标系。当向地图中加入新节点时，新局部占据栅格会与当前全局占据栅格进行融合：清除旧障碍物并添加新障碍物。

当发生回环时，必须根据地图图中所有节点优化后的新位姿，重新组装整个全局地图。这一重新组装过程是必要的------它能将回环发生前被错误清除的障碍物重新纳入地图。点云输出将所有局部地图中的点合并在一起，并以标准的 ROS sensor_msgs/PointCloud2 格式发布。合并过程中会执行体素栅格滤波，以融合重叠表面。最终得到的点云是便于可视化、调试以及与第三方应用集成的理想格式。