RTAB-Map 论文阅读：核心原理与架构解析

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 格式发布。合并过程中会执行体素栅格滤波，以融合重叠表面。最终得到的点云是便于可视化、调试以及与第三方应用集成的理想格式。