目前同时定位与地图构建（Simultaneous Localization and Mapping，SLAM）技术是机器人、自动驾驶、增强现实等领域的关键技术之一。定位与地图构建是自主移动平台导航与控制领域的两个基本问题，SLAM技术恰好是同时解决这两个问题的最有效解决方案。人们在生产生活中对自主移动产品与日俱增的需求也使得SLAM技术成为自主移动平台领域研究的热点问题。
回溯SLAM技术的发展历程，早期主要研究基于概率估计的SLAM，例如拓展卡尔曼滤波[1]、粒子滤波[2]和组合滤波[3]。文献[4]中以扩展卡尔曼滤波构图与定位
面向半稠密三维重建的改进单目ORB-SLAM
周彦1，旷鸿章1，牟金震2，王冬丽1，刘宗明2
1.湘潭大学自动化与电子信息学院，湖南湘潭411105
2.上海航天控制技术研究所，上海201109
摘要：构建更详细的地图以及估计更精准的相机位姿一直都是同时定位与地图构建（Simultaneous Localization And Mapping，SLAM）技术所追求的目标，但是以上目标与实时性要求、较低的计算代价和受限的计算资源条件是相矛盾的。提出一种在单目ORB-SLAM（Oriented FAST and Rotated BRIEF-SLAM）方法的基础上利用关键帧中提取到的直线特征进行半稠密三维重建的方法。由ORB-SLAM实时
提供一组关键帧及其对应的相机位姿信息和一系列地图点，提出一种关键帧再剔除算法进一步减少冗余帧数目，使用直线段提取方法提取各帧中的直线段，使用纯几何约束方法对以上检测得到的直线段进行匹配，生成一个由直线段构成的半稠密三维场景模型。实验结果表明新方法持续稳定的运行，能在低计算代价条件下快速地在线三维重建。
关键词：单目视觉；半稠密；三维重建；同时定位与地图构建；关键帧再剔除
文献标志码：A中图分类号：TP242.6doi：10.3778/j.issn.1002-8331.1912-0198
Improved Monocular ORB-SLAM for Semi-den3D Reconstruction
ZHOU Yan1,KUANG Hongzhang1,MU Jinzhen2,WANG Dongli1,LIU Zongming2
1.School of Automation and Electronic Information,Xiangtan University,Xiangtan,Hunan411105,China
2.Shanghai Aerospace Control Technology Institute,Shanghai201109,China
Abstract：Building more detailed3D maps and estimating more accurate camera pos have always been the goal pur-sued by Simultaneous Localization And Mapping（SLAM）technology.The above goals are contradictory to real-time require-ments,low computational cost,and limited computing reso
urce of SLAM.A novel mi-den3D reconstruction method bad on the monocular Oriented FAST and Rotated BRIEF-SLAM（ORB-SLAM）by using the line gment features extracted from keyframes is propod.Specifically,the improved method builds upon ORB-SLAM,which first provides a ries of map points and a t of keyframes and their corresponding camera pos information in real-time.A Keyframe Re-Culling（KRC）algorithm is propod to further reduce redundant keyframes.A line gment extraction algorithm is employed to extract line gments in each keyframe.By adopting a purely geometric constraints method to match2D line gments from different keyframes to generate a mi-den3D scene model.Experimental results show that the novel method runs steadily and reliably,and can quickly perform online3D reconstruction with low computational cost.
Key words：monocular-vision;mi-den;3D reconstruction;Simultaneous Localization And Mapping（SLAM）;key-frames re-culling
基金项目：国家自然科学基金（61773330，61100140，61104210）；湖南省自然科学基金（2017JJ2253）；上海市科学技术委员会项目（19511120900）；湖南省教育厅科研项目（17B259）
。
（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 方法
单目ORB-SLAM 方法能从由单目摄像头获取到的连续视频中估计关键帧的位姿以及由这些关键帧计算得到大量的地图点。该基于特征的SLAM 方法选择了ORB 特征进行精确跟踪与构建精简的地图，同时使用
基于DBoW2[10]
的词袋向量模型去服务于全局重定位和闭环检测。
醉赤壁吉他谱ORB-SLAM 方法框架由跟踪线程（Tracking ）、局部建图线程（Local Mapping ）和回环检测线程（Loop Closing ）组成。跟踪线程负责对每帧图像的相机位姿进行定位，并决定何时插入新的关键帧。局部建图线程负责处理新的关键帧，并对新插入的关键帧周围的关键帧
对应的相机位姿进行局部捆集约束（BA ）[11]
，
同时也负责剔除冗余关键帧。回环检测线程判断新插入的关键帧是否使得所有关键帧形成闭环，如果闭环被检测到则进一步计算累积误差并减小累积漂移量。
2改进ORB-SLAM 方法的框架
本文在ORB-SLAM 算法上进行改进，使其能对场
景进行半稠密地图重建。如图1中各个阶段所示，图中
地图点
视觉词典识别数据库
关键帧生成树
地图点共视图
红色与绿色虚线框出的区域为原来的单目ORB-SLAM 方法部分，图中蓝色与绿色虚线框出的区域为在ORB-SLAM方法的基础上新增的三个部分。本文方法由如下四部分构成。
（1）原来的单目ORB-SLAM方法：获得关键帧信息与一系列地图点。
（2）关键帧再剔除算法（KRC）：将上一阶段得到的关键帧再剔除一遍。
（3）直线段提取算法：提取剔除冗余关键帧后的关键帧中的直线段。
（4）三维直线段重建算法：通过匹配各个关键帧中提取到的直线段来重建场景的三维模型。
3基于ORB-SLAM方法三维重建
3.1关键帧再剔除
首先由ORB-SLAM方法实时提供关键帧信息和计算得到的大量地图点。通过分析与实验，如果直接使用这些关键帧将导致计算复杂度大、不确定性高和信息冗余等问题。本文在文献[12]中的关键帧选择算法的基础上设计了一种使用帧间平移向量和旋转向量的范数和（运动度）作为参考值来再次裁剪关键帧的算法，将其称为关键帧再剔除算法。通过计算帧间运动度来考虑是否将该关键帧用于后续三维重建线程。
相机运动是一个刚体运动，在三维空间坐标系下可用旋转矩阵R和平移向量t描述相机的旋转与平移。旋转矩阵R可由欧式空间中的一个四元数q表示，式（1）中θ代表相机绕单位向量n旋转的角度。
q=é
ëêùûú
cosθ2,n sinθ2T
（1）
如图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即为两关键帧之间的平移向量。由公式推导单位四元数q2q-11把p1旋转到p2,q2q-11定义如式（2），式中
n x,n y,n z=1，角度θ=2×arccosω∈()
0,2π，表示为两个关键帧之间的旋转角度。
q2q-11=[]
ω,x,y,z=é
ëê
ù
ûú
cosθ2,sinθ2()
n x,n y,n z（2）KRC算法的思想是使用当前帧相对于参考帧的平移量Δt和旋转角度θ的范数和（运动度）来度量摄像机的运动程度。如公式（3）定义，max_norm为相机最小运动阈值，max_norm为相机的最大运动度阈值，只有在当前帧相对参考帧的运动度处于上面两阈值之间才把该帧视为关键帧，否则直接剔除该帧。在本文中根据实际经验，min_norm和max_norm分别取值0.4和5。min_norm≤ Δt+min( θ,2π- θ)≤max_norm（3）3.2直线段提取
本文使用Topal等人提出的实时直线段检测（EDLines）方法[13]来检测KRC算法剔除掉冗余帧后的所有关键帧的直线段。EDLines由边缘描绘（ED）算法[14]、直线段提取以及直线段验证三个步骤构成。接下来简单描述这几个步骤。
整个LS提取算法包括三个阶段：首先输入灰度图像，使用边缘检测算法产生一组干净的、连续的边缘图。第二步，利用直线段拟合方法从边缘图中提取直线段。最后，遵循赫姆霍兹原理来验证直线段。
3.3三维直线段场景重建
ORB-SLAM方法能持续地给后续进程输入关键帧信息，记做S t帧关键帧K。同时获取了当前地图中的地图点M t和关键帧的位姿信息T t。在经过3.1节的KRC算法后，被剔除去冗余帧，保留下来的关键帧为K t。
整个三维重建方法流程是一个大的循环过程，假设在t1、t2时刻获得的关键帧K t1与K t2。由关键帧的性质可知K t1与K t2有很大一部分是相同的，所以3.2节的直线段提取算法只需对新增关键帧K new={K n1, }
K n2,⋯,K ni(K new⊆K t2但K new⊄K t1)进行直线段提取。将K new中每一帧输入到直线段提取算法进行直线段检测。算法运行至此处时就拥有K t2帧内的所有直线段、地图点M t和位姿信息T t。将以上信息输入到由Hofer等人提出的基于直线段提取有效三维场景（Line3D++）算法[15]，就能重建得到一系列精确的三维直线数据。下面简单介绍该三维重建算法的步骤，具体原理请参阅文献[15]中算法。
（1）建立不同图像线段之间的潜在对应关系。
（2）基于相邻视图的支持度来评估这些对应关系。
4实验结果与分析
为了评估新算法的性能，接下将使用TUM RGB-D
数据集基准[16]将本文新提出的改进ORB-SLAM 方法与当前最先进的几个SLAM 方法进行比较，包括
PTAM 算法[5]、LSD-SLAM [7]和RGBD-SLAM [8]。该数据集与基准可支持对算法的科学评价和客观比较。通过实验即可记录在统一基准下本文方法与其他方法关键帧定位轨迹的绝对误差（ATE ）。在实验中，本文提出的方法不使用TUM RGB-D 数据集的深度通道的信息。
测试条件为一台普通笔记本电脑，配备Intel ®Core TM
i7-3520CPU @2.9GHz ，8GB 内存与Ubuntu 14.04操作系统，本文提出的改进算法由C++编写实现，同时尽可能多地使用了计算机的并行计算能力。如在三维直线段重构阶段，就多次使用到OpenMP 这一种共享内存并行系统的多处理器程序设计方案。而且原来的ORB-SLAM 部分与本文新增部分也都采用了线程并行计算方式运行。
为了证明本文提出方法性能的良好一致性，且能重建效果出色的半稠密三维场景，实验选取三个主流的测试视频序列：TUM RGB-D 数据集中的fr2_desk 、fr3_nstr _tex _near 和fr3_str _tex _near，并与LSD-SLAM
运行结果进行比较，几个有代表性序列的测试结果展示在图3中，本文方法和LSD-SLAM 方法都能构建场景的半稠密地图，本文的方法半稠密场景由直线段重建，LSD-SLAM 方法则由点云构成，而单目ORB-SLAM 方法构建的地图为稀疏的点云地图。实验表明该改进的
方法能产生由直线段重建的高质量地图，跟踪建图的稳健性能比LSD-SLAM 方法要好。
为了分析本文提出的KRC 算法对改进ORB-SLAM 算法性能影响，将KRC 算法参与与否对整个方法性能的对比数据记录于表1中。从表中可见，在运行同一个序列的情况下，不执行KRC 算法时会减小整个方法三维重建的循环次数，但是执行KRC 算法后，关键帧数目会大量减小。另外，本文提出的改进的ORB-SLAM 方法在不执行KRC 算法时运行一遍约需要166s ，而执行KRC 算法时约需要173s 。同时注意到不执行KRC 算法，会使得单次循环处理的视频帧数量变得很大，致使单次直线段匹配过程十分耗时。KRC 算法参与与否对改进ORB-SLAM 算法在同一数据集上共计运行时长相差不大，但重建的循环次数在执行KRC 算法时增大，从而能使得改进算法的每次循环时间缩短，进而加快了三维地图重建的刷新频率。
如表2所示，对本文提出的方法的关键帧定位精度进行评估。由于程序进程某些阶段的随机性，例如初始化、位置优化和直线段重建等，所有实验运行11次，然后报告记录实验结果的中位数。表中显示了执行5个测试序列的关键帧轨迹的均方根误差（RMSE ）中位数。请注意，本文提出的方法在大多数序列中不断提高了LSD-SLAM 的轨迹精度。除了1个序列外，其他在所有序列中都能产生最佳结果，PTAM 在这方面的表现稍好一些。然而，PTAM 变得不那么可靠，因为其中2个测试序列失去了跟踪。而且LSD-SLAM 也在1个测试中丢失位姿。实验结果表明本文提出的方法的鲁棒性要强于其他三种SLAM 方法。
为了评估本文所提出的方法的实时性能，表3中给
出了改进的ORB-SLAM 方法与原来的ORB-SLAM 方
TRACKING （a ）本文方法个人自评
（b ）ORB-SLAM 方法
表1
KRC 算法参与与否对改进ORB-SLAM 算法性能影响对比
测试项目不执行KRC 算法执行KRC 算法
循环次数弥留之际是什么意思
70113
关键帧数目（max ）
15220
图像匹配平均值/s 1.9076720.735344
最大值/s 5.1170941.817225
三维直线段重建平均值/s 0.3763020.445758
最大值/s 1.0184071.240161
共计运行时长/s 173.109286166.219708
表2英文句子简短
关键帧绝对定位精度对比
注：X 表示跟踪失败，—表示复现失败。
cm
方法LSD-SLAM PTAM RGBD-SLAM 本文方法
fr1_xyz 9.001.151.340.90
fr2_desk 4.57X 9.500.88
fr3_long_office 38.53X —3.45
新驾考fr3_nstr_tex_near 7.542.74—1.39
fr3_str_tex_near X 1.04—1.58
两小无猜是什么意思
法，而且还在不降低定位精度的前提下构建了半稠密的三维场景地图。受限于计算条件，如普通的笔记本电脑的低计算能力、没有使用图形处理器（GPU）算力等条件，虽然本文方法的运行时间对比各个序列的视频采集时间都有一定的延长，但是在较短的时间内构建的场景包含了丰富的直线段信息，而原来的单目ORB-SLAM 方法只能构建稀疏的点云地图。
5结束语
本文提出一种用于半稠密三维重建的改进单目ORB-SLAM算法。实验表明该算法能生成精确的、定位良好的直线段，且能高效地匹配和聚类直线段来进行半稠密三维场景重建。为了方便更好地解释该改进的单目ORB-SLAM方法的工作原理，读者可以在GitHub 项目托管平台上下载本文提出的SLAM方法的C++代码。开源项目网址为https：///BTREE-C802/ 3DLine-SLAM。
实习律师
目前，改进的方法只能对二维直线段进行三维重建。未来，计划持续改进该方法使其能对圆弧进行三维重建，因为使用直线段与圆弧的组合几乎可以构造出所有常见事物的三维结构。此外，对于局部特定目标的三维重建将是今后的一个研究方向。
参考文献：
[1]QUAN M，PIAO S，TAN M，et al.Accurate monocular
visual-inertial SLAM using a map-assisted EKF approach[J].
arXiv：1706.03648，2017.
[2]SIM R，ELINAS P，GRIFFIN M，et al.Design and analysis
of a framework for real-time vision-bad SLAM using Rao-Blackwellid particle filters[C]//The3rd Canadian Conference on Computer and Robot Vision，2006.
[3]LUO Jingwen，QIN Shiyin.A fast algorithm of SLAM
bad on combinatorial interval filters[J].IEEE Access，
2018，6：28174-28192.
[4]郭文县，高晨曦，张智，等.基于特征稀疏策略的室内机器
人SLAM研究[J].计算机工程与应用，2017，53（16）：110-115.
[5]KLEIN G，MURRAY D.Parallel tracking and mapping
for small AR workspaces[C]//6th IEEE and ACM Inter-national Symposium on Mixed and Augmented Reality，2007：1-10.
[6]MUR-ARTAL R，MONTIEL J M M，TARDOS J D.ORB-
SLAM：a versatile and accurate monocular SLAM sys-tem[J].IEEE Transactions on Robotics，2015，31（5）：1147-1163.
[7]ENGEL J，SCH T，CREMERS D.LSD-SLAM：large-scale
direct monocular SLAM[C]//European Conference on Com-puter Vision，2014.
[8]ENDRES F，HESS J，STURM J，et al.3-D mapping with
an RGB-D camera[J].IEEE Transactions on Robotics，2014，30（1）：177-187.
[9]陈世浪，吴俊君.基于RGB-D相机的SLAM技术研究综
述[J].计算机工程与应用，2019，55（7）：35-44.
[10]GÁLVEZ-LÓPEZ D，TARDÓS J D.Bags of binary words
for fast place recognition in image quences[J].IEEE
Transactions on Robotics，2012，28（5）：1188-1197. [11]MOURAGNON E，LHUILLIER M，DHOME M，et al.
Generic and real-time structure from motion using local
bundle adjustment[J].Image and Vision Computing，2009，27（8）：1178-1193.
[12]李弋星，刘士荣，仲朝亮，等.基于改进关键帧选择的
RGB-D SLAM算法[J].大连理工大学学报，2017（4）：83-89.
[13]AKINLAR C，TOPAL C.EDLines：a real-time line g-
ment detector with a fal detection control[J].Pattern
Recognition Letters，2011，32（13）：1633-1642.
[14]TOPAL C，AKINLAR C.Edge drawing：a combined real-
time edge and gment detector[J].Journal of Visual
Communication and Image Reprentation，2012，23（6）：862-872.
[15]HOFER M，MAURER M，BISCHOF H.Efficient3D scene
abstraction using line gments[J].Computer Vision and
Image Understanding，2017，157：167-178.
[16]STURM J，ENGELHARD N，ENDRES F，et al.A bench-
mark for the evaluation of RGB-D SLAM systems[C]//
2012IEEE/RSJ International Conference on Intelligent
Robots and Systems，2012.
表3实时运行时长对比
方法
序列时长ORB-SLAM 本文方法fr1_xyz
30.09
34.96
48.86
fr2_desk
99.36
134.51
161.01
fr2_xyz
122.74
198.08
198.72
fr3_nstr_
tex_near
56.48
73.26家谱树
110.89
fr3_str_
tex_near
36.91
51.74
58.50
s
