基于采样点的直线匹配视觉惯性SLAM算法

林 林，黄宜庆*，李 宏

(1.安徽工程大学 高端装备感知与智能控制教育部重点实验室，安徽 芜湖 2410002.安徽工程大学 安徽省电气传动与控制重点实验室，安徽 芜湖 241000)

在自动驾驶和智能机器人领域中，同时定位与建图得到了广泛的应用。视觉惯性系统亦称VIO(Visual-inertial Odometry)系统，是融合相机和IMU(Inertial Measurement Unit)数据实现SLAM(Simultaneous Localization and Mapping)的算法，也是目前SLAM研究领域的研究热点。相机在快速运动时会出现图片模糊的问题，而IMU正好可以解决该类问题，且其价格便宜，IMU的缺点是它经过积分所得的加速度和角速度存在漂移问题，而相机没有这个缺点，所以两者可以很好地互补。

Tong等[1]提出并开源了VINS-Mono，这是一个具有鲁棒性的初始化过程，采用了一种基于紧耦合、非线性优化的方法，其效果在当时的VIO系统中是非常优秀的。但是基于点特征的VIO系统，其视觉包含的信息总是有限的，在光照强度较弱和快速运动的场景中，点特征的提取就变得尤为困难，使得SLAM系统难以保持较强的鲁棒性和较高的精度。线特征广泛存在于人造环境中，它可以提供额外的视觉约束，并为自动导航建立一个信息更丰富的地图，因此将线特征信息加入到视觉前端很有意义。文献[2]在VINS-Mono的框架基础上将线特征集成到VIO系统中。文献[3]使用梯度密度滤波器剔除图像中线特征的密集区域，使得线特征的误匹配大大降低，但加入滤波器后,密集区域的内部将不进行线段检测,而仅在其周围轮廓区域提取部分线段，这会导致很多的线段没有被检测到，从而失去很多线段信息。文献[4]则提出引入线段分组的方法，将可能是同一条线上的线段合并，生成匹配和提取效率高的长线段。但生成的长线段存在一些误匹配，影响了系统的精度。

文献[5]在ORB-SLAM2[6]的系统上进行创新，采用几何约束进行线特征匹配，利用线特征的深度信息构造虚拟线段，提出不一样的重投影误差计算方法，但其重投影误差的计算仅采用图像的像素观测，却忽略了深度观测。文献[7]将直线分为结构化直线和非结构化直线，并对这两类直线建立不同的数学模型，构建残差，加入到目标函数中进行优化。

基于上述研究，在视觉前端加入线特征，提出一种新的直线匹配方法，将帧到帧和帧到平面的两种直线匹配方法结合，并在线特征提取阶段，设置动态的长度阈值过滤掉短线特征，用以提高系统在弱纹理和快速运动的场景中的鲁棒性和精度；在视觉后端，将构建好的线特征重投影误差模型加入到基于滑动窗口内的目标函数进行求解，最后在数据集上验证所提算法。

1 算法框架

本文算法以VINS-Mono为系统框架，采用优化方法将点线特征的视觉观测数据与IMU观测数据紧密耦合。算法的系统框架如图1所示。由图1可知，系统框架主要包含3个线程：数据预处理、回环检测与全局位姿图优化、滑动窗口优化。其中，算法改进的地方已经用底纹标出。

数据预处理：分别对IMU和相机得到的点线特征进行提取、匹配并对齐。在点特征方面，采用Harris角点[8]，LK(Lucas-Kanade)光流以及双向跟踪[9]相结合的方法提高跟踪的精度。在正向光流跟踪中，如果上一帧有特征点，就直接进行LK跟踪并进行预测，然后统计和上一帧成功匹配的数目；如果小于10，就扩大搜索，输入的最大金字塔层数为3；如果没有进行预测的话，就直接输入最大金字塔层数为3。接下来进行反向跟踪，如果正向反向都能找到特征点，则认为这些特征点是符合要求的。

在线特征方面，为更好地解决位姿估计问题，在LSD(Line Segment Detector)算法[10]上进行改进，设置提取线特征的长度阈值[11]来去除掉短线特征，提高提取效率。并在直线匹配中加入基于点特征跟踪的平面到平面(2D-2D)和平面到立体(2D-3D)的直线匹配算法，从而提高匹配模块的速度和精度。

滑动窗口优化，回环检测和全局位姿图优化与VINS-Mono的方法大致相同，不同之处将在下文介绍，这里不再赘述。

2 线特征提取及匹配

2.1 线特征提取

对于线特征的处理方法，现阶段基本上都是采用LSD算法。LSD算法是将梯度方向相近的像素点合并，然而由于噪声等一些因素，一条长的直线可能被分割成很多条短的线段，这些短的线段所包含的图像信息不足，用于后面的匹配则效率过低，为此设置长度抑制策略。

长度抑制策略：使用最小长度阈值Lmin来去除短线特征：

Lmin=[γ×min(WL,HL)],

(1)

(2)

式中,min(WL,HL)为输入图像的宽度与高度二者之间较小的那个值;γ为长度比例因子;i为提取到线特征的数量。在文献[11]中，作者是根据经验将γ设为一个固定的值，但这样设置的γ不能使提取数量始终维持在一个合理的值，因此根据提取线特征的数量来动态地改变其值。当线段数量较少时，增大长度阈值,反之减小。为减少计算资源，该算法依然会设置最小长度的下限。通过该方法，线特征提取精度得到提升。

2.2 2D-2D的直线匹配

针对直线匹配的精度和稳定性，采用2D-2D和2D-3D的直线匹配方法。一般情况下，使用2D-2D的跟踪方法来匹配新的检测到的线，如果匹配到的线数量太少，将启用2D-3D的方法来提升匹配到的线的数量。

此方法是对前一帧中的所有线进行采样，得到采样点的集合P1{Ω1}，使用极线搜索和块匹配[12]的方法在新图像中检测到对应的候选匹配点。使用零均值差平方和(Zero-mean Sum of Squared Difference，ZMSSD)[13]计算两点的匹配分数，选取匹配分数最高的点作为跟踪点，最后可以获取跟踪点的集合P2{Ω2}。如果被跟踪的点到当前帧的直线距离小于我们设定的距离阈值，则认为该被跟踪的点是有效的。如果这些有效的点的数量大于前一帧线上采样点的0.8倍，则这条线被认为是匹配最佳的线。

(3)

它可以被近似为

(4)

式中,N是修补的宽度乘以修补的高度。

(5)

计算两张图像之间的匹配点的ZMSSD值，从而判断匹配点的相似程度。

2.3 2D-3D的直线匹配

在2D-3D的直线匹配中，采用零均值归一化互相关(Zero-normailized Cross-correlation,ZNCC)[14]的匹配方法，相比于归一化互相关(Normailized Cross-correlation，NCC)[15]，它在公式中去除了窗口内的均值，对于光照的变化能表现出更强的鲁棒性。两张图像的亮度可能由于照明和曝光条件而发生变化，第一张图像可以首先被归一化。这通常是通过减去平均值和除以标准差的方式在每一步完成。也就是说，两张图像之间的相似程度可以为

(6)

式中,n是f1和f2的像素数；μ是f的平均值；σ是f的标准差。

当相机快速运动和有场景遮挡时，需要匹配的线段长度会相差很大，从而影响匹配的精度。所以采用极线几何约束来限制线的采样范围，配合ZNCC进行匹配。即计算了基本矩阵和当前帧之间的本质矩阵之后，在基本矩阵中线的端点根据极线几何投影到当前帧，并且通过两条极线与当前帧的匹配线来确定对应点的采样范围，这样成功地提高了ZNCC匹配方法。

3 线重投影误差模型及其优化

3.1 线重投影误差模型

线重投影残差以点-线距离为模型。首先，定义线几何变换，给定一个从世界坐标系平面πw到相机坐标系平面πc的变换矩阵Tcw=[Rcw,tcw]，其中Rcw∈SO(3)和tcw∈R3定义为旋转和平移。利用这个矩阵，将Lw从πw变换到πc为

(7)

然后将Lc进行投影得到投影直线：

I=[l1,l2,l3]T=KLnc,

(8)

式中，KL表示直线投影矩阵。

最后假设Lw为第i个相机帧观测到的第j条空间线，线重投影误差可以定义为

(9)

3.2 基于滑动窗口的优化

在滑动窗口中定义与IMU、点和线测量信息相关的所有状态向量：

(10)

oT=(ΦT,φ),

(11)

式中,Φ为一个三维向量;φ定义为一个标量。

通过最小化所有测量残差项之和来优化滑动窗口内的所有状态变量：

(12)

4 实验验证与结果分析

本文采用公共数据集EuRoc对此算法进行评估，并将评估的结果与经典SLAM算法VINS-Mono的结果进行比较。本实验在台式电脑上展开，电脑的基本配置为：Intel(R)Core(TM)i7-9700 CPU@3.00GHz 8G内存，Ubuntu 16.04。

本文算法在MH_05_difficult序列上的可视化如图2、3所示。由图2、3可以看出，在光照较暗的场景中线特征的提取效果依然良好。在V2_03_difficult序列上的两种算法轨迹与真实轨迹的对比如图4所示。由图4可以明显看出，改进后的算法轨迹更接近真实的轨迹。

为验证本文改进系统相比于VINS-Mono具有更高的精确度，采用场景复杂程度较高的3个不同序列进行实验，系统所得结果与实际结果的误差如图5～7所示。本文算法在EuRoc数据集运行的绝对位姿误差的结果如表1所示，绝对位姿误差与VINS-Mono的对比结果如表2所示。

表2 绝对位姿误差与VINS-Mono的对比 (单位：m)

根据表1、2可得，在公共数据集EuRoc的7个不同难度、不同场景的序列中，本文算法相比于VINS-Mono都有提升，在MH_01_easy和MH_02_easy两个场景比较简单的序列中，提升效果不明显，究其原因是在简单场景中，点特征所包含的视觉信息足以使系统达到了一定的精度，即使加入线特征并改进了算法，也不能将精度提高很多，相反线特征的加入会增加计算量，降低系统的实时性。但在复杂的场景中，该算法的优越性就得到了体现，其中在快速运动的场景中，精度提高得更明显。这得益于在线特征提取阶段使用长度抑制策略，匹配阶段使用了两种直线匹配的方法。

表1 本文算法在EuRoc数据集运行的绝对位姿误差的结果(单位：m)

5 结束语

该算法在VINS-Mono的基础上，通过将线特征融合到系统中，解决点特征包含视觉信息不足的问题，并在原有的直线特征提取算法上通过长度抑制过滤掉短线特征，用来改善提取效果。并且将2D-2D与2D-3D的直线匹配方法结合使用，用于提高匹配的精度。而后建立线重投影误差模型，整合各部分残差，构建目标优化函数。通过在不同难度场景下的实验仿真，得到本系统的轨迹和误差，并与VINS-Mono运行结果进行对比，通过分析可知该算法在快速运动和弱纹理的情况下表现出更强的鲁棒性和更高的精度。