收稿日期:2020-03-22 修回日期:2020-07-23基金项目：香港创新及科技基金(ITP /053/16LP )
作者简介：李铁维(1996-)，女，硕士,CCF 会员(B 8046G )，研究方向为计算机视觉㊂
李铁维1，王牧阳2，周 炎1
(1.武汉大学遥感信息工程学院，湖北武汉430079;2.香港理工大学建设及环境学院，香港999077)
摘 要:SLAM (即时定位与地图构建)系统是近年来计算机视觉领域的一大重要课题，其中特征法的SLAM 凭借稳定性好㊁计算效率高的优点成为SLAM 算法的主流㊂目前特征法SLAM 主要基于点特征进行㊂针对基于点特征的视觉里程计依赖于数据质量，相机运动过快时容易跟丢，且生成的特征地图不包含场景结构信息等缺点，提出了一种基于点线结合特征的优化算法㊂相较于传统基于线段端点的六参数表达方式，算法采用一种四参数的方式表示空间直线，并使用点线特征进行联合图优化估计相机位姿㊂使用公开数据集和自采集鱼眼影像数据分别进行实验的结果表明，与仅使用点特征的方法相比，该方法可有效改善因相机运动过快产生的跟丢问题，增加轨迹长度，提升位姿估计精度，且生成的稀疏特征地图更能反映场景结构特征㊂ 关键词：计算机视觉；单目视觉里程计；点线结合特征；普吕克坐标；图优化
纯棉纱
中图分类号:TP 242 文献标识码:A 文章编号:1673-629X (2021)01-0048-06doi :10.3969/j.issn.1673-629X.2021.01.009
Monocular Visual Odometry Based on Point and Line Features
LI Tie -wei 1,WANG Mu -yang 2,ZHOU Yan 1
(1.School of Remote Sensing and Information Engineering ,Wuhan University ,Wuhan 430079,China ;
2.Faculty of Construction and Environment ,The Hong Kong Polytechnic University ,Hong Kong 999077,China )
Abstract :In recent years ,SLAM has been an important topic in the field of computer vision.is one of the essential tasks in computer vision area.Among all the algorithms ,feature -based SLAM stands out for its robustness and efficiency ,especially point feature -based SLAM.However ,as the visual odometry based on point features depends on data quality and is difficult to be tracked when the camera moves too fast ,and the map constructed contains little scene structure information ,a method based on point and line combination features is proposed.Rather than the traditional six -par
ameter representation of two end points for lines ,the proposed algorithm applies a four -parameter representation to express space lines ,and uses both point and line features to optimize camera position via graph optimization.Experimental results on public datasets and self -collected fisheye camera image sequences show that compared with methods that only use point features ,the proposed algorithm can effectively make improvements on lost tracking caused by camera moving too fast ,hence increasing the length of trajectory as well as the accuracy of position estimation.The structure of the scene is also better represented in sparse point -line feature map.
Key words :computer vision ;monocular visual odometry ;point and line features ;Plücker coordinate ;graph optimization
0 引 言
视觉里程计是SLAM (即时定位与地图构建)系统的前端部分，其主要任务是增量式地估计相邻图像之间的运动关系[1]㊂根据位姿估计时目标函数的不同构建方式，可分为直接法和特征点法㊂直接法假设影像灰度不变，通过计算最小光度误差估计相机姿态，而特征点法主要通过特征匹配和跟踪，计算最小重投影误差来进行估计的㊂直接法所需计算量更小，而特征 法不易受到光照变化的影响，更为稳定㊂目前最具有代表性的直接法SLAM 是J.Engel 等人提出的LS
D -SLAM [2]㊂LSD -SLAM 针对像素进行，可以构建半稠密的场景地图㊂早期的特征法单目SLAM 是以点特征为基础，使用扩展卡尔曼滤波器进行位姿估计，如A.J.Davison 提出的第一个单目SLAM 系统MonoSLAM [3]㊂Klein 等人提出的PTAM (parallel tracking and mapping )系统[4]第一个使用了非线性优
第31卷 第1期2021年1月 计算机技术与发展COMPUTER TECHNOLOGY AND DEVELOPMENT
Vol.31 No.1
Jan. 2021北大图书馆
化方法，引入了关键帧机制，并实现了跟踪和构图过程并行化，奠定了此后基于特征的单目视觉SLAM算法的雏形㊂PTAM系统的继承者ORB-SLAM[5]则使用具有旋转和缩放不变性的ORB点特征，提出了跟踪㊁局部构图和闭环三线程并行的方案㊂ORB-SLAM的缺点是在仅旋转的轨迹中容易跟丢，且生成的特征稀疏地图无法直观地表现场景特征㊂
点特征简单易表达，但维度单一，缺少约束，形成的特征稀疏地图难以真正表达地理信息，而加入线特征可有效地改善这些缺点㊂Albert Pumarola等人提出的PL-SLAM[6]中，使用线段的两端点表达线
特征，但实际在不同帧中，提取的三维线端点常常沿直线方向产生漂移，因此需要将其与在某帧投影中最匹配线段的端点绑定[7]㊂三维直线也可使用在两平面上的投影进行表达，如Spetsakis[8]和Rother[9]所使用的㊂Chan⁃
draker[10]提出的双目系统利用了线段从四个投影平面出发(包括相邻两帧各从两个视点采集影像)必然交于一线的原理㊂Joan Solà等人[11]总结了基于普吕克坐标的线特征表达方式㊂普吕克坐标可由直线上任意两点齐次坐标的转换得到，且不随所取点的位置而变化，因此避免了由端点漂移带来的不确定因素，是一种更为稳定的直线特征表达方式㊂在普吕克坐标系的基础上,Adrien Bartoli[12]提出了只需四个参数的正交法表达方法，并在Zhang Guoxuan等人提出的双目系统slslam[13]中得到了应用㊂
该文实现了一种基于点线结合特征的视觉里程计算法，在ORB-SAM[5]系统的基础上加入了线特征，并使用了基于图优化(graph optimization)方法估算和优化位姿图，相较于仅包含点特征的系统，提高了相机跟踪的精度和稳定性㊂
1 点线结合的视觉里程计
算法包括四个部分：特征提取与匹配㊁线特征的表达和投影㊁关键帧判定和图优化解算㊂
1.1 特征提取与匹配
算法中，点特征的提取与匹配参考ORB-SAM[5]中介绍的方法㊂对于线特征，算法使用了LSD(line segment detector)[14]方法提取图像线段特征,LSD算法能够以较高的效率提取图像中带有明显地理信息的主要线特征㊂在LSD方法的基础上，使用基于LBD 描述子(line band descriptor)[15]的方法对不同图像中提取的线段进行匹配㊂在处理线段匹配的过程中，加入以下两个筛选步骤，以优化匹配的有效性:(1)对于彩图像，进行RGB多通道匹配，仅有在复数个通道中被匹配的线段可被标记为有效匹配;(2)对每次匹配，使用K最近邻(K nearest-neighbor)方法寻距离仅次于该次匹配的线段，仅当两次匹配的距离差大于设定的阈值时，才将该次匹配标记为有效匹配㊂图像中线段提取和匹配的部分结果如图1所示
㊂
图1 线段提取和匹配结果
(数据集为TUM-Fr1-room手持相机数据，匹配线段用相同编号标注)
每新加入一帧影像，算法首先使用LBD方法提取线段特征，然后与已加入地图的有效线特征进行描述子差异计算，为匹配完成的线特征加入该帧的信息㊂在位姿估计完成后，根据有效匹配点线特征和新加入特征的数量，判定该帧是否作为关键帧加入地图，并把在关键帧新检测到的线段加入地图㊂
随着相机移动，大量线段会在视野中消失，算法为每个线特征L i设置了一个计数器参数ct i，在每次对全图线特征进行匹配之前，将所有线特征对应的ct i加1，匹配结束之后得到匹配的参数减1；当ct i参数累积达到阈值τc，即直线L i已在τc帧影像中不可见时，将直线L i标注为不可见，不再参与之后新加入帧的匹配㊂
1.2 线特征三维表达和投影
不同于点，线特征具有四个自由度，却难以到使用最少参数且完全非奇异的数学表达方法㊂在传统方法中，根据不同情况，一条空间直线可使用两点连线(六参数)㊁平面交线(八参数)㊁普吕克坐标(Plücker coordinate，六参数)或在平面上的投影坐标(六参数)等方法表达，但这些表达方法都存在过
参数化的问题㊂点连线或平面交线的参数法可能会造成光束法平差过程中的数值不稳定；而使用投影坐标的方法可能会需要切换投影平面，因为无法保证所有线段在平面内可见㊂Bartoli和Sturm[12]提出了一种由普吕克坐标演化而来的四参数表达方法，即正交参数法㊂根据Zuo X 等[16]，假设三维直线的普吕克坐标为L，通过空间中两点P1(x1,y1,z1,r1)和P2(x1,y1,z1,r1),P1和P2的非齐次坐标分别表示为P
~
1㊁P~2，则L可表示为式(1): L=
P~
1
×P~2
r1P~1-r2P~
é
ë
ê
êê
ù
û
ú
úú
2
=né
ë
êêù
û
úúv⊂ℝ6(1)其中,v指向直线方向,n指向直线与坐标原点构成平面的垂直方向㊂则线段的正交坐标[12]可表示为式
㊃94㊃
第1期 李铁维等：基于点线结合特征的单目视觉里程计
(2):
C (3×2)=
n ‖n ‖
v ‖v ‖n ×v ‖n ×v [
]
‖
=U σ1000
σ2éëêêêùû
úúú0
(2)
直线的普吕克坐标L 可表示为L T
=(σ1u T
1
|σ2
u T
2)㊂由式(2)，令矩阵W =σ1-σ2σ2
σéëêêù
û
úú1，则U ∈SO(3),W ∈SO(2)㊂此时线段参数可简化为矩阵U 的对数映射向量θ和W 对应角度θ的组合:δθ=[θT ,θ]T ∈ℝ4，即直线的三维坐标正交表示㊂向量δθ
中各个参数都具有各自的几何意义：矩阵W 包含的比率信息σ1/σ2表示从坐标原点O 到直线L 的距离d ，参数θ与d 的方向相关㊂矩阵U 包含了直线L 的三维坐标信息,θ1㊁θ2㊁θ3各自对应的几何关系为:(1)θ1表示直线在以O 为圆心,d 为半径,OL 平面上的圆上转动;(2)θ2表示直线在以O 为圆心,d 为半径，垂直于OL 平面，并交直线于点Q 上的圆转动;(3)θ3表示直线围绕轴OQ 转动，如图2所示
单体建筑㊂
图2 参数θ1㊁θ2㊁θ3的几何意义
设从世界坐标系到相机坐标系的转换矩阵为T cw ∈
SE(3)，由旋转矩阵R cw ∈SO(3)和位移向量t cw ∈ℝ3
组成，如式(3)所示:
T cw =R cw
t cw éëêê
ù
û
úú0
1(3)
直线的普吕克坐标在世界坐标系中表示为L w =
[n ,v ]T
，在相机坐标系中表示为L c =[n c
,v c
]T
，根据
Bartoli 和Sturm 等人的研究[12]
，从L w 到L c 的转换关系可表示为式(4)㊂
L c =n c
v éëêêùûúúc =H cw L w =R cw [t cw ]×R cw 0
R éëêêùû
úúcw
L w
(4)
其中,[]
㊃×
表示向量对应的斜对称矩阵，可用于简便
地计算向量叉乘(a ×b =[a ]×b )㊂若向量a =[a 1,a 2,a 3]T ，则斜对称矩阵可表示为式(5)㊂
[a ]×=0-a 3a 2a 3
-a 1-a
2
a 1éëê
êêù
û
ú
ú
ú0(5)
直线普吕克坐标对应的相机内置矩阵如式(6),
其中f x ㊁f y ㊁c x ㊁c y 为相机标定参数㊂
K =f y 0
00
f x
0-f y c x
-
f x c y
f x f éëêêê
ùû
úú
ú
úy (6)
直线投影到影像后，坐标l 如式(7)所示㊂
I =l 1l 2l éëêêêù
û
ú
úú3=Kn c
(7)
若l 在影像中两端点用齐次坐标分别表示为^p 1
和^p 2，则l =^p 1×^p 2
㊂1.3 关键帧判定
结合ORB -SLAM 2[5]的关键帧判定方式，加入关键帧的条件为:(1)距离上次提取关键帧已经经过至少n 个普通帧;(2)该帧内追踪到的特征数小于阈值τt ，且至少检测到了τc 个新特征㊂根据实际实验测试，取n =9,τt =100,τc =70时，可得到较好的结果㊂
1.4 图优化解算1.4.1 算法详解
图优化(graph optimization )是目前最前沿的
SLAM 位姿估计方法㊂图优化使用最大后验估计进行非凸优化，从而估算相机位姿㊂待优化变量组成图的顶点(vertex )，其间转换关系(在SLAM 中通常为观测方程)为图的边(edge )，调整顶点的值使得位姿和顶点坐标尽量满足边的约束关系，从而达到优化的目的㊂在此过程中，使用高斯-牛顿法等非线性优化技术迭
代解算㊂
对于一个带有n 条边的图，其目标函数的形式通常如式(8)所示:
min x
∑n k =1
e k (x k ,z k )T Ωk e k (x k ,z k )
(8)
其中,Ωk 为信息矩阵，即协方差矩阵的逆，其含义为对误差相关性的大致估计，通常设置为对角矩阵，对角线元素表示各个误差值的权重㊂
为避免陷入局部最优解，实验时通常引入鲁棒核函数，并选择具有更大收敛池的迭代方法，如Levenberg -Marquardt 法㊁随机梯度下降法等㊂G 2O (general graph optimization )开源库[17]提供了实现图优化算法的C ++工具㊂
根据直线从世界坐标系到影像的投影l 与实际量
㊃05㊃ 计算机技术与发展 第31卷
测线段端点p 1和p 2(用齐次坐标表示)之间的距离，可得出重投影误差，如式(9)所示，其中d (㊃)表示距离函数㊂
e l =d (l 0,l )=p T
1l
l 21+l 2
2
p T 2l
l 21+l éëêêùû
ú
ú22(9)
而点特征的重投影误差为点从世界坐标系到影像的投影与实际量测点的距离㊂即在第k 帧中，特征点i 的世界坐标为P i ，影像坐标为p k ,i ，重投影误差如式(10)所示(其中包含一次从齐次坐标到非齐次坐标的转换)；综合式(4)~式(7)，设特征线j 的世界普吕克坐标为L j ，在该帧中对应的量测线段p k ,j 用其端点的齐次坐标表示，则特征线j 的重投影误差如式(11)所示,[]1-3表示向量的前三维㊂
ep k ,i =p k ,i -KT k ,cw P i
(10)el k ,j =d (l k ,j ,K [H k ,cw L j ]1-3)
(11)
算法使用光束法平差对相机姿态和特征坐标进行估计和优化，平差的代价函数根据式(10)和式(11)可得，如式(12)所示㊂
C k =
∑k ,i
ρp
(ep T
k ,i
Cov -1k ,i
ep k ,i )+
∑k ,j ρl
(ep
T k ,j
Cov -1k ,j ep k ,j )
(12)
其中,Cov k ,i 和Cov k ,j 分别表示点线特征在第k 帧中的协方差矩阵，与特征尺度相关,h p 和h l 分别表示代价函数中点线特征对应的鲁棒Huber 核函数㊂
向系统输入新帧k 时，需要根据该帧中提取特征的匹配结果，以目前的运动模型对姿态进行粗略估计，再使用光束法平差对该帧的相机姿态和特征坐标进行计算㊂设k 时刻相机的转换矩阵为T k ,cw ,T
k ,cw 的估计过程如式(13)所示:
T k ,cw =argmin T C k
(13)
使用G 2O [17]提供的多边图优化工具进行光束法平差时，将使用正交表达式表示的线特征㊁世界坐标表示的点特征和转换矩阵表示的相机位姿分别设为顶点，同时需要设置空顶点，则构成图的可能有两种边：点-空线-相机(点特征边)，或空点-线-相机(线特征边)㊂对于每一帧，需要进行多次重复平差计算，每次计算时将误差大于阈值th 的边的对应特征设置为离值，并从平差计算中剔除后再进行下一次迭代平差，以此来获得较稳定的位姿估值㊂在实验中，取th p =5.991,th l =7.815㊂如果初步位姿计算后非离特征数量足够，则需要在局部地图中搜索匹配特征，进一步优化位姿关系㊂1.4.2 雅克比矩阵
算法使用LevenBerg 方法估计使代价函数达到最
小的相机位姿值，设相机位姿R ,t 分别表示相机的旋
转矩阵和平移向量，它的李代数表示为ξ,ξ的扰动用δξ表示，迭代优化过程中需要计算重投影误差对δξ求导的雅克比矩阵㊂
首先，根据e l =[e 1,e 2]T =d (l 0,l )，量测线段端点
坐标为p 1=[x s ,y s ,1]T ,p 2=[x e ,y e ,1]T ，误差对重投影直线l =
l
1
,l 2,l []
3T
求偏导得到雅克比矩阵
(Jacobian matrix )结果如式(14)所示㊂
∂e l ∂l =1l n x s -l 1e 1
l n y s -l 2e 1
l n
1x e -l 1e
2
l n y e -l 2e 2
l n
éë
êê
êêùû
ú
úúú1(14)
其中,l n =
l 21+l 2
2㊂由式(4)和式(7)所示的投影关
系可得求导式(15)㊂
∂l ∂L c =∂Kn c ∂L c
=[K 0]3×6
(15)
根据文献[18]中的推导，可知直线在相机坐标系坐标的估值L *c 对该扰动求导，如式(16)所示㊂其中,
∂L *c
∂δξ=-[Rn ]×-[[t ]×Rv ]×-[Rv ]×-[Rv ]×éë
êêùûúú0
6×6
(16)
最后根据链式法则，结合式(14)~式(16)可得线特征重投影误差对位姿扰动求导，如式(17)所示㊂
∂e l ∂δξ=∂e l ∂l ∂l ∂L c ∂L *c
∂δξ
(17)
2 实验结果
该文使用了多组影像数据进行了实验㊂由于单目视觉里程计无法获取准确的深度信息，实验后通过计算相对位姿误差(relative pose error ,RPE )分析位姿精度㊂RPE 精度计算方法通过选定某个窗口大小，比较待估计轨迹和真值在窗口内的位姿变化一致性评价轨迹估算精度㊂精度估计过程中,TUM 数据集的参照真值为数据集提供的轨迹数据，鱼眼相机数据以Lidar SLAM 算法和IMU 综合计算的轨迹数据作为真值㊂实验表明，该文所述点线结合的跟踪算法可以有效避免跟丢，有效提高轨迹长度和估计精度㊂2.1 TUM 数据集
TUM 数据集[18]是一个常用的公开数据集，提供了一系列办公室㊁工厂厂房㊁标定棋盘格等RGB -D 影像数据，并提供标定相机内置参数和带有时间戳的轨迹真值信息㊂该文使用办公室数据(fr 3-long office household )和工厂数据(fr 2-large with loop )进行了实验㊂影像每帧大小为640*480，数据场景
如图3所示㊂
只使用点数据跟踪时，由于部分场景纹理缺失，常常出现跟丢的情况㊂加入线特征后，跟丢的情况有所
㊃
15㊃ 第1期 李铁维等：基于点线结合特征的单目视觉里程计
律师执业管理办法
改善㊂以上两个数据集的实验结果如表1所示
㊂
图3 fr 3(左)和fr 2(右)中的场景,
表1 TUM 数据集实验结果
中国经济为什么行数据跟踪帧数轨迹长度RPE (RMSE )
RPE (RMSE )/%
ORB 跟踪帧数
ORB RPE (RMSE )/%
fr 250511.4960.312.7153434.1fr 3
1013
6.680
0.27
4.094
--
2.2 鱼眼相机数据
自采集数据为使用鱼眼相机采集的会议室数据，数据大小为1232*1024㊂采集时，鱼眼相机放置于装有LiDAR 等多种传感器的背包上，在数据采集人员背着背包行走时越过采集者的头顶采集前方数据㊂鱼眼相机的焦距短，视野宽阔，同样的采集频率下相邻帧之间可以达到更高的重叠度，但拍摄出的影像变形较大，处理之前需要先标定相机，矫正影像畸变㊂矫正后的影像周围会存在黑框和镜头被遮挡产生的阴影，需
进行掩膜处理以避免在阴影处和边框边缘错误地提取出特征㊂在提取点特征时，通过坐标筛选剔除外点㊂而提取线特征时，考虑到处理效率，用一个简单的判断确定线是否位于无效区域：若线段l i 的两端点p si ㊁p ei 和中点p mi 有两个或以上位于无效区域时，标记线端l i 为无效特征，不参与后续特征匹配㊂相机轨迹的真值由LiDAR 点云数据和IMU 传感器计算得到㊂矫正后的影像实例和使用的掩膜如图4所示，筛选前后的线特征如图5所示
㊂
图4
自采集鱼眼相机数据和预处理掩膜
图5 进行筛选前后的线段提取结果对比
鱼眼数据实验结果与精度分析如表2所示㊂
表2 鱼眼相机会议室序列影像数据实验结果
数据跟踪帧数轨迹长度RPE (RMSE )RPE (RMSE )/%
ORB 跟踪帧数
ORB RPE (RMSE )/%
鱼眼
1136
17.289
0.0678
0.19
678
0.39
㊃25㊃ 计算机技术与发展 第31卷
>43cao
