陈文佑，章 伟，史晓帆，宋 芳

(上海工程技术大学 机械与汽车工程学院,上海 201620)

同时定位与地图构建(Simultaneous Localization and Mapping，SLAM)[1]是指安装了特定传感器的智能体在环境先验信息未知的情况下，能够在运动过程中建立周围环境地图，并同时估计自己运动的一种算法[2]。SLAM技术可以应用在自动驾驶、机器人、无人机、VR/AR等领域，是未来人工智能时代的关键技术之一。SLAM按照传感器的不同，分为激光SLAM和视觉SLAM。激光SLAM已较为成熟，而视觉SLAM技术目前仍还有很多问题尚未解决。

自20世纪80年代以来，视觉SLAM就已经被计算机视觉和机器人领域的科研人员所关注。经过几十年的发展，目前已形成了基于特征法和直接法的两种SLAM方法。视觉SLAM所用到的传感器分为单目相机、双目相机和RGBD相机。在目前国内外的视觉SLAM方案中，比较著名的算法有基于直接法的大范围单目定位与制图[3](Large-Scale Direct Monocular SLAM，LSD-SLAM)方法、使用RGB-D 摄像头的 RGBD-SLAM[4]方法、基于点线特征的VSLAM算法PL-SLAM[5](Points and Line Segments SLAM)以及基于特征点的ORB-SLAM[6](Oriented FAST and Rotated BRIEF-SLAM)方法等。ORB-SLAM算法鲁棒性良好，建图误差及运算复杂度较低，自提出以来就一直是研究者们重点关注的对象。但是ORB-SLAM算法建立的是稀疏点云图，只保留了图像中特征点的一部分作为关键点，固定在空间中进行定位，难以描绘地图中障碍物的存在。故本文对单目ORB-SLAM算法进行改进，在ORB-SLAM框架基础上，提出一种改进的半稠密三维重建算法，并将建立的地图同ORB-SLAM的建图效果进行对比。本文利用德国慕尼黑工业大学计算机视觉实验室发布的TUM数据集进行仿真实验，证明本文所提方法能够改进ORB-SLAM的建图效果。

1 ORB-SLAM算法原理

1.1 ORB特征

ORB-SLAM算法最大的特点是所有步骤均使用ORB[7]特征。ORB特征是一种快速的特征提取方法，它改进了FAST检测子不具有方向性的问题，并采用速度较快的二进制描述子BRIEF[8]，加快了整个图像特征的提取。相较于SIFT(Scale-Invariant Feature Transform)[9]和SURF(Speeded Up Robust Feature)[10]特征，ORB特征在特征点的提取速度上有显著提升，且不需要用GPU加速就可以满足SLAM所需的实时性。同时，使用统一的ORB特征也有助于SLAM算法在特征提取与追踪、关键帧选取、三维重建、闭环检测等步骤中具有内生的一致性。图1为一张图像提取ORB特征点的效果图。可以看出，采用FAST的角点检测算法在角点比较密集的地方，例如树枝和汽车，可检测到较多的特征点，点云也更丰富；在角点较稀疏的地方，例如草坪和路面，特征点就比较少，点云稀疏，这就是ORB的主要特点。

图1 OpenCV提供的ORB特征点检测结果Figure 1.ORB feature point detection results provided by OpenCV

1.2 ORB-SLAM算法

ORB-SLAM共有3个线程，分别是跟踪(Tracking)、局部建图(Local Mapping)和闭环控制(Loop Closing)。

图2给出了ORB-SLAM算法建图的详细流程。跟踪线程主要用ORB算法提取图像帧中的特征点，同时确定相机位姿，并决定插入新关键帧的时机。局部建图线程负责关键帧的插入，并处理从关键帧中得到的地图点，再进行局部BA(Bundle Adjustment)，使得重投影误差最小，最后剔除冗余的关键帧。闭环控制线程包括闭环检测和闭环修正：闭环检测负责检测所有关键帧是否形成闭环并计算累积误差；闭环修正负责减小累积漂移量。详细步骤如下：

图2 ORB-SLAM算法框架Figure 2.ORB-SLAM algorithm framework

步骤1跟踪线程。ORB算法在8层金字塔模型上提取FAST角点，确保特征点均匀分布，设定角点阈值，根据保留的FAST角点计算方向和ORB特征描述子。然后，通过前一图像帧估计相机的初始位姿，并通过全局重定位来初始化位姿。在当前帧和局部帧地图之间找到更多的匹配点对，来优化当前帧的位姿。最后，通过筛选冗余关键帧来决定当前帧是否可以作为关键帧；

步骤2局部建图线程。首先更新互视图，添加一个关键帧节点Ki，检查与Ki有共同云点的其他关键帧，用边线连接。计算表示该关键帧的词袋，并利用三角法生成新的地图云点。三角化的云点必须经过地图点云筛选测试，保证留下的云点都是能被跟踪的。通过对互视图中连接的关键帧Kc中的ORB特征点进行三角化实现新的地图云点的创建。然后对当前处理的关键帧Ki，以及在互视图中与Ki连接的其他关键帧Kc进行局部BA。为了使重构保持简洁，构建局部地图时应尽量检测并删除冗余的关键帧；

步骤3闭环控制线程。闭环控制线程抽取最后一帧局部地图关键帧Ki用于检测和闭合回环。为了获得候选回环，必须检测3个一致的候选回环(互视图中相连的关键帧)。得到候选回环后，需要计算从当前关键帧Ki到回环关键帧KI的相似变换，以获得回环的累积误差。计算相似变换也可以作为回环的几何验证。回环修正融合重复的地图云点，在互视图中插入与回环相关的新边缘。融合过程中所有的关键帧将会更新它们在互视图中的边缘，创建的新边缘将用于回环检测。为了有效地闭合回环，通过本质图优化位姿图，将回环闭合的误差分散到图像中去。优化过后，每一个地图云点都根据关键帧的校正进行变换。

2 SURF算法原理

由于传统ORB算法使用的是FAST算法来检测图像中的角点，而FAST最大的问题在于无法保证检测到的角点具有尺度信息，因此特征点不具有尺度不变性。当图像中特征点的尺度发生较大的变化时，传统ORB算法会产生非常大的误匹配，匹配效果较差，如图3所示。单目视觉SLAM的特征点容易发生较大的尺度变化，因此为提高特征点匹配精度，需要采取一些措施增加ORB算法的尺度不变性。

图3 传统ORB算法在尺度发生变化时的特征点匹配状况Figure 3.Matching status of feature points of traditional ORB algorithm when the scale changes

本文在传统ORB算法的基础上，引入SURF算法。SURF算法的一个重要特性是创建了尺度空间。本文提出的算法结合SURF算法的尺度空间思想，使得改进ORB算法具有一定的尺度不变性。

尺度空间理论是指在处理图像信息时引入一个尺度参数，通过图像尺度的变换，获得不同尺度下的一系列图像，然后在这一系列图像上进行相应的处理[11]。Hessian矩阵是SURF算法的核心，构建Hessian矩阵的目的是为了生成图像稳定的边缘点(突变点)，为后面的特征提取做好基础。每一个像素点都可以求出一个Hessian矩阵，如式(1)所示。

(1)

Hessian矩阵的判别式为

(2)

当Hessian矩阵的判别式取得局部极大值时，则判定当前点是比周围邻域内其他点更亮或更暗的点，由此来定位关键点的位置。

在SURF算法中，图像像素l(x,y)即为函数值f(x,y)。但是由于特征点需要具备尺度无关性，所以在进行Hessian矩阵构造前，需要对其进行高斯滤波。本文选用二阶标准高斯函数作为滤波器。

L(x,t)=G(t)·I(x,t)

(3)

通过特定核间的卷积计算二阶偏导数，即可得出H矩阵的3个矩阵元素Lxx、Lxy、Lyy，从而计算出H矩阵。

(4)

由于高斯核是服从正态分布的，从中心点向外，系数越来越低。为了提高运算速度，SURF使用了盒式滤波器来近似替代高斯滤波器。盒式滤波器(Boxfilter)将图像的滤波转化成计算图像上不同区域间像素和的加减运算问题，只需要查找积分图就可以完成。

每个像素的Hessian矩阵行列式的近似值为

(5)

在Dxy上乘以一个加权系数0.9，目的是为了平衡因使用盒式滤波器近似所带来的误差。

3 改进ORB-SLAM算法

3.1 半稠密建图线程

本文在ORB-SLAM算法基础上应用单目高精度半稠密建图算法[12]，在局部建图线程后添加半稠密建图线程，与闭环控制线程同步进行，建立较为直观的基于特征法的SLAM半稠密三维点云地图。算法增加部分的框架如图4所示。半稠密建图线程的详细步骤如下：

图4 半稠密建图线程框架Figure 4.Semi-dense mapping thread framework

步骤1在N个相邻关键帧上，沿极线搜索高梯度区域的每一个像素，得到N个逆深度假设；

步骤2每个逆向深度假设采用高斯分布表示，并在匹配过程中考虑图像噪声、时差和模糊度；

步骤4由文献[13]提出，在逆深度地图上应用一个过渡的步骤，以便于一个像素可以和它临近的像素进行平均。如果一个像素的逆深度分布与它临近的像素不兼容，则剔除该像素；

步骤5在相邻关键帧计算出它们各自的逆深度地图后，跨越相邻关键帧来检查每个像素深度的一致性以剔除异常值。最后通过优化使深度更加精确。

3.2 改进ORB特征提取

本文将图2 ORB-SLAM算法框架中跟踪线程的ORB特征提取部分改为上文中具有尺度不变性的ORB特征提取算法。SURF算法通过改变模板的大小来获得不同的尺度，因此本文改进的ORB算法结合了SURF的特征。本文采用积分图像和盒氏滤波器进行卷积，通过高斯核函数构建尺度空间并求解快速Hessian响应提取特征点，并在特征点的3×3×3邻域内，将Hessian矩阵处理的每个像素与其三维空间内邻域的26个点进行比较，若为最大值或最小值，则保留。然后，采用三维线性插值法[14]得到亚像素级别的特征点，此时的特征点具备尺度不变性。最后，使用ORB描述子对具备尺度不变性和旋转不变性的特征点进行描述，得到二进制描述符。

通过上述步骤，机器人在运动过程中就可以使用单目摄像头获取周围环境的图像，并通过对图像进行改进ORB特征检测，获取具备尺度不变性的特征点作为环境特征信息，将更加稳定、精确的特征点输入到半稠密ORB-SLAM算法中。

4 仿真实验与分析

本文使用TUM RGBD数据集基准[15]测试改进算法的性能，进行室内环境下改进ORB-SLAM算法三维建图的仿真。TUM RGBD数据集能够客观评价算法的科学性，并对比不同算法之间的性能。为验证算法的一致性，本文选用了具有代表性的数据集freiburg1_xyz和freiburg2_xyz。在freiburg1_xyz中，Kinect相机(实验中未使用该相机深度通道所提供的信息)沿着相机主轴平移运动，方向保持固定。该数据集运行时间短，数据量小，较为简便，比较适用于算法调试以排除故障。在freiburg2_xyz中，Kinect相机在地面坐标系x、y、z3个方向上沿着相机主轴缓慢移动。相机的缓慢移动消除了数据中的运动模糊和滚动快门现象，能够提高建图效果。

本文仿真实验平台为一台Intel(R)Core(TM)i5-7300U CPU @ 2.60 GHz，8 GB内存的计算机，安装有Ubuntu16.04版本Linux操作系统。在ROS(机器人操作系统)环境下运用C++编程语言实现改进的半稠密算法。原ORB-SLAM算法与本文提出的改进算法均广泛使用了计算机的线程并行计算能力，加快了算法的运行速度。

图5为本实验中数据集freiburg1_xyz与freiburg2_xyz的建图效果。图中第1、第2行分别为在数据集freiburg1_xyz和freiburg2_xyz中的实验结果。在第1列图片中，采用具有尺度不变性的ORB特征点检测算法检测动态图像的角点。第2列图片为ORB-SLAM算法根据检测到的特征点建立的稀疏三维点云图。第3列图片为改进ORB-SLAM算法建立半稠密三维点云图。通过第2列和第3列图片的对比可以看出，ORB-SLAM算法能基本建立小场景环境的三维点云地图，但是点云非常稀疏，而且没有颜色特征，难以分辨地图的真实形态。本文提出的改进ORB-SLAM算法使用文献[12]中提出的半稠密建图方法，并引入具有尺度不变性的ORB算法，使得改进ORB-SLAM算法建立的三维点云图能明显分辨出物体的形状特征，生成更加直观的三维地图。

图5 数据集freiburg1_xyz与freiburg2_xyz的建图效果Figure 5.The mapping effect of freiburg1_xyz and freiburg2_xyz datasets

本实验将ORB-SLAM算法、半稠密建图算法和改进的ORB-SLAM 算法分别在数据集freiburg1_xyz与freiburg2_xyz上运行。实验结果表明，ORB-SLAM算法在freiburg1_xyz中运行时长为34.96 s，比该序列本身时长多4.87 s；半稠密算法由于在ORB-SLAM的基础上增加了半稠密建图线程，相比ORB-SLAM的时长增加了1.59 s；改进ORB-SLAM算法既包括了半稠密建图线程，改进了ORB特征匹配，相比半稠密算法的时长增加了0.68 s，时长增加量在可接受范围内。时长对比如表1所示。

在特征匹配精度方面，ORB-SLAM算法和半稠密算法包含相同的跟踪线程，即具有相同的ORB特征提取方法，故特征匹配精度一致，在序列freiburg1_xyz中精度均为83.8%。改进ORB-SLAM算法的ORB特征匹配具有尺度不变性的特点，精度达到88.9%。在实验中，数据集freiburg1_xyz与freiburg2_xyz的主要区别在于前者时长短，运行速度快，检测到的正确匹配点较少，而后者时长较长，相机运动缓慢，能够检测到更多的正确匹配点，所建的地图场景更加丰富，点云更加稠密。由表1和表2可以看出，两个序列的实验结果有相同规律，证明了本文改进算法的一致性。

表1 实时运行时长对比Table 1. Comparison of real-time running time /s

表2 特征匹配精度对比Table 2. Comparison of feature matching accuracy /%

5 结束语

数据集仿真实验表明，相比传统ORB-SLAM算法，本文提出的改进ORB-SLAM算法能够建立效果更好的半稠密三维点云地图，并提高ORB特征点的匹配精度，且运行时长的增加量在可接受范围内。但是本文所研究的半稠密三维点云地图仍存在很多缺陷，并不能完全复现真实环境中的物体。稠密三维点云地图更加贴近于真实环境，也有助于后期语义SLAM[16]的研究，这也是今后的研究重点。