面向半稠密三维重建的改进单目ORB-SLAM

周 彦，旷鸿章，牟金震，王冬丽，刘宗明

1.湘潭大学 自动化与电子信息学院，湖南 湘潭411105

2.上海航天控制技术研究所，上海201109

目前同时定位与地图构建（Simultaneous Localization and Mapping，SLAM）技术是机器人、自动驾驶、增强现实等领域的关键技术之一。定位与地图构建是自主移动平台导航与控制领域的两个基本问题，SLAM技术恰好是同时解决这两个问题的最有效解决方案。人们在生产生活中对自主移动产品与日俱增的需求也使得SLAM技术成为自主移动平台领域研究的热点问题。

回溯SLAM技术的发展历程，早期主要研究基于概率估计的SLAM，例如拓展卡尔曼滤波[1]、粒子滤波[2]和组合滤波[3]。文献[4]中以扩展卡尔曼滤波构图与定位（Extended Kalman Filter-SLAM，EKF-SLAM）为基础，对该拓展卡尔曼滤波SLAM 算法进行了改进以实现机器人的定位与地图创建。之后基于关键帧的单目视觉SLAM逐渐发展起来，自从Klein和Murray在2007年提出了具有开创性的并行跟踪与制图（Parallel Tracking And Mapping，PTAM）方法[5]以来，现已经有许多研究者提出其他实时视觉SLAM（visual SLAM，vSLAM）方法，包括基于特征点的ORB-SLAM[6]（Oriented FAST and Rotated BRIEF-SLAM）方法，以及基于直接法的大范围单目定位与制图[7]（Large-Scale Direct Monocular SLAM，LSD-SLAM）方法和使用RGB-D摄像头来同时定位与制图的RGBD-SLAM[8]方法。国内研究者在文献[9]中对vSLAM在无人平台上的研究进展做了深刻地论述。

一个好的SLAM 算法应该具有在复杂环境下进行准确处理、算法计算复杂度要求尽可能低、实时性好、能产生精确的结果等优点，而且要在运算条件和传感器价格低的条件下尽可能产生稠密的、细节丰富的地图。Mur-Artal 等于2015 年提出的ORB-SLAM 方法是目前最先进的算法之一。ORB-SLAM具有远超其他方案的稳健性，当然也存在一些不足之处，如构建的稀疏特征点地图只能满足对定位的需求，而无法提供机器人导航、避障和交互等诸多功能。为了尽可能实现这些优点，本文将详细论述在多个算法基础上重新设计的一个新算法，此算法拥有上述多种优点，在多个数据集上取得了很不错的实验效果。

图1 改进ORB-SLAM方法的框架

1 回顾单目ORB-SLAM方法

单目ORB-SLAM方法能从由单目摄像头获取到的连续视频中估计关键帧的位姿以及由这些关键帧计算得到大量的地图点。该基于特征的SLAM 方法选择了ORB 特征进行精确跟踪与构建精简的地图，同时使用基于DBoW2[10]的词袋向量模型去服务于全局重定位和闭环检测。

ORB-SLAM方法框架由跟踪线程（Tracking）、局部建图线程（Local Mapping）和回环检测线程（Loop Closing）组成。跟踪线程负责对每帧图像的相机位姿进行定位，并决定何时插入新的关键帧。局部建图线程负责处理新的关键帧，并对新插入的关键帧周围的关键帧对应的相机位姿进行局部捆集约束（BA）[11]，同时也负责剔除冗余关键帧。回环检测线程判断新插入的关键帧是否使得所有关键帧形成闭环，如果闭环被检测到则进一步计算累积误差并减小累积漂移量。

2 改进ORB-SLAM方法的框架

本文在ORB-SLAM 算法上进行改进，使其能对场景进行半稠密地图重建。如图1中各个阶段所示，图中红色与绿色虚线框出的区域为原来的单目ORB-SLAM方法部分，图中蓝色与绿色虚线框出的区域为在ORBSLAM 方法的基础上新增的三个部分。本文方法由如下四部分构成。

（1）原来的单目ORB-SLAM方法：获得关键帧信息与一系列地图点。

（2）关键帧再剔除算法（KRC）：将上一阶段得到的关键帧再剔除一遍。

（3）直线段提取算法：提取剔除冗余关键帧后的关键帧中的直线段。

（4）三维直线段重建算法：通过匹配各个关键帧中提取到的直线段来重建场景的三维模型。

3 基于ORB-SLAM方法三维重建

3.1 关键帧再剔除

首先由ORB-SLAM方法实时提供关键帧信息和计算得到的大量地图点。通过分析与实验，如果直接使用这些关键帧将导致计算复杂度大、不确定性高和信息冗余等问题。本文在文献[12]中的关键帧选择算法的基础上设计了一种使用帧间平移向量和旋转向量的范数和（运动度）作为参考值来再次裁剪关键帧的算法，将其称为关键帧再剔除算法。通过计算帧间运动度来考虑是否将该关键帧用于后续三维重建线程。

相机运动是一个刚体运动，在三维空间坐标系下可用旋转矩阵R和平移向量t描述相机的旋转与平移。旋转矩阵R可由欧式空间中的一个四元数q表示，式（1）中θ代表相机绕单位向量n旋转的角度。

如图2 所示，单位四元数q1将向量p0旋转到p1，q2将向量p0旋转到p2。p0对应的c0是相机的初始帧光心，p1和p2对应的c1和c2是两个旋转后的相机的光心。c0到c1的平移向量是t1,c0到c2的平移向量是t2,c1到c2的平移向量是Δt=t2-t1，Δt即为两关键帧之间的平移向量。由公式推导单位四元数把p1旋转到p2,q2q1-1定义如式（2），式中‖nx,ny,nz‖=1，角度θ=2×arccosω∈(0,2)，表示为两个关键帧之间的旋转角度。

图2 单目相机运动关系示意图

KRC 算法的思想是使用当前帧相对于参考帧的平移量Δt和旋转角度θ的范数和（运动度）来度量摄像机的运动程度。如公式（3）定义，max_norm为相机最小运动阈值，max_norm为相机的最大运动度阈值，只有在当前帧相对参考帧的运动度处于上面两阈值之间才把该帧视为关键帧，否则直接剔除该帧。在本文中根据实际经验，min_norm和max_norm分别取值0.4和5。

3.2 直线段提取

本文使用Topal等人提出的实时直线段检测（EDLines）方法[13]来检测KRC 算法剔除掉冗余帧后的所有关键帧的直线段。EDLines由边缘描绘（ED）算法[14]、直线段提取以及直线段验证三个步骤构成。接下来简单描述这几个步骤。

整个LS 提取算法包括三个阶段：首先输入灰度图像，使用边缘检测算法产生一组干净的、连续的边缘图。第二步，利用直线段拟合方法从边缘图中提取直线段。最后，遵循赫姆霍兹原理来验证直线段。

3.3 三维直线段场景重建

ORB-SLAM方法能持续地给后续进程输入关键帧信息，记做St帧关键帧K。同时获取了当前地图中的地图点Mt和关键帧的位姿信息Tt。在经过3.1 节的KRC算法后，被剔除去冗余帧，保留下来的关键帧为Kt。

整个三维重建方法流程是一个大的循环过程，假设在t1、t2时刻获得的关键帧Kt1与Kt2。由关键帧的性质可知Kt1与Kt2有很大一部分是相同的，所以3.2 节的直线段提取算法只需对新增关键帧Knew={Kn1,Kn2,…,Kni} (Knew⊆Kt2但Knew⊄Kt1) 进 行 直 线 段 提取。将Knew中每一帧输入到直线段提取算法进行直线段检测。算法运行至此处时就拥有Kt2帧内的所有直线段、地图点Mt和位姿信息Tt。将以上信息输入到由Hofer 等人提出的基于直线段提取有效三维场景（Line3D++）算法[15]，就能重建得到一系列精确的三维直线数据。下面简单介绍该三维重建算法的步骤，具体原理请参阅文献[15]中算法。

（1）建立不同图像线段之间的潜在对应关系。

（2）基于相邻视图的支持度来评估这些对应关系。

（3）为每个二维直线段选择最合理的对应结果作为其3D位置假设。

（4）基于三维空间邻近关系对二维线段进行聚类。

（5）使用组合束调整（BA）优化结果[可选]。

表1 KRC算法参与与否对改进ORB-SLAM算法性能影响对比

4 实验结果与分析

为了评估新算法的性能，接下将使用TUM RGB-D数据集基准[16]将本文新提出的改进ORB-SLAM方法与当前最先进的几个SLAM方法进行比较，包括PTAM算法[5]、LSD-SLAM[7]和RGBD-SLAM[8]。该数据集与基准可支持对算法的科学评价和客观比较。通过实验即可记录在统一基准下本文方法与其他方法关键帧定位轨迹的绝对误差（ATE）。在实验中，本文提出的方法不使用TUM RGB-D数据集的深度通道的信息。

测试条件为一台普通笔记本电脑，配备Intel®CoreTMi7-3520 CPU @ 2.9 GHz，8 GB内存与Ubuntu 14.04操作系统，本文提出的改进算法由C++编写实现，同时尽可能多地使用了计算机的并行计算能力。如在三维直线段重构阶段，就多次使用到OpenMP这一种共享内存并行系统的多处理器程序设计方案。而且原来的ORBSLAM 部分与本文新增部分也都采用了线程并行计算方式运行。

为了证明本文提出方法性能的良好一致性，且能重建效果出色的半稠密三维场景，实验选取三个主流的测试视频序列：TUM RGB-D 数据集中的fr2_desk、fr3_nstr_tex_near 和fr3_str_tex_near,并与LSD-SLAM运行结果进行比较，几个有代表性序列的测试结果展示在图3中，本文方法和LSD-SLAM方法都能构建场景的半稠密地图，本文的方法半稠密场景由直线段重建，LSD-SLAM 方法则由点云构成，而单目ORB-SLAM 方法构建的地图为稀疏的点云地图。实验表明该改进的方法能产生由直线段重建的高质量地图，跟踪建图的稳健性能比LSD-SLAM方法要好。

图3 三维重建效果对比

为了分析本文提出的KRC算法对改进ORB-SLAM算法性能影响，将KRC 算法参与与否对整个方法性能的对比数据记录于表1中。从表中可见，在运行同一个序列的情况下，不执行KRC 算法时会减小整个方法三维重建的循环次数，但是执行KRC算法后，关键帧数目会大量减小。另外，本文提出的改进的ORB-SLAM 方法在不执行KRC 算法时运行一遍约需要166 s，而执行KRC 算法时约需要173 s。同时注意到不执行KRC 算法，会使得单次循环处理的视频帧数量变得很大，致使单次直线段匹配过程十分耗时。KRC算法参与与否对改进ORB-SLAM算法在同一数据集上共计运行时长相差不大，但重建的循环次数在执行KRC算法时增大，从而能使得改进算法的每次循环时间缩短，进而加快了三维地图重建的刷新频率。

如表2所示，对本文提出的方法的关键帧定位精度进行评估。由于程序进程某些阶段的随机性，例如初始化、位置优化和直线段重建等，所有实验运行11 次，然后报告记录实验结果的中位数。表中显示了执行5 个测试序列的关键帧轨迹的均方根误差（RMSE）中位数。请注意，本文提出的方法在大多数序列中不断提高了LSD-SLAM的轨迹精度。除了1个序列外，其他在所有序列中都能产生最佳结果，PTAM在这方面的表现稍好一些。然而，PTAM变得不那么可靠，因为其中2个测试序列失去了跟踪。而且LSD-SLAM 也在1 个测试中丢失位姿。实验结果表明本文提出的方法的鲁棒性要强于其他三种SLAM方法。

表2 关键帧绝对定位精度对比cm

为了评估本文所提出的方法的实时性能，表3中给出了改进的ORB-SLAM 方法与原来的ORB-SLAM 方法实验条件下的运行时长对比和数据集拍摄时长数据。从表中可以看出本文改进的方法在5 个数据集上运行时间都较原来的ORB-SLAM 方法要长，这主要是改进的方法不仅运行了基于特征点的ORB-SLAM 方法，而且还在不降低定位精度的前提下构建了半稠密的三维场景地图。受限于计算条件，如普通的笔记本电脑的低计算能力、没有使用图形处理器（GPU）算力等条件，虽然本文方法的运行时间对比各个序列的视频采集时间都有一定的延长，但是在较短的时间内构建的场景包含了丰富的直线段信息，而原来的单目ORB-SLAM方法只能构建稀疏的点云地图。

表3 实时运行时长对比s

5 结束语

本文提出一种用于半稠密三维重建的改进单目ORB-SLAM 算法。实验表明该算法能生成精确的、定位良好的直线段，且能高效地匹配和聚类直线段来进行半稠密三维场景重建。为了方便更好地解释该改进的单目ORB-SLAM 方法的工作原理，读者可以在GitHub项目托管平台上下载本文提出的SLAM 方法的C++代码。开源项目网址为https：//github.com/BTREE-C802/3DLine-SLAM。

目前，改进的方法只能对二维直线段进行三维重建。未来，计划持续改进该方法使其能对圆弧进行三维重建，因为使用直线段与圆弧的组合几乎可以构造出所有常见事物的三维结构。此外，对于局部特定目标的三维重建将是今后的一个研究方向。