基于点线特征的快速视觉惯性SLAM算法

周书杰，吴功平，何文山

(武汉大学动力与机械学院，湖北武汉 430072)

0 前言

同时定位与建图(SLAM)可以解决移动机器人在GPS信号较弱的环境中的定位问题，同时通过其自身的传感器获得周围环境的三维结构及自身的姿态，进而完成相关任务。视觉SLAM相比于激光SLAM，其相机成本低廉、功耗低、体积小，可以获取丰富的环境信息，受到了工业界与学术界的广泛关注。

通常，V-SLAM/VIO算法利用图像特征点来跟踪定位。例如，QIN等[1]提出的VINS-Mono以FAST角点[2]为特征点，用KLT光流法[3]追踪完成特征匹配。MUR-ARTAL等[4]提出的ORB-SLAM，基于改进的ORB算法[5]来提取特征点，使得特征点在图像中分布均匀。CAMPOS等[6]提出的ORB_SLAM3，在ORB-SLAM中融入惯性测量单元(Inertial Measurement Unit，IMU)，显著提升了算法的鲁棒性和精度，在各个数据集上表现优异。然而基于特征点的SLAM系统在弱纹理场景中，提取到的点特征较少，往往无法满足跟踪需求，导致算法性能下降。

为解决以上问题，研究人员将图像中的线特征引入到V-SLAM/VIO中，相比于点特征，线特征可以提供更多的结构信息，尤其是在人造场景中，如办公楼和城市建筑。因此，线特征可以弥补点特征的不足，提高基于特征点SLAM系统的鲁棒性和稳定性。PUMAROLA等[7]在ORB-SLAM的基础上融入线特征，构建一个单目点线SLAM系统，用两个端点来表示空间直线。HE等[8]提出的PL-VIO，在VINS-Mono框架中加入线特征，用普吕克坐标和正交表示法[9]来表示空间直线，减少待优化变量个数，提高线特征优化精度。ZUO等[10]提出的双目点线SLAM系统PL-SLAM，将线段误差函数定义为线段投影端点到线段距离。

王丹[11]改进了SVO中提取特征点的方式并加入线特征，使轮式机器人在室内环境获得了更精确的定位。魏琛琛[12]利用点线特征的共线性，增加了SLAM系统中的测量约束，使得移动机器人在低纹理的走廊环境中定位精度远高于其他算法。曾继超[13]用梯度过滤机制改进传统LSD算法，提升了线段提取的稳定性。

以上研究在线特征检测时均采用LSD算法[14]，线段匹配用LBD描述子[15]。然而，LSD线段检测和提取LBD描述子速度较慢，对算力弱的计算平台不友好，限制了点线特征SLAM的跟踪速度。且LSD算法容易受图像质量影响，检测到的线段被分割为多个短线，易产生线段误匹配，降低SLAM系统定位精度。许多基于线特征的SLAM算法都对LSD进行改进，提升了系统精度，但同时也增加了SLAM前端耗时[10，13，16]。

针对以上点线特征SLAM系统存在的问题，本文作者提出一种融合点线特征的快速单目视觉惯性SLAM算法，使用ELSED(Enhanced Line Segment Drawing)线段检测算法[17]，快速提取场景中的长线段，剔除较短的线段，减少短线误匹配对系统的干扰。在追踪普通帧时，基于连续帧间微小运动假设，并利用几何约束实现线段快速匹配，显著提升了线特征匹配速度与匹配准确率。并在地面机器人OpenLORIS数据集上对此算法进行测试，结果表明：此算法在定位精度和实时性上都表现良好。

1 系统架构

文中SLAM系统包括跟踪、局部建图和回环检测3个线程，系统框架如图1所示，该系统在ORB-SLAM3系统的基础上加入线特征，其中涉及点特征的操作与ORB-SLAM3基本一致。

图1 系统框架设计

主要针对传统线特征提取和匹配计算耗时久的问题，引入新的线段提取算法ELSED，提高线段提取的质量，并利用连续帧之间的微小运动假设，提出一种相邻图像之间的无线段描述子的线特征匹配算法，避免了不必要的线特征描述子的计算。在关键帧上提取线段描述子，完成关键帧之间线段匹配，创造新的地图线。

2 线特征提取与匹配

2.1 线特征提取

点线特征SLAM算法领域普遍使用LSD算法提取线特征，但LSD算法消耗大量计算资源，实时性差，并且其对图像遮挡、局部模糊等情况鲁棒性差，容易将一条长线检测为多条短线，加大匹配难度。故本文作者使用ELSED算法，其线段检测大致为以下6步：

(1)高斯滤波去除噪声；

(2)利用Sobel算子计算像素梯度与方向；

(3)选取局部梯度最大的像素作为锚点像素；

(4)依托锚点同时进行边缘检测与直线拟合；

(5)长线段断口检测合并；

(6)剔除错误检测线段。

ELSED有普通检测和跳跃检测2种模式，二者区别在于跳跃检测模式执行其算法的第5步，而普通检测跳过该步。由于图像模糊，图像像素梯度方向变化或者梯度值转为零，进而出现线段边缘不连续，导致一条长线段被检测为多条短线。为尽可能检测出完整长线段，算法一旦检测到不连续边缘，会跳过不连续边缘并沿线段方向继续检测线段，在一定像素区间内，若仍可检测出线段，则拟合该方向所有线段形成长线段。

ELSED中大多数参数已根据经验设置，无需进行更改。其检测的最短线段长度设置为15，较短的线段容易导致误匹配，连续跟踪效果差，过多的短线往往会降低点线融合的SLAM系统精度，故对线段提取的最短长度Lmin做一些调整：

Lmin=[α·min(W，H)]

(1)

其中：W和H分别为图像宽度和高度，取其中较小值为阈值基准，以适应不同尺寸图像；α是尺度因子，根据经验设置为0.083；[·]定义为向下取整函数，如848像素×480像素的图片，min(W，H)=480像素，Lmin=40像素。

2.2 连续帧之间的快速线段匹配

现在大多数基于线特征的视觉SLAM系统采用LBD线段描述子进行线段匹配跟踪，这种方式在大多数场景下表现良好，但计算量相对较大。通常，相机在相邻帧之间运动量较小，线特征匹配可以不需要描述子完成，本文作者提出一种连续帧间快速线段匹配算法，算法示意如图2所示。

图2 线段匹配算法示意

对于连续两帧图像I1和I2，将I1中提取的n条线特征存入集合L1={l1，l2，…，li，…，ln}，li表示图像I1中第i条线段；将I2中提取的m条线特征存入集合L2={l1，l2，…，lj，…，lm}，lj表示图像I2中第j条线段。R和t分别代表两图像之间的相对旋转与位移。

λ2x2=λ1Rx1+t

(2)

从而有式(3)：

λ2K-1p2=λ1RK-1p1+t

(3)

一般情况下，相机若不是极其剧烈的抖动，其连续两帧的相对位移t基本趋于0，匹配像素对应的深度λ1≈λ2，因此，可将公式(3)简化为

p2=KRK-1p1

(4)

其中，文中算法中旋转矩阵R是由ORB-SLAM3跟踪线程中恒速运动模型得到的当前帧初始位姿，在IMU未初始化时，通过上一帧的位姿以及上一帧的速度获得当前帧的位姿初始值，在IMU完成初始化后，当前帧的位姿初始值为IMU预积分得到。

本文作者提出的快速线段匹配算法基于以上假设完成，具体步骤如表1所示。

表1 快速线段匹配算法

其中，相比于传统的KLT算法，式(4)给予KLT迭代优化一个更可靠的初值，从而提高关键点跟踪精度。线段间的匹配距离γij为

(5)

(6)

为防止误匹配，设置匹配因子β：

随着教学改革的步伐不断迈进，老师在课堂中不但要传授学生知识，还需要把学生的探索精神与创造能力培养起来，但是由于受到根深蒂固的传统教育观念所影响，很多的老师在教学过程中并没有把提升学生的综合素质重视起来。在教学的过程中老师必须改变这种传统的教学观念，充分应用新课改背景下的现代化教学理念，尊重学生学习过程中的主观能动性，引导每个学生都能够主动深入思考，并能够展现自己的个性。

(7)

其中：β为最小匹配距离γmin与次小匹配距离γmin2的比值。匹配因子β越小，匹配质量越好，若β大于某一阈值，则认为该匹配是误匹配。

3 直线重投影误差及后端优化

3.1 直线重投影误差

文中线特征的重投影误差为3D-2D的方式，为非线性误差，如图3所示。其中，误差表示为空间直线投影到图像平面的直线与匹配线段两端点s=[us，vs，1]和e=[ue，ve，1]的距离误差ds和de：

(8)

图3 线特征重投影误差

其中：rl为线段重投影误差；l=[l1，l2，l3]为投影直线方程。

3.2 后端优化

(9)

由IMU预积分可以获得i、i+1时刻之间的相对旋转ΔRi，i+1、相对速度Δvi，i+1、相对位置Δpi，i+1和信息矩阵∑Ii，i+1，故惯性残差可定义为

rIi，i+1=[ΔRi，i+1，Δvi，i+1，Δpi，i+1]

(10)

(11)

其中：Π(•)为相机重投影函数；uik表示地图点在像素平面的观测值；TCB∈SE(3)是从body坐标系(惯性坐标系)的转移矩阵，一般通过传感器标定获得；运算符⊕表示李群SE(3)在R3上转化操作。

ORB-SLAM3在局部或者全局优化中，将视觉和惯性的联合优化视作基于关键帧的最小化问题，本文作者在其中加入线特征，故视觉惯性联合优化问题可以写为

(12)

图4 后端优化因子图

4 实验及结果分析

采用地面机器人OpenLORIS室内数据集对提出的线段检测与匹配算法效果和SLAM系统运行轨迹精度进行评估。

地面机器人的移动轨迹真值由其头顶携带的激光雷达与动态捕捉系统提供，前置Intel的RealSense D435i摄像头拍摄单目及深度图像，并提供IMU信息，如图5所示。实验使用CPU为Intel Core i5-11400H@2.7 GHz，内存为16 GB，系统版本为Ubuntu 18.04LTS64位。

图5 地面机器人

4.1 线特征提取与匹配实验

为验证ELSED算法的性能，在OpenLORIS数据集走廊场景和咖啡馆场景上分别与LSD算法进行对比测试，其中走廊场景(corridor)包括5个子序列(corridor1-5)，咖啡馆场景(cafe)包括2个子序列(cafe1-2)。统计线特征提取算法ELSED和LSD平均每帧耗时、线特征提取数量及线特征平均长度，其中ELSED采用跳跃检测模式，两者均剔除了长度小于Lmin的线段，结果如表2所示。

表2 线特征提取算法性能对比

由表2可以看出：ELSED和LSD两种算法提取的线段数量基本一样，在corridor序列上提取的线段数量均在140条左右，在cafe系列上提取的线段数量在130条左右；而ELSED算法耗时为2.4 ms，仅为LSD算法的13%左右，大大减少SLAM前端提取线特征时间；同时ELSED提取的平均线段长度为80像素，是LSD提取线段长度的1.3倍。

线段提取效果如图6所示，其中图6(a)原始图像来自corridor4系列，文中算法提取效果如图6(b)所示，LSD算法提取线段效果如图6(c)所示。可以较为明显看出，LSD提取某些长线段时，易受图片质量的干扰，使得本来的长线段被分割为多条短线，而ELSED提取出高质量的长线段，提取时间短，更有利于系统定位精度提升，使系统更具鲁棒性。

图6 线段提取效果

为定量评估文中提出的无描述符的线段匹配算法，使用ELSED检测线段，用正确匹配对、正确匹配率和匹配时间作为评价指标。其中正确匹配率定义为正确匹配对占总匹配对的比值，将线段重投影误差小于5像素的定义为正确匹配[18]，其余的定义为误匹配，分别在OpenLORIS数据集的走廊场景和咖啡馆场景做测试，结果如表3所示。

表3 文中线段匹配算法和LBD线段匹配方法对比

由表3可以看出：相比于文中算法，使用LBD获得的正确匹配对更多，但正确匹配率相对较低，仅为80%左右，而文中算法的线段正确匹配率均在96%以上。这是因为是文中的匹配算法含有较为严格的几何约束，对于匹配线段外形差别较大的容忍度低，减少线段无效匹配，提高SLAM系统的精度。其匹配效果如图7所示。

图7 线段匹配算法对比

同时文中算法的复杂度为O(n2)，与检测线段数量密切相关，在相关数据集上完成130条线段的匹配耗时大致在2.6 ms左右，相比于LBD描述子，时间效率提高了83%，大大缩短了连续帧之间的线段匹配时间，提高SLAM系统的跟踪速度。

4.2 定位精度实验

根据绝对轨迹误差(Absolute Trajectory Error，ATE)，即计算算法得到的相机位姿与真值之间的均方根误差(Root Mean Square Error，RMSE)来评估算法的定位精度。将ORB-SLAM3和文中算法在OpenLORIS数据集上进行对比，结果如表4所示。可以看出：相比于ORB-SLAM3，除了在cafe2序列上，文中算法定位精度略微下降，但在场景中纹理较少的走廊环境下，文中算法表现亮眼，尤其是在corridor3序列上，文中算法精度提高87%，在走廊场景下其他序列上，精度均有提高。

表4 多种算法的绝对轨迹误差对比 单位：m Tab.4 Comparison of APE of various algorithms Unit：m

图8为在corridor3和cafe1序列上，各算法轨迹在xy平面上与轨迹真值的对比，也可以发现文中算法轨迹与真值最为贴近，精度最高。图9(a)为corridor3序列上，各算法实时绝对位姿误差(Absolute Pose Error，APE)，可以看出文中算法APE波动较小，误差值在较低水平，算法效果稳定。图9(b)为误差箱形图，从中位数和平均数来看，文中算法优于ORB-SLAM3。图10分别表示了ORB-SLAM3算法和文中点线特征融合算法的在corridor3场景下地图构建效果。

图8 算法轨迹与轨迹真值对比

图9 实时轨迹误差(a)及误差箱形图(b)

图10 地图构建

4.3 系统耗时

ORB-SLAM3和文中算法在OpenLORIS数据集下各模块的平均耗时如表5所示。文中算法在ORB-SLAM3的框架中加入线特征，相比于ORB-SLAM3在特征提取阶段，用改进的ORB算法和ELSED算法并行提取点特征和线特征，故文中算法在特征提取时并未增加系统耗时。在连续帧跟踪时，利用本文作者提出的无描述符的线段匹配算法跟踪线特征，耗时增加2.8 ms。在创作关键帧时，需要提取LBD线段描述符，为局部建图线程三角化新地图线提供关键帧之间的线段匹配，使得耗时增加。文中算法的跟踪线程平均耗时29.82 ms，帧率为33 帧/s，满足系统的实时性要求。

表5 PC平台算法耗时对比 单位：ms

5 结束语

提出一种在ORB-SLAM3基础上加入线特征的点线特征融合的视觉惯性SLAM算法。为解决传统线特征提取算法耗时久和提取线段质量低的问题，引入ELSED线段提取算法，并进行参数调整，在短时间内可提取出高质量的长线段。传统线段匹配方式采用LBD线段描述子，对大多数场景鲁棒，但同样存在耗时过长的问题。本文作者提出的连续帧间的快速线段匹配算法，减少线段匹配时间和线段的误匹配。在公共的数据集上实验结果表明：与其他开源算法相比，文中算法具有更高的定位精度，且保证了系统的实时性。