基于ROS机器人的3D物体识别与三维重建(五）ORB-SLAM2与RTAB-MAP三维重建

相关的代码资料：
https://github.com/Rayso777（后续会陆续整理上传）
视频：
1、ElasticFusion TUM数据集＆buntu16.04+kinect2演示流程.
2、RTAB-MAP实时三维重建-Kinect2
3、RTAB-MAP三维重建-基于gazebo仿真
4、ORB-SLAM2室内稀疏三维重建-基于gazebo仿真
5、ORB-SLAM2室外三维重建-gezebo仿真
6、三维重建视频集锦-视觉SLAM

1 引言

智能机器人大多具备路径规划、跟随、避障和人机交互等功能，而这些上层功能离不开底层SLAM技术支持,通过SLAM构建环境地图和估计机器人的在环境的位置，这对其功能实现具有重要意义。在第四章，我们对视觉SLAM中的一些基本概念和理论进行了介绍。这一章，我们将研究目前主流的SLAM算法进行研究，探讨如何通过视觉SLAM技术进行地图重建的。

目前，对于通过 vSLAM 技术的地图重建可分为稀疏建图和稠密建图两大类。

稀疏
2015年提出的ORB-SLAM是稀疏重建中最具代表性的算法之一，并且也是特征点法建图中最具影响力的算法。然而第一代算法主要是基于单目相机，随后提出的 ORB-SLAM2 算法框架在第一代的基础上加上了双目和RGB-D相机。因此，本节我们将以 ORB-SLAM2 算法为代表探讨稀疏重建。
稠密
RTAB（Real Time Appearance-Based）是2013年开源的一中算法，随着近年的改进，该算法将里程计的框架独立了出来，可以支持相机、激光雷达等不同的传感器，而且可以构建二维的栅格地图和三维的稠密地图，并且该算法包括了视觉里程计、优化、回环检测和建图各个环节，十分具有代表性，因此我们将以 RTAB-MAP 为代表研究稠密的地图重建。

最后，在 ROS 的仿真环境中对两种算法进行实验测试。

2 ORB-SLAM2框架

2.1 简介

ORB-SLAM2 算法是基于ORB-SLAM改进的算法，添加了全局BA的环节，并且增加了适配的相机种类，适用于室内外环境和不同规模的重建问题，而且对快速剧烈的运动有很好的鲁棒性。下图展示了该算法完整流程，包括了追踪、局部建图、回环检测三个主要线程和随后的全局优化，全局BA是在第一代的框架上拓展对全局地图点和位姿进行优化的部分。接下来，本文将对三个主要的线程实现的机理分别进行介绍。

2.2 追踪线程 Tracking

首先在系统输入中，定义了对单目、双目和深度相机数据读取的函数，并提取图像的ORB特征，构造 Frame 对象进行封装，然后将数据流接入追踪线程。追踪线程主要就是完成对特征的预处理、位姿初始化、局部地图点和位姿估计和创建关键帧的功能。其中位姿初始化时十分重要的模块，这一部分会确定世界坐标系，并关乎后续内容的准确度，因此我们先对这一部分进行详细介绍。

该线程接收到 Frame 数据后，会首先判断数据类型，并采用不一样的初始化方式。针对单目相机，主要采用对极几何和三角量测方法确定位姿然,主要计算基础矩阵和单应性矩阵两种不同的数学模型，综合两种的得分进行结果选择，确定初始化位姿，然后通过全局优化的方法优化初始的位姿解。对于双目和RGB-D相机由于可以获取深度信息，将主要采用ICP的方法对位姿进行初始化再进行全局BA进行优化。

在初始化好第0帧到第1帧的相机位姿和路标点后，我们便可以正式进入追踪线程。

如上图所示，对后续获取到的Frame进行ORB特征的提取（过程会用到图像金字塔），然后根据这些特征对之前的帧进行识别，若匹配到后则使用恒速运动模型估计下一帧的位姿和上一帧相同，投影后进行重投影误差优化，如果没有匹配到，则进行重定位，与之前事先构建好的字典中的签名（由单词向量组成）匹配，寻找共视图中最多边的帧，这也就是词袋加速特征匹配的环节。找到符合的关键帧后将该帧的位姿赋予新的帧，如果此位姿下投影得到的路标内点数量十分少，便认定此次重定位失败，开始新的重定位环节。若内点的数量满足要去，便将该位姿下的 MapPoint进行投影，寻找更多约束完成优化，便完成了重定位环节。

在局部地图模块，主要将相机观测到的当前帧同之前构建的局部地图中的路标点进行匹配（只限当前帧的视野范围内，一般取60°），观测到后作为约束进行优化。追踪线程的最后一个模块是关键帧的生成，这里主要是通过一些人为设定的规则进行的。重定位模或者距离上次插入关键帧的20帧后需要生成关键帧，追踪到20个点以上的帧需要生成，参考帧中含有本次当前帧后去的路标点数目小于百分之九十则也创建关键帧。

2.3 局部建图

局部建图线程的流程如上图所示，将新的关键帧添加到共视图和生成树，计算BoW关键帧的单词更新关键帧数据库。为了避免误匹配和错误的三角化，我们还需要剔除冗余的路标点。同时，新插入的关键帧存在某些特征点没有被匹配到，这时候在共视图中查找其他关键帧的路标进行投影，映射到当前关键帧的路标点（在加入前需要进行三角化、极线约束、重投影等检验过程）。随后，对新的局部地图和位姿进行局部优化（主要优化新加入帧附近有共视关系的帧数据）。最后，为了保证不让系统冗余，也会进行关键帧的剔除操作，主要去掉观测路标点九成在其他帧中出现过的关键帧。

2.4 回环检测

展示了回环检测线程包含的五个模块。第一步，先进行候选关键帧的检测。通过计算当前关键帧和共视图中每一帧的相似度，确定最低的相似度。随后在关键帧数据库中获取相似度大于最低相似度并且共视图中未出现的关键帧作为候选，应当具备该帧附近连续三帧相似度相近。因为在回环处某些特征会被连续几帧共同观察到。第二步，进行位姿（以李群的形式）计算。我们通过词袋模型进行对当前帧和候选关键帧进行匹配，再通过RANSAC和ICP结合的方法求解位姿，在获得的位姿下将路标定进行投影，根据获得的内点数目判断是否需要进行重新投影。若内点数目不够，则重新匹配求解位姿，直到内点数目满足要求，便认为帧便是检测到的回环。第三步是进行回环融合，利用回环帧求得的位姿将当前帧进行漂移纠正。

后将闭环的关键帧及其附近的共视图中的关键帧中路标点投影到当前帧下进行重新匹配融合。第四步便是对本征图（简化后的共视图，也叫做生成树）优化，主要利用本征图的约束进行位姿优化，而对三维路标点并不进行改变。

最后，在完成回环检测纠正以后，我们会将本征图优化后的关键帧的位姿与三维路标点进行全局的优化。全局BA也是单独的一个线程，这里我们不在进行单独介绍，这一步完成后，所有的位姿和路标点都会得到相应的调整完善。

3 RTAB-MAP框架

相较于现在研究的大多基于理论的算法，RTAB更具有落地的价值，现在已经可以较好的应用于产品。从下图算法框架流程图可以看出，该算法的灵活性十分强。在传感器的数据获取方面，它可以接收深度相机、双目相机、TF、里程计、激光雷达点云数据等。在建图类型上，可以进行八叉树、点云地图、二维栅格地图等的构建，这是一种稠密建图的方法。我们可以对这些选项进行选择配置，对各种类型的任务可以很好适配。

由该算法的流程图可以看出，包含了视觉里程计、回环检测、后端图优化、全局优化等内容，这和大多视觉SLAM算法的框架构成是一样的。在 ORB-SLAM 2 中已经对视觉SLAM建图的各个环节进行了探讨，在 RTAB中我们不再对各个线程模块详细叙述。该算法的实时性比较好的主要原因便是引入了内存管理的机制，以便于适用于长时间大规模的环境重建。所以，接下来我们主要对内存管理机制进行详细介绍。

其中Odometry是一个空余的输入模块，可以接收不同传感器类型的数据输入RTAB系统。Synchronization是一个帧率同步模块，将各种数据信息频率同步到相同的时间戳，随后输入到STM。图中的WM（working memory）模块代表工作内存，STM( short-term memory)模块接收传感器信息进行处理为其他模块提供服务。LTM（long-term memory）模块代表长期内存，主要用于存放不需要进行运算的数据。

该算法的核心思想是，在闭环检测过程中使用尽量少的定位路标点，而在需要时可以对整个地图的定位路标点进行检测访问。当定位路标点的数目较多使得匹配的时间增长到某个阈值，我们便将形成闭环可能性低的路标点从WM中转移到LTM中，在之后的运算中不再计算这些路标点，于是可以提高效率。而当需要时，我们可以将某关键帧的邻近帧的路标点从LTM中提取出来。对于判断闭环相似性，该算法通过对某定位路标定的访问频率进行量化的，访问次数多，该点的相似性权重便高。所以，我们便将权重低并且存在时间长的路标点转换到LTM模块中。STM便是依据这个原理计算相似性权重的。

4 基于Gazebo的仿真实验

4.1 Turtlebot机器人

在第三章曾使用Turtlebot机器人在Gazebo创建的环境中实现物体识别。本章将使用该机器人完成在仿真环境中的稀疏和稠密的三维重建。在实验之前，先了解Turtlebot机器人。第一代Turtlebot机器人发布于2010年，它包括了Kinect相机、激光雷达、IMU等传感器，可以获取多种类型的传感器信息。小巧方便，价格低廉，一直是ROS发展过程中最重要的机器人之一。

并且，ROS官方提供了该机器人的软件仿真包，方便了研究人员在没有机器人设备的情况下也可完成相应的工作。它包含了机器人的驱动和主要仿真模块，十分便于开发者自己编写各种导航、交互控制的算法接入，加速研发。在第三章对Kinect相机仿真运行进行了介绍，本章将在其基础上编写键盘控制节点，发布速度话题消息/cmd_vel，控制Turtlebot在RVIZ和Gazebo中进行运动。控制节点内容如下图所示：

4.2 ORB-SLAM2仿真实验

（1）创建Gazebo室内外仿真环境

基于Turtlebot_world空环境，添加中科院数字博物馆的室内模型，不同于一般的墙面重复纹理贴图，该室内仿真环境中墙面上有大量的人物和文字介绍照片， 所以对ORB特征提取带来很大的便处。该室内模型包括了墙面的海报贴图、瓷砖、铝制的牌匾等，可以模拟真实世界的大多材质，具体内容如下图所示。最后，将该仿真环境保存为indoor.world文件，方便之后通过launch重映射机制加载该仿真环境。

同样的，基于Gazebo空环境，添加学校、操场、喷泉、垃圾桶和松树等室外的模型构建室外模拟环境，如下图所示，并保存为outdoor.world文件。

（2）ORB-SLAM2配置

随后，基于ORB-SLAM2开源算法将其修改完善和适配在ROS系统中。编译之前，需要安装Opencv（用于图像处理）、Eigen（用于矩阵运算）、Pangolin（基于OpenGL的图形化显示）等工具。

因为该算法最初是用于双手持相机运动进行地图重建的，若在使用在机器人的仿真重建上，需要将订阅的话题消息修改为Gazebo中机器人发布的话题，最后，通过shell脚本进行自动化的编译。

（3）实验结果

1、室内环境仿真
加载indoor.world室内仿真环境、运行键盘控制节点和ORB-SLAM2算法，通过移动室内外环境中的机器人，最终得到建好的特征点地图。重建过程界面如下图所示：

上图展示了gazebo机器人仿真环境、Kinect相机视野、终端键盘控制节点和ORB-SLAM2实时建图界面。重建结果如下图所示：

图中，黑色点为map中确定好的路标点，红色点是还可以进行优化的点。可以观察到，该算法在室内环境中对于瓷砖、海报和铝材等不同表面提取ORB特征点漂移较小，都有比较好的鲁棒性。下图展示了机器人在建图过程中的运动轨迹，其中蓝色点为添加的关键帧，绿色连线代表共视图（某关键帧与其他关键帧若观察到相同特征点视为共视）。

上图展示了机器人运动到起点处时进行回环检测纠正运动轨迹的偏移。通过两个图中的对比的轨迹明显发生了偏移，这是由于累积误差引起的。而在第二图中，可以看到通过回环检测，将该偏移的轨迹拉到了正确的轨迹上。

由上图特征点的提取对比可看出，该方法对于纹理要求比较高，左侧特征点数量集中在柱子的角点和之上的图片上，纯墙面的数量明显较少。因此该方法也有一定的局限性，在真正的应用当中应当考虑其应用的场景纹理丰富程度。
（2）室外环境仿真

上图展示室外仿真环境的过程，同室内仿真实验相同，展示了outdoor.world仿真环境和实时建图等界面。通过终端键盘控制节点在室外环境中移动机器人，最后创建的特征点地图如下图所示：

RTAB可以对室外景物进行特征点提取，在室外也可以进行很好的应用。但明显可以看出，室外结果的噪点比较多，而且轨迹也存在偏移，这是由于重建环境中可识别的物品距离较远，在视觉转换时会存在跟踪丢失的情况，因而重建效果降低。可看出，ORB-SLAM2算法对重建环境的纹理丰富程度要求比较高，需要大量可以识别的特征点帮助机器人估计自身的位姿和运动轨迹，对弱纹理环境应用时需要着重注意。

4.3 RTAB-MAP 仿真实验

（1）通过gazebo创建一个新的室内封闭环境，如下图所示。

（2）RTAB-MAP算法已经可以在许多商用机器人中应用，具有很好的拓展性，拥有很多的开源资料供研究者学习，并且还具有ROS的功能包。首先需要配置RTAB_ROS功能包，随后启动机器人的键盘控制节点和RTAB算法程序，通过键盘控制Turtlebot在环境中移动，进行三维重建。创建过程如下图所示。

（3）在 RVIZ 中的最终创建结果如下，同gazebo的仿真环境相对比可看出，该算法恢复了室内环境的大部分纹理信息。三维重建的结果如下图所示，由过程的图片可以看出RTAB-MAP算法出该算法是基于二维栅格地图基础构建三维纹理地图。

==下篇博客节介绍ElasticFusion实时重建 ==