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功能包。能满足以下实际需求。
- 在线处理：接收传感器数据后，SLAM模块的输出应限制为最大延迟。 特别是对于基于图（graph）的SLAM，随着地图（map）的增长，需要更多的处理时间进行闭环检测，优化graph和组合map。 此外，与用于控制，导航，避障，用户交互，目标识别等的其他处理模块的集成，还可能限制CPU对SLAM的计算力。 因此，尽可能限制计算负荷有利于避免SLAM系统与其他模块的滞后问题，以防止不安全情况的产生。
- 鲁棒、低漂移的里程计：虽然闭环检测可以修正大多数的里程计漂移，但是在真实环境中，机器人通常不能在map中准确定位自身，因为它探索新的区域或者环境中没有足够的特征。在这段时间内，里程计漂移应该尽可能地被减小以至于在定位准确之前能完成自主导航，避免错误的覆盖已经建图的区域（例如在房间的入口错误的添加了障碍物，使其成为封闭空间）。当环境中有足够的特征时，使用相机和lidars等外部感知传感器来估计里程可以非常准确，但如果只使用一种感知模式，当它们在环境中跟踪的特征不再可见时，就会出现问题，容易导致定位失败。使用本体感觉(如车轮编码器、惯性测量单元[IMU])和外部感觉传感器的组合，将增强对里程计估计的鲁棒性。
- 鲁棒的定位：当机器人重新访问之前的位置（闭环检测）时，SLAM方法必须有所识别，并且修正地图。动态环境、光照变化、几何形状变化，甚至重复的环境都可能导致不正确的定位或定位失败，因此，这种方法应该对错误（假阳性）具有鲁棒性。
- 实际地图的生成和开发：主流导航算法都基于占用栅格，因此发展能提供3D或2D占有栅格来方便其他算法集成的SLAM方法是有益的。此外，当环境大部分是静态的时候，更实际的做法是进行mapping session，然后切换到定位，设置内存使用情况，并节省建图管理时间。
- 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，有很多开源方法存在，但在机器人上使用起来并不容易。对于导航功能，为了避免处理尺度不确定性问题，我们将综述的范围限制在能估计环境真实尺度的方法上（双目相机、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”，根据从节点创建的数据应该彼此重叠多少（翻译的感觉很怪）。但是举了一个例子：当机器人运动快并且相机视场小的时候，这个值应该被提高，来保证连续节点的数据有重叠，但是设置太大会占用内存和耗费时间。对于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同理于视觉相机。
视觉里程计
贴个大图，很清晰的结构，有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优化
-
位姿更新（pose update）
-
关键帧和特征图更新（key frame and feature map update）