基于点线结合特征的单目视觉里程计

李铁维，王牧阳，周 炎

(1.武汉大学 遥感信息工程学院，湖北 武汉 430079；2.香港理工大学 建设及环境学院，香港 999077)

0 引 言

视觉里程计是SLAM(即时定位与地图构建)系统的前端部分，其主要任务是增量式地估计相邻图像之间的运动关系[1]。根据位姿估计时目标函数的不同构建方式，可分为直接法和特征点法。直接法假设影像灰度不变，通过计算最小光度误差估计相机姿态，而特征点法主要通过特征匹配和跟踪，计算最小重投影误差来进行估计的。直接法所需计算量更小，而特征法不易受到光照变化的影响，更为稳定。目前最具有代表性的直接法SLAM是J.Engel等人提出的LSD-SLAM[2]。LSD-SLAM针对像素进行，可以构建半稠密的场景地图。早期的特征法单目SLAM是以点特征为基础，使用扩展卡尔曼滤波器进行位姿估计，如A.J.Davison提出的第一个单目SLAM系统MonoSLAM[3]。Klein等人提出的PTAM(parallel tracking and mapping)系统[4]第一个使用了非线性优化方法，引入了关键帧机制，并实现了跟踪和构图过程并行化，奠定了此后基于特征的单目视觉SLAM算法的雏形。PTAM系统的继承者ORB-SLAM[5]则使用具有旋转和缩放不变性的ORB点特征，提出了跟踪、局部构图和闭环三线程并行的方案。ORB-SLAM的缺点是在仅旋转的轨迹中容易跟丢，且生成的特征稀疏地图无法直观地表现场景特征。

点特征简单易表达，但维度单一，缺少约束，形成的特征稀疏地图难以真正表达地理信息，而加入线特征可有效地改善这些缺点。Albert Pumarola等人提出的PL-SLAM[6]中，使用线段的两端点表达线特征，但实际在不同帧中，提取的三维线端点常常沿直线方向产生漂移，因此需要将其与在某帧投影中最匹配线段的端点绑定[7]。三维直线也可使用在两平面上的投影进行表达，如Spetsakis[8]和Rother[9]所使用的。Chandraker[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方法提取线段特征，然后与已加入地图的有效线特征进行描述子差异计算，为匹配完成的线特征加入该帧的信息。在位姿估计完成后，根据有效匹配点线特征和新加入特征的数量，判定该帧是否作为关键帧加入地图，并把在关键帧新检测到的线段加入地图。

随着相机移动，大量线段会在视野中消失，算法为每个线特征Li设置了一个计数器参数cti，在每次对全图线特征进行匹配之前，将所有线特征对应的cti加1，匹配结束之后得到匹配的参数减1；当cti参数累积达到阈值τc，即直线Li已在τc帧影像中不可见时，将直线Li标注为不可见，不再参与之后新加入帧的匹配。

1.2 线特征三维表达和投影

(1)

其中，v指向直线方向，n指向直线与坐标原点构成平面的垂直方向。则线段的正交坐标[12]可表示为式(2)：

(2)

图2 参数θ1、θ2、θ3的几何意义

设从世界坐标系到相机坐标系的转换矩阵为Tcw∈SE(3)，由旋转矩阵Rcw∈SO(3)和位移向量tcw∈3组成，如式(3)所示：

(3)

直线的普吕克坐标在世界坐标系中表示为Lw=[n,v]T，在相机坐标系中表示为Lc=[nc,vc]T，根据Bartoli和Sturm等人的研究[12]，从Lw到Lc的转换关系可表示为式(4)。

(4)

其中，[·]×表示向量对应的斜对称矩阵，可用于简便地计算向量叉乘(a×b=[a]×b)。若向量a=[a1,a2,a3]T，则斜对称矩阵可表示为式(5)。

(5)

直线普吕克坐标对应的相机内置矩阵如式(6)，其中fx、fy、cx、cy为相机标定参数。

(6)

直线投影到影像后，坐标l如式(7)所示。

(7)

1.3 关键帧判定

结合ORB-SLAM2[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)所示：

(8)

其中，Ωk为信息矩阵，即协方差矩阵的逆，其含义为对误差相关性的大致估计，通常设置为对角矩阵，对角线元素表示各个误差值的权重。

为避免陷入局部最优解，实验时通常引入鲁棒核函数，并选择具有更大收敛池的迭代方法，如Levenberg-Marquardt法、随机梯度下降法等。G2O(general graph optimization)开源库[17]提供了实现图优化算法的C++工具。

根据直线从世界坐标系到影像的投影l与实际量测线段端点p1和p2(用齐次坐标表示)之间的距离，可得出重投影误差，如式(9)所示，其中d(·)表示距离函数。

(9)

而点特征的重投影误差为点从世界坐标系到影像的投影与实际量测点的距离。即在第k帧中，特征点i的世界坐标为Pi，影像坐标为pk,i，重投影误差如式(10)所示(其中包含一次从齐次坐标到非齐次坐标的转换)；综合式(4)～式(7)，设特征线j的世界普吕克坐标为Lj，在该帧中对应的量测线段pk,j用其端点的齐次坐标表示，则特征线j的重投影误差如式(11)所示，[]1-3表示向量的前三维。

epk,i=pk,i-KTk,cwPi

(10)

elk,j=d(lk,j,K[Hk,cwLj]1-3)

(11)

算法使用光束法平差对相机姿态和特征坐标进行估计和优化，平差的代价函数根据式(10)和式(11)可得，如式(12)所示。

(12)

其中，Covk,i和Covk,j分别表示点线特征在第k帧中的协方差矩阵，与特征尺度相关，hp和hl分别表示代价函数中点线特征对应的鲁棒Huber核函数。

向系统输入新帧k时，需要根据该帧中提取特征的匹配结果，以目前的运动模型对姿态进行粗略估计，再使用光束法平差对该帧的相机姿态和特征坐标进行计算。设k时刻相机的转换矩阵为Tk,cw，Tk,cw的估计过程如式(13)所示：

(13)

使用G2O[17]提供的多边图优化工具进行光束法平差时，将使用正交表达式表示的线特征、世界坐标表示的点特征和转换矩阵表示的相机位姿分别设为顶点，同时需要设置空顶点，则构成图的可能有两种边：点-空线-相机(点特征边)，或空点-线-相机(线特征边)。对于每一帧，需要进行多次重复平差计算，每次计算时将误差大于阈值th的边的对应特征设置为离群值，并从平差计算中剔除后再进行下一次迭代平差，以此来获得较稳定的位姿估值。在实验中，取thp=5.991，thl=7.815。如果初步位姿计算后非离群特征数量足够，则需要在局部地图中搜索匹配特征，进一步优化位姿关系。

1.4.2 雅克比矩阵

算法使用LevenBerg方法估计使代价函数达到最小的相机位姿值，设相机位姿R,t分别表示相机的旋转矩阵和平移向量，它的李代数表示为ξ，ξ的扰动用δξ表示，迭代优化过程中需要计算重投影误差对δξ求导的雅克比矩阵。

首先，根据el=[e1,e2]T=d(l0,l)，量测线段端点坐标为p1=[xs,ys,1]T，p2=[xe,ye,1]T，误差对重投影直线l=[l1,l2,l3]T求偏导得到雅克比矩阵(Jacobian matrix)结果如式(14)所示。

(14)

(15)

(16)

最后根据链式法则，结合式(14)～式(16)可得线特征重投影误差对位姿扰动求导，如式(17)所示。

(17)

2 实验结果

该文使用了多组影像数据进行了实验。由于单目视觉里程计无法获取准确的深度信息，实验后通过计算相对位姿误差(relative pose error，RPE)分析位姿精度。RPE精度计算方法通过选定某个窗口大小，比较待估计轨迹和真值在窗口内的位姿变化一致性评价轨迹估算精度。精度估计过程中，TUM数据集的参照真值为数据集提供的轨迹数据，鱼眼相机数据以Lidar SLAM算法和IMU综合计算的轨迹数据作为真值。实验表明，该文所述点线结合的跟踪算法可以有效避免跟丢，有效提高轨迹长度和估计精度。

2.1 TUM数据集

TUM数据集[18]是一个常用的公开数据集，提供了一系列办公室、工厂厂房、标定棋盘格等RGB-D影像数据，并提供标定相机内置参数和带有时间戳的轨迹真值信息。该文使用办公室数据(fr3-long office household)和工厂数据(fr2-large with loop)进行了实验。影像每帧大小为640*480，数据场景如图3所示。

只使用点数据跟踪时，由于部分场景纹理缺失，常常出现跟丢的情况。加入线特征后，跟丢的情况有所改善。以上两个数据集的实验结果如表1所示。

图3 fr3(左)和fr2(右)中的场景,

表1 TUM数据集实验结果

2.2 鱼眼相机数据

自采集数据为使用鱼眼相机采集的会议室数据，数据大小为1 232*1 024。采集时，鱼眼相机放置于装有LiDAR等多种传感器的背包上，在数据采集人员背着背包行走时越过采集者的头顶采集前方数据。鱼眼相机的焦距短，视野宽阔，同样的采集频率下相邻帧之间可以达到更高的重叠度，但拍摄出的影像变形较大，处理之前需要先标定相机，矫正影像畸变。矫正后的影像周围会存在黑框和镜头被遮挡产生的阴影，需进行掩膜处理以避免在阴影处和边框边缘错误地提取出特征。在提取点特征时，通过坐标筛选剔除外点。而提取线特征时，考虑到处理效率，用一个简单的判断确定线是否位于无效区域：若线段li的两端点psi、pei和中点pmi有两个或以上位于无效区域时，标记线端li为无效特征，不参与后续特征匹配。相机轨迹的真值由LiDAR点云数据和IMU传感器计算得到。矫正后的影像实例和使用的掩膜如图4所示，筛选前后的线特征如图5所示。

图4 自采集鱼眼相机数据和预处理掩膜

图5 进行筛选前后的线段提取结果对比

鱼眼数据实验结果与精度分析如表2所示。

表2 鱼眼相机会议室序列影像数据实验结果

使用鱼眼影像数据生成的场景地图如图6所示，其中左图为场景稀疏特征地图，右图为左图中实线框内部分对应实际场景。可见点特征难以真正包含场景结构信息，而根据线特征地图对照影像数据可以清晰地看出属于场景的哪一部分。

图6 场景稀疏特征地图

3 结束语

从特征提取、匹配、位姿估算和场景地图生成等方面详细介绍了一种基于点线结合特征的视觉里程计方法，并展示了使用多种数据对该方法进行实验的结果。这些结果证实了在基于点特征的SLAM系统中加入线特征的可行性和有效性，在增加可跟踪轨迹长度的同时，提升了跟踪的精度。在后续研究中，将会在闭环检测等后端部分加入线特征运算，将线的辅助效果最大化，以构建完整的SLAM系统。