基于点线特征的改进视觉惯性SLAM 方法

严纪强，吴一琳，胡剑凌

（苏州大学 电子信息学院，江苏 苏州 215000）

0 引言

同步定位与建图（Simultaneous Localization And Mapping，SLAM）是一种基于设备搭载的传感器对周边环境信息进行感知以实现设备自身定位并建立周边环境地图的方法。在过去十几年里，因价格较低、感知信息量多、适应能力强等优点，以相机作为传感器的视觉SLAM 一直是移动机器人、自动驾驶以及增强现实等领域的研究热点之一。

视觉SLAM 根据相机位姿估计的策略不同主要可以分为直接法和特征点法两种。直接法基于灰度不变性假设，利用图像的灰度信息进行匹配，最小化光度误差优化求解相机位姿，计算量小，运行速度快，但易受光照条件的影响，并仅能应用于较小的位移，其代表性算法包括LSD-SLAM[1]和DSO[2]等。特征法基于特征的提取和匹配建立约束，最小化重投影误差求解相机位姿，能够适应更大的场景变化，具有更强的鲁棒性，其代表性算法包括PTAM[3]、ORB-SLAM[4]等。当前特征法主要利用的特征为点特征，然而基于点特征的视觉SLAM 存在许多不足，一是单目相机无法建立尺度约束而双目相机运算量较大，资源占用多；二是基于点特征的视觉SLAM 在纹理缺失、运动模糊、弱光照等情况下难以建立良好的数据关联，从而导致运动估计失败。

为了解决这些问题，研究人员将视觉和惯性传感单元（Inertial Measurement Units，IMU）相结合，提出了许多紧耦合的视觉惯性SLAM 方法，联合优化视觉和IMU 信息，如MSCKF[5]、VINS-Mono[6]等。另外，考虑到纹理缺失往往发生在室内等人造环境中，而这些场景往往有丰富的线段特征，因此许多研究人员将线特征加入到图像之间的数据关联中。LU Y[7]等人基于RGB-D 相机实现了点线特征融合的视觉里程计，并证明点线特征比单一特征具有更小的运动不确定性。ZUO X[8]等人首次在SLAM 中采用正交表示最小参数化直线，并分析推导出相对于直线参数的重投影误差的雅可比矩阵，大大改善了SLAM 的解决方案。GOMEZ O R 等人提出了基于双目相机的PL-SLAM[9]方法，在所有过程中都使用了点线特征，包括视觉里程计、关键帧选择、光束法平差，并提出一种新的词袋模型，在闭环过程中综合考虑点线特征的信息。YIJIA H等人基于VINS-Mono 方法提出了PL-VIO[10]方法，将点线特征与IMU 信息紧耦合优化，优于仅基于点特征的VIO 系统。LI X[11]等人又在PL-VIO 的基础上增加了面特征和共面约束，基于点特征生成的三维网格进一步建模环境拓扑。然而，由于LSD[12]算法提取线特征时间长，以上点线特征结合的算法都难以实时运行，赵良玉等人[13]在VINS-Fusion[14]方法基础上利用改进的FLD[15]算法实现了双目视觉惯性SLAM 系统的实时运行。QIANG F[16]等人对LSD 算法的隐参数进行调整，在尽量不影响准确性的情况下实现了LSD 算法的实时运行。LEE J[17]等人在PL-VINS 基础上增加了平行线约束，基于灭点对平行线聚类，将平行线残差加入到优化函数中，进一步提升了实时定位精度。

目前的点线SLAM 方法大多使用LSD 算法提取线特征并计算LBD[18]描述子进行线段匹配，然而原始的LSD 算法在策略选择时容易将一条长线段分割为多条短线段，这些短线段不够稳定且容易造成误匹配。另外，当前点线误差的融合策略为直接融合，未考虑到点线特征之间的差异性以及点线特征融合对系统带来的影响。同时，目前的点线特征在视觉惯性SLAM中利用不够充分，在闭环检测中仅使用了点特征来计算图像间的相似度。

针对上述存在的问题，本文提出一种基于点线特征的改进视觉惯性SLAM 方法。首先，提出一种新的短线段合并算法，合并原属于同一条直线的短线段，增加图像之间的数据关联；其次，基于LU Y[7]等人的结论提出一种新的误差融合策略，调整了点线特征及IMU 之间优化的权重；最后，将线特征加入到视觉惯性SLAM系统的闭环检测中。基于以上改进，所提的方法提升了视觉惯性SLAM 系统的鲁棒性和定位准确性。

1 系统框架

本文提出的方法主要基于VINS-Mono 系统进行改进，点线特征及IMU 的融合优化策略使用了PLVIO 的方法。所设计的系统框图如图1 所示，主要分为特征提取与跟踪、局部滑窗优化以及闭环检测三部分。初始化过程与VINS-Mono 采取相同的策略，基于视觉信息和IMU 信息的松耦合策略，首先利用纯视觉SFM（Structure from Motion）估计滑动窗口内所有帧的位姿及3D 点的逆深度，最后与IMU 预积分的结果进行对齐，并求解初始化参数。

图1 基于点线特征的视觉惯性SLAM 改进方法

特征提取与跟踪首先对输入的图像提取Shi-Tomasi 特征点和LSD 特征线，LSD 算法使用了QIANG F 等人调整后的隐参数，从而实现系统的实时运行，然后使用所提出的线段合并算法对线段进行合并。对于Shi-Tomasi 点特征，基于光流法进行特征点匹配与跟踪，对于LSD 线特征，计算LBD 描述子，并基于描述子距离进行匹配，并将结果通过机器人操作系统（The Robot Operating System，ROS）话题发布/订阅机制发送到局部滑窗优化部分，局部滑窗优化对IMU 数据进行预积分，并将预积分的结果与点线特征重投影误差基于本文提出的加权方式进行联合优化，求解得到相机位姿。同时，在这一过程中，根据特征在前后帧图像间的视差量决定是否插入关键帧。若插入关键帧，则将当前关键帧的点线特征发送到闭环线程，在闭环线程采用DBoW[19]词袋模型分别计算图像间的点特征相似度和线特征相似度，并根据点线特征的数量和分布情况对图像相似度进行加权，确定是否发生闭环，之后根据描述子匹配以及几何验证确定闭环的准确性，若发生闭环则进行4 自由度全局位姿优化。

2 改进的LSD 算法

LSD 算法是一种能在线性时间内得到亚像素级准确度的线段检测算法，其在任何数字图像上都不需要额外调整参数。但由于在基于梯度进行区域增长的过程中每个像素点仅允许被一条直线使用，因此对于遮挡、相交、边缘模糊等情况，LSD 算法容易将一条长线段分割为多条短线段。

对此，谢晓佳[20]提出了一种利用点线距离、端点距离以及描述子距离进行匹配的方法，林利蒙等人[21]详细地阐述了基于角度、端点距离的筛选合并策略。但这些方法往往每次只能处理合并两条短线段，对于多条线段的合并往往需要多次迭代，会带来较高的时间复杂度。另外，如图2 所示，对于线段a来说，相对于线段c，往往线段b更加应该被合并，尽管在欧氏距离上线段c可能更近一些。在合并时，上述方法难以确定线段的相对位置，需要再进行多个端点间的欧式距离计算，如图2 中的a、b、d线段难以确定合并后直线的起点和终点，这会导致计算资源的浪费。

图2 线段的相对位置示意图

本文提出的线段合并算法首先基于角度差值以及端点到直线的距离对线段进行分组，其次根据线段的角度实行不同的排序方案，实现线段基于相对位置的有序排列，最后根据线段端点之间的距离和线段的长度之和进行比较，确定是否属于同一条直线，并基于LBD 描述子对合并后的直线进行筛选和剔除。

2.1 线段分组

实验发现，LSD 线段提取得到的相邻平行线往往方向相反，因此，无需特意进行平行线的剔除或合并。但也会有一些短线段和长线段平行且方向相同的情况发生，针对此类情况，首先使用设定的长度阈值Lth将短线段剔除。

考虑到长线段特征更加稳定，首先对LSD 提取的线段根据长度进行降序排序，得到L={L1,L2,L3,…,Ln}。从最长的线段开始进行分组，直到线段长度小于设定的阈值Lmin。针对每一条线段Li，首先基于角度进行筛选得到候选线段组Lθ，通过这一步可以快速筛选掉大量不匹配的线段，降低后续的计算量：

式中：Lj表示线段组L中长度比Li短的线段，θi和θj分别为Li及Lj与x轴的夹角，θth表示Li及Lj之间的角度差阈值。

然后，计算Lj两端点到Li所在直线的距离，设线段Lj起点坐标为ps，终点坐标为pe，线段Li所在直线表示为l=[l1,l2,l3]T，即直线方程为l1x+l2y+l3=0，则两端点到直线的距离d为：

式中：ds表示Lj起点到Li所在直线的距离，de表示Lj终点到Li所在直线的距离。

基于公式（2）计算得到距离d，筛选得到最终的候选线段组Lg：

式中：dth表示端点到直线的距离阈值。

通过以上条件筛选，可以将图片中属于同一条直线的线段筛选出来。这些线段可能本来属于同一条直线，也可能是空间中不同直线上的线段投影到一条直线上的。因此，针对分组后的线段，通过线段合并策略，在尽量将属于同一条直线的线段合并的同时剔除误分组的线段，将之存放到LSD 提取的原始线段组L中，用作后续处理。

2.2 线段合并

经过线段分组后得到一组线段Lg，由于在线段分组过程中未对线段端点距离进行限制，分组中会存在一些在图片中处于同一条直线，但在实际空间相差甚远的线段，在线段合并时要对这些错误分组的线段进行剔除。LSD 算法提取的直线是有方向的直线，对于一张二值图颜色取反，则LSD 提取的直线不变，但方向完全相反，而在计算LBD 描述子的时候会对线段进行方向的区分，因此在线段合并的时候需要判断线段的相对位置。起始点和终点位置必须具有一致性，以便后续线段匹配准确。

首先，为了防止排序的歧义性，将直角坐标系根据角度分为4个部分，如图3所示.其中，S表示线段起点，E表示线段终点。以区间1 为例，以该区间的线段为主线段时，经过上述线段分组后获得的线段组，均为S在下、E在上的形式。因此仅需要根据起点S的纵坐标进行排序，即可得到在几何位置上有序的线段序列。

图3 基于线段方向的分区示意图

将之前分组时参考的长线段称为主线段lm，根据的角度θm执行不同的排序策略。这样做的目的是将线段组按照主线段的方向进行对齐，便于后续端点距离的计算。设两条线段li和lj，起点和终点分别为(si,ei)，(sj,ej)，每个点的坐标为(x,y)，则根据式（4）所示的策略进行排序。

式中：sort(*)表示基于括号内的变量大小进行排序。

通过该方法排序后的线段根据端点位置按照{si,ei,sj,ej}顺序排列，之后先将排序后的线段根据主线段的位置分为两组，如图4 所示，并对每一组都进行前缀和计算。前缀和计算时，每一组线段都要包含主线段。以排序后的线段组2 为例，设前缀和计算结果为spre=[lsum0,lsum4,lsum5,lsum6]。

图4 基于相对位置分组后的线段示意图

从距离主线段最远的线段开始，即图4 中的l6，计算主线段lm起始端点sm和线段l6起始端点sj的距离d，并与之前计算得到的前缀和中对应的线段总长度lsum6进行比较，如式（5）所示：

式中：rth为比例阈值。

若线段总长度占距离的比例大于阈值rth，则认为从lm到l6之间的所有线段属于同一条长线段，连接lm的起点和l6的终点作为合并后的线段，否则将线段l6释放回线段组L中，用于后续的处理，这是通过对线段进行是否使用的标记实现的，同时，继续判断次远的线段l5。由于长线段往往具备足够的信息和稳定性，且本不属于同一直线的长线段的错误合并往往会对系统造成更大的误差，在进行计算的过程中，对比例系数rth进行了线性计算，限制长线段的合并。

式中：α和β为参数因子。

最后，为了减少错误合并，基于LBD 描述子进行筛选，计算合并后的线段和主线段的描述子并计算描述子距离。若主线段及与其合并的线段属于同一条直线，则合并前后的描述子的汉明距离相差应该不会很大，因此当描述子距离大于某个阈值Dth时，放弃此次合并，仅保留主线段作为当前合并的结果。

长线段特征往往来自于纹理比较明显、梯度较大的区域，因此长线段要比短线段更稳定。在计算描述子之前，对线段进行筛选，选取策略如式（7）所示：

式中：WI，HI表示图像的宽和高，η代表比例系数。

3 视觉惯性联合优化

后端视觉惯性联合优化基于滑动窗口模型。窗口长度设置为11 帧，第n+N时刻滑动窗口中的优化变量定义如下：

式中：χ表示整个滑动窗口中所有的状态量，xi表示IMU 在第i帧的状态，b表示body 坐标系，即IMU 坐标系；w表示世界坐标系，分别代表在世界坐标系下的IMU 位置、姿态速度以及在body 坐标系下IMU 加速度计（accelerometers）偏置和陀螺仪（gyroscopes）偏置。λ表示空间点特征的逆深度，O表示空间直线的正交表示。

滑窗优化基于ceres 库，最小化目标函数（9）优化求解所有状态变量。目标函数包括点重投影误差、线特征重投影误差、IMU 预积分误差以及边缘化带来的先验误差，其中线特征重投影误差也是基于端点到直线的距离：

式中：rp−JpX 表示在滑动窗口中边缘化一帧后的先验信息，表示两次IMU 信息之间的测量残差，其中B表示滑动窗口中的所有预积分的IMU测量值；分别表示点投影残差和线投影残差，F和L分别表示滑动窗口中相机观测到的点特征和线特征，右下角角标∑p，表示各残差项的协方差矩阵；ρ表示鲁棒核函数，一般采用Cauchy 核函数或Huber 核函数。

VINS-Mono 方法认为视觉约束项的噪声协方差与标定相机内参时的重投影误差，即偏离像素的个数有关，原作者代码中使用1.5 个像素作为协方差系数。对应到归一化相机平面上的协方差矩阵需除以焦距f，则信息矩阵等于协方差矩阵的逆，为：

PL-VIO 以及PL-VINS 方法均沿用了这一策略，对点和线段的重投影误差均采用1.5 个像素的偏差。然而，根据Y LU 等人在文献[7]的结论，点线特征结合能比单一特征带来更小的不确定度，而不确定度大小主要体现在协方差矩阵的大小。另外，由于本文使用的LSD 算法在提取线特征时对图像进行了0.5 倍缩放，其不确定度设为点特征的两倍，取点特征协方差系数为0.707，线特征协方差系数为1.414，即加权后的信息矩阵为：

式中：Λl表示线特征的信息矩阵，表示线特征的协方差矩阵。

4 闭环检测

系统在长期运行中往往会产生累积误差。在没有外部参照的情况下，累积误差会越来越大，导致估计出来的轨迹偏离实际位置。闭环检测可以检测到相机是否回到之前经过的位置，在当前帧和回环帧之间建立约束，从而可以通过后续的全局优化修正轨迹，减小累积误差。当前主要使用的闭环检测策略为词袋模型（Bag of Words，BoW）。

4.1 词袋模型

BoW 是用于衡量两幅图像相似性的一种数据结构。如图5 所示，将视觉特征通过使用K叉树，在根节点使用k-means++算法将所有样本聚类为k类，然后下一层针对每一类再聚类为k类，聚类d次，最后得到叶节点，即单词（Words）。由于不同的特征具有不同的区分度，常用的相似度计算使用的是TF-IDF（Term Frequency-Inverse Document Frequency）方 法。TF 的思想是，某一特征在图像中出现频率越高则越具有区分度，IDF 的思想则是某特征在词典中出现频率越高，则这一特征越常见，越没有特异性。

图5 K 叉树词袋示意图

IDF 一般是在建立词典时计算的，统计某个叶子节点wi中的特征数量相对于所有特征数量的比例：

式中：ni表示当前节点在词典中出现的次数，n表示所有单词出现的总次数。显然，ni越大，当前单词在所有单词中出现的次数越多，该单词的权重也就越小。

TF指的是该特征在单幅图像中出现的概率：

在游戏中，设计了多种游戏场景来考核学生的学习效果，例如：有任务场景、闯关场景、限时场景等。玩家在游戏中，可以自由选择进入某一场景。通过在游戏中回答问题，获得或失去一定分数。在游戏的过程中，如果玩家的分数低于指定的分值时，游戏即判断玩家失败。在游戏中为了提高学习效率，对于玩家回答错误的问题，会继续出现在游戏中，而对于玩家回答正确的问题，则会被游戏屏蔽，而不再出现。以下图2是一次玩家回答问题的图示：

考虑权重后，图像I的特征点可对应许多个单词，这就组成了I的词袋：

给定两个图像的词袋vA，vB，其相似度评分可基于L1范数求解：

4.2 点线特征融合策略

VINS-Mono 方法中，前端对于点特征匹配使用的是基于Shi-Tomasi 角点特征的光流跟踪，因此VINSMono 方法在闭环线程重新提取FAST 角点并计算BRIEF 描述子，而在前端对于线特征使用了LSD 和LBD 描述子进行线特征匹配。线特征的描述子已经被计算出来，因此可以直接通过ROS 系统将线特征描述子发布到闭环节点中，避免重复计算。同时，也要将线特征发布到闭环节点，后续用来计算线特征的分布情况。

在计算获得点特征描述子以及线特征描述子后，分别在对应词袋中进行查询获得递减的n个匹配关键帧的ID 以及相似度得分，得到点特征相似度得分sp和线特征相似度得分s1。由于点特征是在闭环线程重新提取的FAST 角点，数量一般在2 000 以上，而前端获取的线特征数量往往为100 左右，若基于点线特征的数量比例对相似度得分进行加权，则线特征的作用无法体现出来，因此采用固定值wp与w1进行加权。另外，基于点线特征在图像中分布的均匀情况对点线特征进行加权，点特征的分布基于其坐标求标准差σp，线特征的分布基于线段中点的位置进行计算σ1：

最后，图像总相似度评分s如式（18），然后根据这一得分判断是否发生闭环：

5 实验结果与分析

5.1 实验环境和数据集介绍

实验所用的数据集为苏黎世联邦理工学院开源的双目+IMU 数据集EuRoC[22]，主要采集自厂房和室内两个环境，包含从低速运动到在纹理重复、纹理缺失、光照变化等复杂场景下的快速移动。实验基于HP Z460 工作站进行，详细参数配置如表1 所示。

表1 实验环境参数配置

为了评估不同算法的准确性，通常采用EVO（Evaluation of Odometry and SLAM）与真实值进行时间戳对齐以及绝对轨迹误差（Absolute Trajectory Error，ATE）计算，常用的ATE 评价参数为均方根误差（Root Mean Square Error，RMSE）：

式中：trans表示平移误差，Test,i表示i时刻的估计位姿，Tgt,i表示第i时刻的真实位姿。

5.2 改进线特征质量测试

选取EuRoC 数据集中的MH_01_easy 序列进行试验，统计平均每帧的线段平均长度和前后两帧之间成功匹配的线段数量，对LSD 算法和改进后的LSD 算法进行性能分析。结果显示，相比于原始的LSD 算法，提取线段的平均长度提升了19.03%，前后帧间线段匹配数量平均提升了43.8%。这表明，基于改进后的LSD 算法，建立了更多前后帧之间的数据关联，从而能够提升系统的鲁棒性。

表2 改进前后LSD 算法性能比较

线段提取结果图如图6 所示。图6（a）为在MH_01_easy 序列中选取的一张图片，图6（b）为基于LSD 提取的线段结果，图6（c）为对LSD 提取的结果直接进行长度阈值筛选的结果，图6（d）为对经过线段合并后长度阈值筛选的结果。图6（b）和图6（d）中的矩形框展示了合并前后的线段，可以看出，提出的线段合并算法将属于同一条直线的短线段进行了合并，因此与图6（c）相比，图6（d）增加了更多的长线段。

图6 线段提取结果图

图7 为线段匹配结果图。图7（a）展示了基于原始LSD 算法的两帧间线段匹配结果，图7（b）展示了基于合并后的线段的两帧间匹配结果。从匹配线的数量可以看出，基于改进的LSD 算法能够在前后帧之间建立更多的线特征匹配约束，增强了前后帧之间的数据关联，从而提升了算法的鲁棒性。

图7 线段匹配结果图

5.3 系统精度测试

表3 展示了不同算法在EuRoC 数据集下的表现。其中，PL-VIO 方法为视觉里程计，没有闭环检测部分，X 表示跟踪丢失，加粗部分表示最佳结果。由表3 可知，在无闭环的情况下，除了MH_01_easy 和MH_02_easy 两个序列，本文提出的算法在其他序列上的性能均优于之前的算法；在无闭环情况下，RMSE较VINS-Mono 方法平均降低了14.90%，较PL-VIO方法平均降低了13.17%，较PL-VINS 方法平均降低了8.96%；在有闭环的情况下，本文提出的算法也在绝大多数数据集序列上取得了比之前算法更好的效果，RMSE较VINS-Mono 方法平均降低了24.74%，较PLVINS 方法平均降低了5.99%，相较于PL-VINS 方法，本文提出的算法的RMSE在V1_03_difficult 序列降低了19.34%，在V2_03_difficult 序列降低了23.9%。这些序列场景都相对复杂，V1_03_difficult 序列运动快速、场景模糊且有弱光场景，V2_03_difficult 则主要有场景模糊及纹理稀疏问题，这体现了本文提出的算法对各种场景更具有鲁棒性。

表3 不同算法在EuRoC 数据集下的均方根误差

图8 展示了在序列V2_03_difficul 的算法绝对轨迹误差的琴型直方图对比图，其纵轴长度表示误差的分布范围，横轴宽度表示误差的分布密度。图8（a）为无闭环的结果，从左到右分别为本文结果、PL-VINS结果、PL-VIO 结果以及VINS-Mono 结果；图9（b）为有闭环的结果，从左到右分别为本文结果、PLVINS 结果以及VINS-Mono 结果。可以看出，本文提出的算法绝对轨迹误差的分布范围更小，且误差主要集中在下部，具有更多的小误差项，即估计轨迹与真实轨迹更接近。

图8 绝对轨迹误差对比图

6 结语

本文提出了一种基于点线特征的改进视觉惯性SLAM 方法，采用修改隐参数的LSD 进行线特征提取，并对短线段进行合并，增加了成功匹配线段的数量，增强了帧间数据关联，使之更适用于SLAM 系统。另外，通过对已有文献结论的分析和实验，改进了点线和IMU融合误差项的权重比，提升了系统整体的精确度。在闭环检测阶段，在视觉惯性系统中引入线特征，将前端计算得到的线段描述子通过ROS 直接发布到闭环节点，然后基于点线特征的数量和图像纹理对图像相似度进行加权来确定闭环，增强了系统的鲁棒性。