RTAB-Map：适用大范围长周期的开源雷达视觉SLAM库

写在前面
最近一直在做无人机自主定位和导航的相关工作，虽然开源方案很多，但是放在实物上之后效果总是感觉不太理想。2018年8月份，Field Robot接受了一篇论文《RTAB‐Map as an open‐source lidar and visual simultaneous localization and mapping library for large‐scale and long‐term online operation》，这篇论文对目前ROS平台上的开源SLAM做了一个全面的总结，并且着重介绍了RTAB-Map的算法框架和不同传感器在数据集下的运行结果。之前也了解过RTAB，但是没想到近一两年更新这么多新东西，从之前的rgbd、双目相机扩展到相机、雷达都可以进行定位建图的完整SLAM系统。并且在每个模块都提供了很多集成方案，所以准备先从论文入手，然后上飞机实测一下。

摘要

RTAB从2013年开始作为开源库，作为一个基于外观（appearance-based）闭环检测方法，同时拥有一个内存管理（memory management）在线处理大尺度、长时间（large-scale and long-term）定位和建图问题。由于每个应用程序都对传感器，处理能力和运动带来了一系列限制，因此它提出了在成本，准确性，计算能力和集成简易性方面最适合使用哪种SLAM方法的问题。很多SLAM系统都是基于视觉或者雷达信息的，因此很难对比它们的性能。因此，我们决定扩展RTAB-Map以支持视觉和激光雷达SLAM，在一个软件包中提供一个工具，允许用户使用不同的机器人和传感器实现和比较各种3D和2D解决方案，适用于各种应用。本文介绍了RTAB-Map的这一扩展版本，以及它在定量和定性地比较大量流行的真实数据集， 从自主导航应用的实用角度概述视觉和激光雷达SLAM配置的优势和局限性。

Introduction

rtab的发展流程，2009年开始设计，2013年首次开源，2016年完全基于图优化，2017年扩展了很多新应用，目前被发展成跨平台独立的C++库和一个ROS功能包。能满足以下实际需求。

1. 在线处理：接收传感器数据后，SLAM模块的输出应限制为最大延迟。 特别是对于基于图（graph）的SLAM，随着地图（map）的增长，需要更多的处理时间进行闭环检测，优化graph和组合map。 此外，与用于控制，导航，避障，用户交互，目标识别等的其他处理模块的集成，还可能限制CPU对SLAM的计算力。 因此，尽可能限制计算负荷有利于避免SLAM系统与其他模块的滞后问题，以防止不安全情况的产生。
2. 鲁棒、低漂移的里程计：虽然闭环检测可以修正大多数的里程计漂移，但是在真实环境中，机器人通常不能在map中准确定位自身，因为它探索新的区域或者环境中没有足够的特征。在这段时间内，里程计漂移应该尽可能地被减小以至于在定位准确之前能完成自主导航，避免错误的覆盖已经建图的区域（例如在房间的入口错误的添加了障碍物，使其成为封闭空间）。当环境中有足够的特征时，使用相机和lidars等外部感知传感器来估计里程可以非常准确，但如果只使用一种感知模式，当它们在环境中跟踪的特征不再可见时，就会出现问题，容易导致定位失败。使用本体感觉(如车轮编码器、惯性测量单元[IMU])和外部感觉传感器的组合，将增强对里程计估计的鲁棒性。
3. 鲁棒的定位：当机器人重新访问之前的位置（闭环检测）时，SLAM方法必须有所识别，并且修正地图。动态环境、光照变化、几何形状变化，甚至重复的环境都可能导致不正确的定位或定位失败，因此，这种方法应该对错误（假阳性）具有鲁棒性。
4. 实际地图的生成和开发：主流导航算法都基于占用栅格，因此发展能提供3D或2D占有栅格来方便其他算法集成的SLAM方法是有益的。此外，当环境大部分是静态的时候，更实际的做法是进行mapping session，然后切换到定位，设置内存使用情况，并节省建图管理时间。
5. multisession mapping（也称为绑架机器人或初始状态问题）：当一个机器人开始工作时，它不知道它相对于之前创建地图的位置，这使得无人机不可能规划一条路径去往之前访问过的位置。为了避免机器人重置建图过程，以及使用以前建过的地图，multisession mapping允许SLAM方法在启动时根据自己的参考去初始化一个新的map，然后当一个之前访问过的位置出现时，两个map之间的转换被计算出来。这带来的好处就是，避免了只有一小部分重映射或者一块新的区域需要加进来的时候，机器人重映射整个环境。

由于SLAM方法的多样性，在特定的平台和场景中决定使用哪一种成了一个困难任务，因为缺乏各种算法之间的对比分析。SLAM方法通常基于视觉（Fuentes‐Pacheco, Ruiz‐Ascencio, & Rendón‐Mancha, 2015），或者基于雷达（Thrun，2002），并且仅使用一个相机或者雷达在数据集上测试，很难有一个有意义的对比。尤其是算法只能离线运行或者缺少机器人平台上所需的输入格式时，情况变得更加困难。当前，在ROS中集成的视觉SLAM算法通常不在自主机器人上测试，只是通过手持传感器的方法，这样测试省去了一些SLAM之外的研究（路径规划，障碍规避等），但是，这同时也限制了各种SLAM方法的比较。

因此，自从RTAB-Map向解决实际问题发展，我们决定去扩展rtab的功，对比视觉和雷达SLAM配置解决机器人自主导航。rtab是一个闭环方法，以内存管理作为核心，它独立于所使用的里程计方法，这意味着它可以使用视觉、雷达甚至是车轮里程计。也就是rtab可以应用于视觉、雷达或者二者结合的方法，使在一个真实机器人上对比不用传感器配置成为可能。这篇论文描述了rtab库的扩展版本，并陈述了与当前主流方法的使用情况对比，最后列出提纲，说明两种范例在实际应用中的限制。

ROS上的主流SLAM算法

基于雷达的算法

Gmapping

基于粒子滤波方法并且有回环检测过程，是ROS默认的SLAM方法，广泛应用于生成2D栅格地图，地图建好后，可以配合蒙特卡洛定位方法使用。

Hector

快速，低计算资源的建立2D栅格地图，被证实在真实环境中产生很低的定位漂移，也可以加入IMU来估计机器人的3D位置，但是它不算一个完整的SLAM算法，因此没有回环检测。同时，该算法不需要里程计，在没有很多几何约束的环境下效果受限。

ICP-Mapper

可以创建2D栅格地图和点云图，和Hector相似，没有闭环检测。

Karto SLAM和Cartographer

都是基于图的雷达SLAM方法，在mapping时，它们创建由图中的约束连接的submap，当检测到闭环时，submap的位置被重优化来修正误差（由传感器噪声和扫描匹配的精度引起），与Hector不同的是，它们可以接收其他外置里程计，在低几何复杂度的环境中，得到更鲁棒的扫描匹配。

BLAM

基于图的雷达SLAM方法，且仅支持3D雷达。当机器人出现在之前访问过的位置时，通过scan matching局部地进行闭环检测（全局优化效果一般）。

SegMatch

3D雷达闭环检测，基于图。闭环检测通过匹配3D分割信息（车辆，建筑，树木等）。在这些方法中，仅SegMatch方法支持multisession或multirobot mapping。

视觉SLAM

关于视觉SLAM，有很多开源方法存在，但在机器人上使用起来并不容易。对于导航功能，为了避免处理尺度不确定性问题，我们将综述的范围限制在能估计环境真实尺度的方法上（双目相机、RGBD相机或者VIO）。

maplab和VINS-Mono

二者都是视觉-惯导，基于图的视觉SLAM方法。maplab的工作流程分为两部分：
（1）仅使用VIO记录开环数据；
（2）离线完成地图管理（闭环检测，图优化，稠密建图等）。

得到的视觉地图可以在之后的定位模式中使用。相反，vins-mono的地图管理进程是在线的，TSDF可以在GPU上实时计算来提供障碍规避和路径规划。为了保持大尺度环境的处理时间，vins-mono限制了graph的尺寸，首先移除没有闭环的节点，然后根据graph的稠密度移除其他节点。

ORB-SLAM2和双目PTAM

这两个算法是目前视觉SLAM中的state-of-the-art。二者都是基于图的算法，闭环基于DBoW2，map优化使用BA。在单独的线程中对闭环后的graph进行优化，以避免影响相机跟踪帧率性能。 对于ProSLAM，通过直接比较map中的描述子来检测循环闭包，而不是使用词袋的方法。闭环检测和图优化进程的耗时随着map的增长会产生明显延迟，并且map是稀疏特征，没有占用栅格和稠密地图，因此很难用在实际平台上。

DVO-SLAM RGBiD-SLAM

估计运动时不使用特征（feature）而是使用所有像素的光度和深度误差。DVO没有闭环检测，大尺度建图劣势。

ElasticFusion

可以在线重建surfel-based地图，需要GPU提供计算力。建图比较精细，但是不支持ROS。

之前提出的SLAM方法假设相机从未被遮挡，或者图像中一直有足够多的特征。这两个假设在实际飞行中不可能一直满足，以下两种方法在实际中的鲁棒性更好一些：
（1）多相机PTAM：使用多个相机来增大FOV，至少一个相机感知到视觉特征，该方法就可以完成位置追踪。
（2）RGBDSLAMV2：

多种传感器的里程计信息融合（通过EKF），同时生成稠密地图和3D占用栅格地图。

表1对SLAM算法进行了总结，文字描述省略。

RTAB-Map描述

rtab基于graph，流程如图1所示。

这个流程图是一个标准的基于图的SLAM架构，传感器信息经过时间对准之后，进入STM模块，然后进行前端处理（和VO那套差不多），后端进行图优化和全局地图维护。论文中提到一个参数“rtabmap/DetectionRate”，原文“according to how much data created from nodes should overlap each other”，根据从节点创建的数据应该彼此重叠多少（翻译的感觉很怪）。但是举了一个例子：当机器人运动快并且相机视场小的时候，这个值应该被提高，来保证连续节点的数据有重叠，但是设置太大会占用内存和耗费时间。link(链接)包含了两个节点之间的刚性变换(rigid transform)，在这里有三种链接：邻居，闭环，相似。“邻居”连接是在STM里面通过odom转换建立的，“闭环”和“相似”也是通过闭环检测和相似性检测实现的。这些连接都用作图优化的约束条件。当一个新闭环或者相似产生时，图优化将计算误差传播到全图来减小里程计漂移。伴随着优化过程，Octomap，点云，2D栅格地图都在被全局更新。
对于SLAM的常规操作就不多说了，接下来说rtab比较有特点的内存管理方法（Labbé & Michaud，2013）。

为了防止地图变大带来的问题，rtab的内存被分成工作内存（WM）和长期内存（LTM），当一个节点被分到LTM中后，WM的流程对其不再起作用。当rtab的更新时间超过固定阈值“Rtabmap/TimeThr”，在WM的节点会被转换成LTM中来限制WM的size。同样，还有一个阈值“Rtabmap/MemoryThr”，可以设置WM中最大保持的节点数。

为了决定哪个节点被分到LTS中，一个权重机制决定了“哪个位置”更重要，使用一种启发式的方法：一个位置被观测的越长，它越重要，就应该放在WM中。 因此，当一个新的节点到来时，初始化它的权重为0，并且与上一个节点进行可视化比较（我感觉计算一个相似度的意思），如果它们很相似（对应的视觉单词超过相似阈值“Mem/RehearsalSimilarity“），新节点的权重等于旧节点权重加上1，同时将旧的节点权重置为零（我觉得就是一个权重传递过程），如果机器人没有移动，直接将旧的节点删除避免扩大graph的size。当时间或者内存满了，优先将权重最低的旧节点放到LTM里面。

当WM的一个位置发生闭环检测时，这个position的相邻节点（neighbor nodes）会被从LTM调回WM，当机器人在之前到过的地方移动时，它可以增量地记住过去的位置以扩展当前map并使用过去的位置进行定位。

里程计节点（Odometry node）

里程计节点可以使用任何一种简单的里程计方式（车轮、IMU、单相机、单雷达）。独立于使用的传感器，里程计为rtab提供至少一种位姿估计信息，以odometry message形式配合tf使用（例如/odom to /base_link），当本体传感器不准确时，视觉和雷达传感器是必需的。视觉里程计方面，rtab应用了两种标准的里程计方法，Frame-to-Map（F2M）和Frame-to-Frame（F2F）。

F2M就是当前帧和一张特征图进行关联，当然特征图有之前很多帧图像的特征（有用的、显著的）。F2F就是当前帧和上一帧进行关联。雷达传感器的scan-to-scan和scan-to-map同理于视觉（雷达的point cloud可以认为和视觉的3D visual features在前端等价）。

视觉里程计

贴个大图，很清晰的结构，有slam基础的看起来应该很轻松。

• 特征检测（feature detection）
  当一个新的frame被捕获时，GoodFeatureToTrack（GFTT）特征被检测，最大数量由”Vis/MaxFeatures“参数给出。rtab支持opencv中的所有特征。双目相机根据光流法来计算左右图像的特征视差，rgb-d相机的深度图作为GFTT的mask，避免了提取深度无效的特征。
  
• 特征匹配（feature matching）
  特征匹配使用的是最近邻搜索（nearest neighbor search），引入BRIEF描述子。将当前帧的图像特征和特征图中的图像特征基于描述子进行操作。特征图包含带有来自最近几个关键帧的描述子的3D特征。对于F2F，光流直接作用在GFTT特征上，而不用提取描述子，这样提供更快的关键帧特征对应。
  
• 运动预测（motion prediction）
  运动预测是来预测key frame或者feature map的特征应该出现在当前帧的什么位置上，根据是之前的运动转换（类似于状态方程）。这可以限制feature matching过程中的搜索窗口来提供更好的匹配，尤其是在动态环境和重复的纹理特征。搜索窗口半径由”Vis/CorGuessWinSize“给出，该过程中，一个恒定的速度运动模型被使用。

• 运动估计（motion estimation）
  当相关性被计算出来之后，PnP RANSAC等算法被用来计算当前帧的转换，基于F2F和F2M的特征点。inliers的最小值”vis/mininliers“作为接受转换结果的条件。

• 局部BA优化
  转换结果通过对特征使用局部BA方法进行修正。在F2M模式下是将转换结果基于所有关键帧的特征上做优化，F2F只能基于上一帧的特征点。
  
• 位姿更新（pose update）
  结合估计的转换，里程计输出被更新，/odom to /base_link。
  
• 关键帧和特征图更新（key frame and feature map update）
  如果运动估计过程中inliers的数量低于固定阈值“odom/keyframethr”，则关键帧或者特征地图被更新（这个阈值来衡量当前帧的信息是否丰富）。对于F2F，关键帧被当前帧代替，对于F2M，特征地图被更新（添加了新frame中没有匹配的特征），并且更新之前匹配过的特征点（经过BA优化后）的位置。当特征地图的size大于阈值之后，采取一些措施来维护特征地图，比如丢弃没有和当前帧匹配上的oldest特征。
  
  
  如果当前相机的运动和预测值相差很多时，可能是没有找到一个有效的transformation，这时特征会再次匹配但是不伴随于运动预测。
  （1）对于F2M，当前帧的特征会和特征地图的所有特征进行对比，然后会计算得到另一个transformation。
  （2）对于F2F，为了提高无效结果出现时的系统鲁棒性，特征匹配使用NNDR而不是光流，然后提取BRIEF描述子。
  如果transformation还是没用被计算出，里程计被认为已经丢失（之前是里程计可能丢失），并且下一帧图像会在没有运动预测的情况下进行对比（也许是重新初始化？）。此时里程计输出的位姿会被设置为null且方差很大，这是里程计失效的信息会被模块订阅到。
  
  rtab的里程计是独立于mapping过程的，因此其他的视觉里程计可以被集成在rtab中，并且方便不同里程计之间的比较。rtab继承了七种方法：（1）fast visual odometry（2）Viso2（3）DVO（4）OKVIS（5）ORB-SLAM2（6）MSCKF（7）Google Project Tango。12346五种方法都是vo或者vio可以直接将里程计输出给rtab系统。
  
  ORB-SLAM2是一个完整的SLAM方法，集成在rtab中时，ORB的闭环检测不能使用，但是ORB的局部BA优化依然工作，使得修改之后的模块与F2M相似。大的区别是被提取特征的种类（ORB）和匹配它们的方法（直接描述符比较而不是NNDR，之前说到rtab的F2M的特征匹配方法是NNDR，orb原始框架也是基于NNDR）。 orb-slam2没有维护特征地图的大小，在集成orb到rtab时，弥补了orb本身的这个不足（保证实时性）。
  为了集成google project tango到rtab库，area learning特征被禁用，而是直接使用它的vio。

雷达里程计

先放一张雷达里程计的流程图

使用类似于视觉里程表的术语，关键帧指的是点云（point cloud，3D雷达）或激光扫描（laser scan，2D雷达）。当机器人在scan过程中移动时，激光扫描会产生一些运动畸变（motion distortions），这里假定在将扫描提供给RTAB‐Map之前纠正了这种畸变。注意：如果激光扫描仪的旋转频率相对于机器人速度较高，激光扫描的运动畸变非常小，因此可以忽略校正，而不会显著降低配准精度。（我觉得北洋的雷达好像不进行畸变校准就挺好用的，包括之前cartographer上的测试）。

• 点云滤波
  对输入的点云进行降采样运算并计算法线，利用tf信息将点云转换到机器人的基础帧。
• ICP registration
  register（登记？处理？融合？添加？）新的点云信息到点云图（S2M，和视觉中的特征图一样）或者上个关键帧（S2S），迭代最近点（ICP）是通过使用Libpointmatcher完成的。点云地图（point cloud map）是由过去的关键帧组成的，registration可以由点到点（point-to-point）或点到平面（point-to-plan）对应来实现。在有很多平面时优先选择P2N。
• 运动预测
  由于ICP处理的是未知的对应关系，因此该模块在估计变换前需要一个有效的运动预测，无论是以前的配准方法还是通过TF外部测程方法。当机器人在有两面平行墙壁的环境会出现一些问题，因为沿着两条平行线中间移动的时候获得的点云信息没什么变化。或者当室外很空旷的时候，雷达多数量测失效这时需要加入外部里程计（IMU）来估计运动。然后如何定义环境复杂度，论文中给出了一点说明，但是我没有仔细了解。
• 位姿更新
  register成功后，里程计位姿将被更新。当使用外部里程计被使用时，tf输出是对外部里程计tf的修正，使两种变换可以位于同一tf树中（odom_icp->odom->base_link）。
• 关键帧和点云图更新
  判断关键帧是根据一个相关性阈值决定，在S2S中，当前帧直接变成关键帧，但是在S2M中，当前帧要经过和原map融合的过程，添加新信息，删除旧信息（维护一个固定大小的地图）。

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

Synchronization（同步）

传感器种类多了之后涉及到ROS环境中的时钟同步问题，没太细看。不过这部分按照说明进行配置，使用起来效果还不错，毕竟都是常用的传感器和ros包。

STM（short-term memory）

第一段说明当新节点在STM里生成时，会得到一个局部占用栅格（local occupancy grid），基于地图的图位姿（pose of the map‘s graph）能把局部的栅格图拼成全局的，并且经过优化后的graph对建立全局栅格图很有帮助。
第二段说明了根据传感器的不同格式可以生成不同的栅格地图，简单说就是2D数据只能生成2D栅格，3D数据可以生成2D或者3D。

loop closure（visual）and proximity detection （laser）

闭环检测是基于2013年提出的基于词袋（bag-of-words）方法，STM提取新节点的视觉特征，并将它们量化为一个递增的视觉单词词汇表。特征可以用opencv中的SURF，SIFT，ORB等表示。当视觉里程计F2F或F2M被使用时，我们可以使用里程计中提取出来的特征来进行闭环检测。这节省了提取两次特征的时间。闭环检测需要的特征不需要想里程计中那么多，因此只需要之前特征集合的一个子集（典型特征组成的子集）来完成闭环检测。新建立的node和WM（work memory保留着整个过程中重要的nodes）中的nodes进行比较来检测闭环，STM里面的nodes都是最近才加入map的，因此这些节点不会用来去进行闭环检测。STM可以作为一个buffer在节点被移动至WM中，为了计算新节点和WM中所有节点的相似性（likelihood），Tf-IDF方法被使用去建立一个贝叶斯滤波器估计闭环假设。

如果闭环检测满足闭环条件，此时有里程计解算出来的转换关系就可以作为一个新的link加到graph中。
2017年提出的proximity detection是基于雷达scan来定位接近当前位置的nodes。 举例：在以不同的方向穿越长廊时，相机没法找到闭环，这时proximity detection就有帮助了。闭环检测的复杂度取决于WM的size，proximity detection的复杂度取决于靠近机器人的nodes数量。这些nodes在graph中必须很靠近（机器人运动增量很小），也就是说，这些nodes和最新node的连接（links）数必须小于一个阈值（原因绿色部分有说明）。
这段主要说明了视觉和雷达在SLAM后端过程的两种策略。

图优化

闭环检测和近似检测之后，图优化方法开始最小化map中的误差（errors）。rtab集成了三种图优化方法（1）TORO（2）G2O（3）GTSAM，（2）和（3）比（1）快，但是鲁棒性略差。但是经验表明，在6DoF的maps中G2O和GTSAM效果更好。

视觉闭环检测不是完全准确的，在相似的地方会触发无效的闭环检测，这反倒增加了map中的误差。为了检测到这些无效的处理，rtab使用了一个新参数，如果经过优化后，link的转换改变大于平移方差的一个因子“RGBD/optimizemaxerror”（假设两个节点理论上的link距离分布为N（0.5,0.2），而结果得到了2，明显就是优化错误。），所有基于新节点的闭环检测结果都被reject，保持闭环检测之前的graph。

全局地图组装

在局部栅格地图信息上先“拼成”全局地图，然后根据后端优化结果对map进行修正。然后map发布在ros标准格式（sensor_msgs/pointcloud2）上。方便其他算法的开发和调试。

不同传感器的轨迹性能评估

数据集

RTAB的视觉和雷达配置的计算性能评估

数据集

Discussion

感觉这部分的总结都比较常规，简单说一些结论：
（1）室内导航，双目精度高，但是更推荐RGBD相机，因为可以检测到纹理特征少的表面来规避障碍。如果室内有玻璃等反射物品，视觉不再安全。
（2）多相机模式能增大FOV，但是有标定和计算载荷问题，对于2D环境使用有点冗余。
（3）对于无人机的3D导航，多相机模式比3D雷达模式在成本上更有优势。
举例子也没有举无人机应用的例子……