一种基于光度信息和ORB特征的建图SLAM

李晨玥， 张雪芹， 曹 涛

（1. 华东理工大学信息科学与工程学院，上海 200237；2. 上海空间智能控制技术重点实验室，上海 201109；3. 上海航天控制技术研究所，上海 201109）

同步定位与建图（Simultaneous Localization and Mapping，SLAM）通常是指在机器人或者其他载体上通过传感器数据进行采集和计算，得到对其自身位置姿态定位和场景地图信息的系统。一个经典的基于视觉传感器的Visual SLAM一般包括前端视觉里程计、后端优化、回环检测、建图4个模块。前端视觉里程计模块用于估计相邻图像之间相机的位姿变换并得到局部地图；后端优化模块对相机位姿及局部地图信息进行优化得到全局一致的轨迹和地图；回环检测模块用于检测传感器是否又回到之前的位置，优秀的回环检测可以有效减少累计漂移误差；建图模块则根据之前得到的跟踪全局地图建立所需要的地图。

经典的视觉里程计采用的方法主要有基于特征的方法和直接法。基于特征的方法通过提取特征以及特征匹配得到图像帧间的几何对应关系，优化重投影误差来估计传感器位姿。早期，基于特征的方法主要侧重于特征点的提取与匹配。其中，由Mur-Artal等[1]提出的ORB-SLAM2是一个同时支持单目、双目及深度相机的基于特征点的SLAM系统，它使用三线程结构取得了很好的跟踪和建图效果，一定程度上保证了轨迹与地图的全局一致性。由于基于特征点的方法不足以提供有效的约束且在低纹理场景中容易失败，不少学者开始尝试结合线、平面、边缘等其他特征。Pumarola等[2]提出了一个同时结合了点和线特征的SLAM系统，该系统在低纹理环境下表现稳定。Hsiao等[3]提出的KDP-SLAM(Keyframebased Dense Planar SLAM)是一种基于关键帧密集平面匹配的SLAM系统，显著地降低了漂移误差。Li等[4]提出了利用点、线、面的结构特性包括线与线之间的平行性和相交性、点线之间的共面性来跟踪相机运动，并在低纹理环境下得到了更高的系统稳定性和较为精确的跟踪结果。

直接法通过最小化图像中像素之间的光度误差来估计传感器的运动信息，一方面能够弥补基于特征的方法在某些特征缺失场景下失败的问题，另一方面能够直接得到稠密的地图显示。LSD-SLAM(Large-Scale Direct Monocular SLAM)[5]是Engel等提出的第一个不需要计算特征点且能构建半稠密地图的单目SLAM系统，但该系统对相机内参及曝光非常敏感，在快速运动时容易丢失。他们后期又提供了一个更为稳健、准确度更高的包含光度校准的直接法视觉里程计[6]，但这个系统不包括回环检测，无法降低累积误差。Gao等[7]在这个直接视觉里程计的基础上加入基于词袋的回环检测功能，减少了累积误差。

基于特征的方法在特征丰富的区域准确率较高且计算代价较小，但在特征点缺失的无纹理场景下容易失败。而直接法不需要特征点，可以在特征缺失的场合下使用，但依赖于灰度不变性，易受光照影响。因此，有不少学者尝试将这两种方法结合以得到对环境更为鲁棒的系统。Younes等[8]提出了FDMO(Feature Assisted Direct Monocular Odometry)方法，它在直接法出现故障的场景,如大基线时使用ORB特征进行位姿估计，提升了直接法系统的鲁棒性。Lee等[9]提出了一种半直接法，以直接法跟踪局部每帧的相机运动，以基于ORB特征的图优化方法优化跟踪关键帧的相机运动，得到了一个准确性和鲁棒性且速度上都有提高的SLAM系统。这两种SLAM系统都是单目的混合SLAM系统，无法得到稠密的三维地图。Nicola等[10]提出以libviso2特征法作为双目LSD-SLAM的运动估计先验，得到了一个精度较高且较为稳定的双目SLAM系统，但它使用特征法作为SLAM系统的前端位姿估计，在特征缺失的环境中可能会跟踪失败。

在环境勘探工程中，机器人常在较空旷环境及非光滑表面行进，通过双目或深度相机对环境进行勘探，并需要通过采集到的图片及视频对三维地形进行重建。针对SLAM系统因为相机抖动、非匀速的运动、快速的旋转及场景结构单一、纹理简单等问题而导致跟踪失败的情况，本文提出了一个鲁棒的具有建图功能的R-ORB SLAM系统。该系统的创新点主要体现在两方面：（1）该系统基于ORB-SLAM2系统，同时采用一种轻量级的位姿估计作为特征视觉里程计的先验，在跟踪失败的情况下，将先验位姿的结果参与位姿估计；（2）通过相机模型计算得到关键帧点云，通过对每个关键帧点云进行拼接得到全局点云地图，使用VoxelGrid滤波器[11]进行下采样，得到稠密三维点云地图；使用Poisson算法[12]实现表面重建，得到三维Mesh地图。该系统充分利用了图片的光度和深度信息，提高了系统的准确率和鲁棒性。

1 ORB-SLAM2系统

1.1 系统流程

ORB-SLAM2系统是基于ORB特征的特征法SLAM。前端通过高斯金字塔模型提取均匀的ORB特征点，保证ORB特征的旋转不变性和尺度不变性。

在ORB-SLAM2系统的双目模式和RGB-D模式中，特征关键点由3个坐标xs=(uL,vL,uR) 表示，其中 (uL,vL) 为该特征关键点在左图（RGB-D相机中的RGB图像）中的像素坐标，uR为右边图像中的水平像素坐标，在RGB-D模式下，uR是一个虚拟的坐标，可由式(1)得到。

其中：fx为相机水平焦距；b为相机基线，d为对应像素深度信息。根据传统相机模型可以将特征点映射到三维空间中。

其中： ρ 为鲁棒Huber损失函数； Σ 为与关键点尺度相关的协方差矩阵； χ 为所有匹配点；投影函数 π(·)可以由式(3)表示：新加入的每一个关键帧都要与之前的关键帧做回环检测。ORB-SLAM2的回环检测基于词袋模型。

1.2 运动估计中的李代数

2 R-ORB SLAM系统

ORB-SLAM2系统在相机发生抖动（图1(a)）、以及特征点数量较少、场景空旷简单（图1(b)）的情况下，极易发生跟踪失败的问题，而其系统自带的重定位功能在发生跟踪失败时，只能回到之前经过的某个位置重新启动跟踪。这样，当相机在新的场景中持续运动时则会彻底跟踪失败。针对该问题，本文提出了一种将光度信息和几何特征相结合的R-ORB SLAM系统。

2.1 系统流程

图1 ORB-SLAM，系统易失败场景Fig. 1 Failure scenarios for ORB-SLAM2 system

R-ORB SLAM系统框图如图2所示。首先对输入的图片序列进行图片矫正、金字塔分层模型及特征提取等预处理操作，然后根据重定位、运动模型或者本文提出的基于光度信息的轻量级位姿估计方法（Roughly Pose Estimation，RPE）得到初始位姿对前一帧进行跟踪，最后根据优化得到的位姿结果生成地图。在建图模块中，保留ORB-SLAM2系统的建图线程用于跟踪，另外使用点云拼接线程拼接每个关键帧点云得到三维点云地图，对得到的点云地图使用VoxelGrid滤波器进行下采样滤波，对滤波后的点云使用Poisson表面重建算法得到三维地图模型。R-ORB SLAM算法主要适用于RGB-D深度传感器。

2.2 轻量级位姿粗估计

RPE模型提取前一帧图像梯度大于阈值的像素，并假设灰度不变，即假设一个空间点在各个视角下成像的灰度不变。将前一帧的任意一点通过与特征法相似的投影方法投影至当前帧平面，此时优化的不再是重投影误差而是光度误差，公式如下：

其中：K为相机内参矩阵；Xi∈R3为与前一帧匹配成功的地图点；xi为后一帧图像平面的对应点；I(·) 表示该 点 的光度；Z2为Xi经 过 e xp(ξ∧) 变换 后 第二帧相机坐标系下的Z轴坐标。

图2 R-ORB SLAM系统流程图Fig. 2 Flow chart of R-ORB SLAM system

使用该式计算优化问题的雅可比矩阵，然后使用高斯牛顿法或者Levenberg-Marquardt方法计算增量迭代求解。

2.3 权重模型

使用RPE模型作为位姿估计模型，与运动模型一起为ORB-SLAM2系统提供先验。

ORB-SLAM2系统中的初始位姿是由运动模型和重定位得到的，系统根据该初始位姿将地图点重投影至图像平面，在投影得到的位置附近寻找匹配点，并在该初始位姿的基础上进行优化。当传感器违背了运动模型做不规律的运动时，使用运动模型得到的初始位姿不能较好地为后续优化提供先验，导致跟踪失败问题。通常这样的情况并不会发生在相机的整个运动路线中，而只是局部发生。针对这种情况，结合2.2节的轻量级位姿粗估计，设计了一个权重模型，将ORB-SLAM2系统得到的不够准确的位姿和RPR模型得到的位姿相结合，得到一个估计值，作为跟踪失败时当前帧的位姿，并在此插入关键帧以防止后续的跟踪失败。

2.4 点云拼接建稠密地图

ORB-SLAM2系统只保留一些在跟踪过程中被反复检测并优化的特征三维点，简单增加跟踪过程中的地图点，不仅会降低跟踪的速度，还会增加一些可能错误的约束导致优化失败。本文将建图和跟踪分离开来，由建图线程单独进行建图。

使用点云拼接的方式建立稠密的点云地图。由跟踪线程得到每个关键帧的位姿和与前一个关键帧的变换矩阵，使用该变换矩阵进行关键帧点云的拼接。每个关键帧的点云都可由深度图和RGB图以及常规针孔相机模型得到，由RGB图中任意一个二维点x(u,v) 得到当前帧点云中点P(X,Y,Z,R,G,B) 的计算 公式，如式(15)所示：

其中：s是尺度因子，即深度图里给的数据与实际距离的比例；R、G、B是该点的颜色信息。

由于ORB-SLAM2系统采用多关键帧的策略，如果采用每个关键帧的点云进行拼接，会导致大量点云的冗余问题。因此，本文使用VoxelGrid滤波器对每个关键帧的点云进行下采样，在保持点云几何特征的同时，减少点的数量。该滤波器通过输入的点云数据创建一个三维体素栅格，在每个体素内，用体素中所有点的重心来表示其他点。

当全局优化线程结束后，可得到一个全局的、经过滤波的三维点云，使用Poisson表面重建算法可以得到一个mesh类型的三维模型，后续颜色的渲染可通过MeshLab软件[14]进行。

3 实验结果与分析

在两个流行的数据集TUM RGB-D[15]和ICLNUIM[16]中开展实验，评估R-ORB SLAM系统的性能，并将R-ORB SLAM系统与其他SLAM系统进行比较。实验在内存为8 GB，CPU为Intel® Core™ i7-8 750H，携带了GTX 1 060的笔记本电脑上进行。由于多线程会造成实验结果的非确定性，实验在每个视频序列上运行5次，得到平均轨迹精度。使用绝对轨迹均方根误差 (Absolute Trajectory Root-Mean-Square Error，ATE RMSE)、失败帧率 (Failure Ratio, FR)、表面重建误差 (Surface Reconstruction Error, SRE)评估SLAM系统的跟踪精度、鲁棒性和重建精度。ATE RMSE通过计算估计位姿与真实位姿之间的欧式距离，得到绝对轨迹误差的均方根误差；FR表示跟踪失败的图片帧在整个图片序列中所占的比例；SRE由计算重建表面的每一点到最近真实3D模型的表面的平均距离得到。实验中，R-ORB SLAM系统中的Nthre取ORB-SLAM2系统中的默认阈值。

3.1 TUM RGB-D数据集

TUM RGB-D数据集包含来自RGB-D传感器的室内序列，这些序列分组在几个类别中，包括手持式采集类及机器人采集类等，以评估不同纹理，不同运动速度、照明和结构条件下的SLAM方法。该公开数据集基准还提供了许多有用的工具，可用于预处理数据集和评估SLAM系统跟踪结果。

本文从TUM RGB-D数据集中选择了几个常用的序列，并与ORB-SLAM2、DVO-SLAM[17]、RGB-D SLAM[18]、Kintinuous[19]、Elastic fusion[20]进行了比较，结果如表1所示。实验结果显示，在几乎每个序列上，R-ORB SLAM系统比ORB-SLAM2系统的跟踪精度都略有提高，与其他系统相比，仅在1个序列上略低于Elastic fusion。

在TUM RGB-D数据集中，fr2/360、fr2/slam、fr2/slam3 图片序列通过安装在Pioneer机器人顶部的Kinect相机拍摄采集得到。由于机器人的运动非匀速且较快，整个环境尺度比办公室场景大，剧烈的抖动以及尺度较大的场景对SLAM系统的稳定性是一个挑战。大多数SLAM系统在这几个序列中都会出现跟踪失败，包括ORB-SLAM2。TUM RGB-D数据集中还提供了用于检测环境结构和纹理对SLAM系统影响的序列。其中，fr3/nostructure_ notexture_far记录了摄像头沿木制平面移动时拍摄的图片序列，几乎无场景结构和纹理特征；fr3/structure_notexture_far记录了一个由木板制成的锯齿形结构而几乎无纹理特征的场景，大部分SLAM系统在这两个场景中都极易跟踪失败。

R-ORB SLAM、ORB-SLAM2和Elastic fusion系统在这5个序列上的失败帧率如表2所示，表中fr3/ns_nt_far、fr3/s_nt_far分别是指fr3/nostr-ucture_notexture_far、fr3/structure_notexture_far序列。从表2可以看出，Elastic fusion和ORB-SLAM2系统重定位可以恢复部分跟踪，但失败帧率仍然较高。本文方法在这5个序列中全部跟踪成功，鲁棒性较ORBSLAM2系统有了显著的提高。

图3示出了R-ORB SLAM系统与ORB-SLAM2系统在这5个序列中的跟踪结果。可以看到，在fr2/360序列中，ORB-SLAM2系统在序列开始不久就由于轻微抖动而跟踪失败，当序列进行至中后半段才重定位成功，而R-ORB SLAM系统跟踪较稳定，且准确度较高。在fr2/slam序列中，由于较高的相机运动速度和高抖动，ORB-SLAM2系统在部分路径中跟踪完全失败，R-ORB SLAM系统则全部跟踪成功，但可以看出，由于相机抖动导致了一定的跟踪误差。

表1 TUM RGB-D数据集上的绝对轨迹均方根误差比较Table 1 Comparison of ATE RMSE on TUM RGB-D dataset

表2 TUM RGB-D数据集的失败帧率比较Table 2 Comparison of failure ratio on TUM RGB-D dataset

图3 跟踪结果比较Fig. 3 Comparison of tracking results

在fr2/slam3序列中，ORB-SLAM2系统在路线的后半段，因为相机的突然旋转加速和抖动，从而跟踪失败，而R-ORB SLAM系统全程跟踪成功，但仍有一定程度的漂移。在fr3/nostructure_notexture_far和fr3/structure_notexture_far序列上，ORB-SLAM2系统运行时，在序列开始就由于无足够的特征地图点而导致初始化失败，而R-ORB SLAM系统则会使用RPE得到的粗略位姿作为结果，启动并维持跟踪。

图4为ORB-SLAM2系统和R-ORB SLAM系统在fr2/360序列上的效果示例图。图4(a)中ORBSLAM2系统由于相机的轻微抖动，已跟踪失败，而R-ORB SLAM系统仍有部分特征点跟踪成功，如图4(b)所示。

3.2 ICL-NUIM数据集

ICL-NUIM数据集[16]通常用于对RGB-D、视觉测距和SLAM算法进行基准测试，是一个对评估表面重建质量很重要的数据集。它包括由手持深度相机拍摄的起居室和办公室两个场景，每个场景都提供真实测量位姿，但起居室场景还提供了一个真实的3D模型以供评估表面重建的质量。

本文从ICL-NUIM数据集中选择了4个序列，并将R-ORB SLAM系统与其他SLAM系统的跟踪精度进行比较，结果如表3所示。

图4 fr2/360序列上的效果示例图Fig. 4 Effect diagrams on fr2/360 sequence

从表3可以看出，虽然R-ORB SLAM系统的ATE RMSE误差在4个序列中并不总是最低，但其平均误差最小，仍然具有较高的精度和稳定性。

为了进一步考察表面重建的精度，在ICL-NUIM的起居室数据集上，将本文系统和其他系统进行了比较，结果如表4所示。

实验中，将VoxelGrid滤波器网格设置为0.05×0.05×0.05。表4中Ours+TSDF表示将R-ORB SLAM系统得到的定位结果，使用TSDF（Truncated Signed Distance Function）[21]算法进行重建。在实验中TSDF中的体素也划分为0.05×0.05×0.05。

表3 ICL-NUIM数据集上的ATE RMSE比较Table 3 Comparison of ATE RMSE on the ICL-NUIM dataset

表4 ICL-NUIM数据集的表面重建误差（SRE）比较Table 4 Comparison of surface reconstruction error on the ICLNUIM dataset

从表4可以看出，R-ORB SLAM系统的重建模块在kt1、kt2序列中并不是效果最好的，但在kt0和kt3序列中得到了很好的重建精度，平均表面重建精度最小。而简单地应用TSDF算法并没有得到很好的重建结果。图5示出了本文方法在kt0序列上重建的结果。图5(a)为kt0序列真实3D模型中的一个表面；图5(b)为未使用Poisson表面重建算法时RORB SLAM系统得到的稠密点云；图5(c)为使用Poisson表面重建算法的重建结果；图5(d)为使用Ours+TSDF的重建结果。由于视角和设定的坐标问题，重建的表面看起来与真实模型是镜像的。从图5可以看到，使用TSDF算法可以得到更光滑且平整的表面，但由于使用了体素划分，在本应圆滑的表面，如枕头、台灯表面，却无法显示原本的弧度，丢失了部分细节，如右下角的盆栽，而本文提出的直接融合点云的方法则能够得到贴近物体原型的重建模型。

4 结束语

本文针对RGB-D 传感器提出了一个使用光度信息和ORB特征相结合的R-ORB SLAM系统，可以有效减少系统因为相机抖动、非匀速运动、快速旋转以及场景结构单一、纹理简单等问题而导致的跟踪失败的情况。同时，通过在系统中加入拼接、滤波等点云处理，并采用Poisson表面重建算法实现了地图三维重建。在公开数据集上的实验结果显示，RORB SLAM系统在跟踪和重建上都有较好的精度和稳定性。在本文的重建部分，由ORB-SLAM2得到的稀疏地图仅用于跟踪，而实际上该地图上的点拥有更准确的位置信息。未来，考虑将这些地图点的信息利用起来得到更精确的重建效果。

图5 kt0序列的某一角度重建结果Fig. 5 Reconstruction results of kt0 sequence from one perspective