基于点线特征融合的双目惯性SLAM算法

2022-04-26赵良玉金瑞朱叶青高凤杰

航空学报 2022年3期

赵良玉，金瑞，朱叶青，高凤杰

1.北京理工大学宇航学院，北京 100081 2.海鹰航空通用装备有限责任公司，北京 100070

同时定位与地图构建(Simultaneous Localization and Mapping，SLAM)被认为是实现移动机器人自主作业的核心技术，已被广泛应用于无人机、无人车和虚拟现实等领域。对于室内环境，由于建筑物会对GPS信号进行遮蔽，无人机定位多采用SLAM技术，为了克服单一传感器自身精度的不足，常采用多传感器融合策略，如视觉惯性就是一种有效的融合方法，且相机和惯性测量单元(Inertial Measurement Unit，IMU)均具有重量轻、成本低的特点，符合室内无人机轻量化和低功耗的需求。依据视觉与IMU的耦合方式，视觉惯性SLAM系统可分为松耦合和紧耦合2种类型。松耦合方法是融合视觉与IMU 2个模块分别独立进行运动估计后的结果，紧耦合方法则是利用相机与IMU的原始数据联合进行优化，更容易获得全局一致的估计结果，如OKVIS和VINS-mono等。从前端里程计的实现方法来看，SLAM系统又可分为特征法和直接法。与直接法相比，特征法更加鲁棒且抗光照变化的能力更强，其中点特征是最常用的特征类型，如经典的PTAM和ORB-SLAM2系统。但基于特征点的SLAM系统在遭遇场景纹理缺失，如走廊、楼道等弱纹理环境的时候，特征点提取效果较差，甚至可能追踪不到足够多的特征点来估计相机运动，从而导致运动估计失败。

为了解决上述特征点SLAM系统所存在的问题，国内外专家学者开展了大量研究工作。其中一种有效的解决方案是在前端提取更高层次的结构特征——线特征。与点特征相比，室内人造场景具有丰富的边缘和线性结构，线特征尤为明显，如图1所示。线特征具有较好的光照变化不变性和旋转不变性，可以提供更多的几何结构信息，因此线特征可以对点特征进行补充，提高基于单一特征点SLAM系统的鲁棒性和准确性。Gomez-Ojeda等依据SVO的框架提出基于双目相机的PL-SVO系统，其基本思路是利用LSD线段检测和匹配线段后，基于光度不变假设最小化各点与重投影点之间的光度误差，之后又将此运动估计方法运用到SLAM前端中提出双目PL-SLAM算法。Pumarola等参考ORB-SLAM的框架引入线特征，将误差函数定义为线段两端点的重投影点和测量线段之间的直线距离。贺一家等在VINS-Mono的基础上加入线特征提出PL-VIO系统。邹丹平等则基于曼哈顿世界假设，将结构线特征加入到StructVIO中，提高了相机方位的可观测性和全局精度。上述研究工作无一例外均采用LSD(Line Segment Detector)算法作为线特征检测的工具，但是LSD算法的设计初衷是用来表征场景的结构特征，提取速度较慢，且无参数调整的LSD算法在面对复杂背景或有噪声的图像时，易检测出过多的短线段特征，不仅浪费了线段检测、描述和匹配的计算资源，且易产生较多离群值，导致定位精度明显下降。此外，LSD算法通常存在线段重复检测和过分割问题，即存在过多邻近相似线段和长线段易被分割为多个短线段的情况，增加了SLAM系统线段检测和定位精度的不确定性，即同一位置不同时刻的线段检测结果差异性较大，且过多被检测到的邻近相似线段和分割产生的短线段特征使后续的线段匹配任务变得复杂，从而降低了SLAM系统的定位精度。

针对室内弱纹理环境，如楼道或走廊等，纯视觉SLAM系统容易丢失特征点，从而导致运动估计失败的问题，提出基于点线特征融合的双目惯性SLAM系统，采用紧耦合的方法有效地融合了点、线和IMU数据，并在后端采用光束法平差(Bundle Adjustmant，BA)方法实现了高精度的位姿估计。采用双目相机相较于RGBD相机测量范围更大，抗光照变化能力更强，与单目相机相比具有尺度恢复能力，鲁棒性更好。针对传统线特征检测算法速度慢且线段提取质量不高的问题，提出一种基于改进FLD(Fast Line Detector)算法的线特征检测器，通过长度抑制、近线合并和断线拼接策略，提升了线段提取的质量。传统特征SLAM所构建的稀疏地图主要应用于提升位姿估计的精度和回环检测以保证构建地图的全局一致性，即稀疏地图仅仅表达结构环境中特征明显的路标，因而无法有效判断地图中缺少路标点的空间是否空置。针对特征SLAM所构建的稀疏地图样式单一，主要被用于定位，不能应用于无人机避障、路径规划以及不够直观等可视化程度与可用性能力不足的问题，利用双目相机左右视差图和计算得到的相机位姿构建能满足多种应用需求的环境点线结构地图、稠密地图和导航地图。最后在公共数据集EuRoC和真实的室内弱纹理环境中进行性能测试，系统在提升准确性的基础上，运行速度更快。

图1 弱纹理环境中点线特征提取效果图Fig.1 Effect diagrams of point-line feature extraction in weakly textured environment

1 系统整体框架

基于点线特征融合的双目惯性SLAM系统共包括3个模块，分别为前端视觉惯性里程计、后端非线性优化和三维环境建图，系统整体框架如图2所示。

1.1 前端视觉惯性里程计

基于点线特征的前端视觉惯性里程计，采用双目相机输入的左右RGB图片，在图像中并行检测、描述和跟踪点线特征。其中点特征采用前后帧和左右帧2种跟踪模式，以前帧为基础，跟踪当前帧，以当前左帧为基础，跟踪当前右帧；线特征则在左侧相机图像的前后帧进行匹配。

1) 点特征：每帧图像提取Shi-Tomasi角点，利用KLT光流法实现特征点跟踪，并基于反向光流法跟踪剔除差异较大的点。

2) 线特征：左侧每帧图像采用改进FLD算法的线特征提取策略，并使用LBD(Line Band Descriptor)描述符来匹配前一帧中检测到的线段。

对于IMU测量得到的加速度计和陀螺仪数据，按照参考文献[17]提到的IMU运动模型在2个视觉帧之间进行预积分处理。

前端中另一项重要工作是初始化，目的是给整个系统提供优化求解的初值，采用视觉和IMU的松耦合方案。首先在纯视觉多帧中，利用运动结构恢复(Structure From Motion，SFM)方法估计出相机姿态和三维点线路标的空间位置，然后与IMU预积分数据对齐，求解出重力方向、尺度因子、陀螺仪偏置及每一帧对应的速度。

图2 视觉惯性SLAM系统整体框架Fig.2 System framework of visual-inertial SLAM

1.2 后端非线性优化

基于滑动窗口的紧耦合后端BA优化系统，采用固定大小的滑动窗口求解包括IMU状态(位置、速度、朝向、加速度计和陀螺仪偏置)及点线路标在内的最优状态向量，通过联合最小化多残差函数，包括边缘化的先验信息、IMU测量残差及视觉点线重投影残差，估计高精度的六自由度相机位姿，其中三维点线路标分别用逆深度和普吕克坐标参数化。相比于滤波求解，BA优化(非线性优化)方法更倾向于使用所有时刻的历史数据，它不仅考虑邻近时刻的特征与轨迹关系，更会考虑之前所有时刻的状态量，称为全体时间上的SLAM(Full-SLAM)，在这种意义下，BA优化方法使用了更多信息，从理论上来说也就更加精确，但计算量会随之提升。考虑到无人机上板载计算机的计算能力有限，本文在BA优化中，采用提取关键帧的方式严格控制需要进行的优化量，对于非关键帧，可用于定位检测，但不再用于建图优化。随着关键帧和路标点(空间地图点)的增多，后端BA优化的计算效率仍会不断下降。为了避免这种情况，本文使用滑动窗口(Sliding Window)将待优化的关键帧限制在一定的数量来控制BA优化的规模。

1.3 三维环境建图

通过双目相机左右目视差恢复点线特征深度，构建基于点线几何结构的环境地图，并在此基础上，综合双目视差得到的深度图和相机位姿，构建稠密面元地图以及用于导航的八叉树地图和ESDF(Euclidean Signed Distance Functions)地图。

2 线特征提取及参数化模型

2.1 改进FLD线特征提取算法

视觉SLAM中线特征检测常用LSD算法，其核心在于像素合并，但是合并像素的方法通常运算量较大。如在文献[20]给出的实验中发现，在480像素×317像素尺寸的图像中提取线段平均需要66 ms，且随着图像尺寸的增大而提高，这严重影响了视觉惯性里程计的实时性。

为解决LSD算法提取时间过长的问题，采用更加快速的FLD线特征提取算法，其应用边缘检测代替运算量较大的像素合并方法，能在线性时间内得到亚像素级准确度的直线段。针对线段提取质量不高的问题，利用长度抑制、近线合并和断线拼接策略加以修正，改进后的线特征提取算法在保留原有FLD算法快速性的同时，得到了更加有效的线段特征。最终实验表明，本文线特征提取算法具备较好的实时性和较高的线段提取质量。

2.1.1 FLD线特征提取算法

现有FLD线特征提取算法，具体步骤如下：

1) 图像灰度化：对于传感器输入的彩色图，根据图像各通道的采样值进行加权平均转化为灰度图。

2) 降噪：使用高斯滤波器滤除噪声并平滑图像，以防止由噪声引起的错误检测。

3) 计算图像中每个像素的梯度值和方向：利用常用的边缘差分算子(如Rober、Prewitt、Sobel)计算水平和垂直方向的差分和，最终得到梯度值和方向的计算公式为

(1)

式中：、表示像素点坐标；(,)表示该像素点的梯度值；表示该像素点方向。

4) 非极大值(Non-Maximum Suppression)抑制：对图像进行梯度计算后，将局部最大值之外的所有梯度值抑制为0。

5) 双阈值检测：为了滤除噪声或颜色变化引起的小梯度值而保留大梯度值，应用高、低阀值策略来区分边缘像素。

6) 滞后边界跟踪：搜索所有连通的弱边缘，如果一条连通的弱边缘的任何一个点和强边缘点连通，则保留这条弱边缘，否则抑制这条弱边缘。

7) 计算线特征参数：通过检测到的线段首尾两端点坐标，计算线段的长度、角度等基本参数，设提取到的线段首尾端点坐标分别为(,)和(,)，则线段长度和角度分别为

(2)

式中：len表示单帧图像条线特征中第条线特征长度；表示线段与水平方向的夹角；为线段序号。

2.1.2 长度抑制

传统线特征检测算法在面对复杂的真实场景时，会检测到大量的短线段特征，如图3(a)所示。过多的短线段特征不仅加重了线段检测、描述和匹配的计算成本，而且增加了线段误匹配的概率。对于点线特征融合的SLAM系统，线特征提供额外约束以提高位姿估计的精度，因此线特征检测算法只需要检测出场景中明显的线段特征，而不需要用过多线特征精细地构建场景。

由于长线段特征更稳定，更容易在多帧图像中被重复检测到，且数量越多，对定位精度的贡献越大，故取线特征长度约束的剔除准则为

(3)

图3 长度抑制测试结果对比Fig.3 Comparison of test results before and after length suppression

2.1.3 近线合并和断线拼接

常用线特征提取算法经常将一段较长的线段分割成几个较短的线段，此外在一些细长区域的边缘处会被多次检测，导致提取到的线段可能离得比较近，存在大量相似线段。这些线段提取质量不高，往往使后续的线特征匹配任务变得复杂，从而增加了线段检测和系统定位精度的不确定性。为了解决上述线特征提取中存在的问题，提出“近线合并”和“断线拼接”方法来改进FLD算法的提取结果，提升线段提取质量，如图4所示。

算法流程如下：首先按线段长度对提取出的线段进行降序排列，得到{,,…,}，其中表示第条线段。考虑到较长的线段特征往往来自具有连续强梯度的图像区域，提取精度更加可靠，因而从最长的线段开始进行线段分组。如图5所示，按照需要合并或拼接线段出现的位置不同，存在首尾2种情形，设线段和为近线合并改进措施下需要合并的2条线段，其首尾端点分别为(,)、(,)和(,)、(,)；线段和为断线拼接改进措施下需要拼接的2条线段，其首尾端点分别为(,)、(,)和(,)、(,)。

设当前最长线段为，则剩余线段集合为

图4 近线合并和断线拼接Fig.4 Merge of near line segments into a new one, and merge of chain short line segments into a longer one

图5 近线合并和断线拼接算法示意图Fig.5 Schematic diagram of algorithm for merge of near line segments and merge of chain short line segments

={,,…,}

(4)

经过角度筛选得到候选线段组：

={∀∈:(|-|<)}

(5)

(6)

式中：为线段集合中的线段，其长度为len，与水平方向的夹角为；为衡量线特征角度相近程度的角度筛选阈值，经实验分析将其值设为π/90；为自适应比例系数函数，由实验所得数据拟合而成，与线段长度len成反比，len越小，系数越大，即线段长度越短，可合并性越大。

经过水平距离筛选分别得到近线合并候选线段组1和断线拼接候选线段组2：

① 近线合并：

1={∀,∈:(|-|<)or

(|-|<)}

(7)

② 断线拼接：

2={∀,∈:(|-|<)or

(|-|<)}

(8)

经过垂直距离筛选分别得到近线合并候选线段组1和断线拼接候选线段组2：

① 近线合并：

1={∀,∈1:(|-|<)or

(|-|<)}

(9)

② 断线拼接：

2={∀,∈2:(|-|<)or

(|-|<)}

(10)

式中：为衡量线特征水平和垂直距离接近程度的距离筛选阈值，经实验分析将其值设为3。

依据上述筛选条件，将角度和空间上都与相接近的线段分为一组，最终得到候选线段集={1,2}。考虑到需要对每一帧图像频繁地进行距离阈值的筛选操作，二维欧几里得距离会增加一定的计算量，给算法的实时性带来挑战，式(10)中选择以水平、垂直距离筛选取代二维欧几里得距离筛选，由于仅涉及绝对值的加减运算，大幅减少了计算代价。并且第2轮垂直距离筛选操作的线段组，是在进行完第1轮水平距离筛选操作后，所剩下满足阈值要求的线段组，这极大地减少了第2轮所需进行距离筛选的线段数量，也进一步提升了筛选效率。

将加入经过筛选的候选线段集中，组成新的线段集合{,}。从中分别选择首尾端点中偏离最远的两端点作为新线段的首尾端点，合成新的线段，重新计算其角度，若满足<，则合并成立，并以合并线段替换线段集合{,}，否则由于合并前后角度相差过大，合并结果偏离原线段，则放弃合并。

重复进行上述步骤1)至步骤5)直到无法再合并和拼接为止。

图6(a)为原始算法检测出的线段，红色虚线框中表明线段存在近线和断线情况，图6(b)为改进后的算法检测结果。可以看出，算法已将邻近线段合并和断线拼接在一起，如图中红色虚线框中所示，大大优化了算法线段提取的质量。

图6 近线合并和断线拼接测试结果对比Fig.6 Comparison of test results before and after merge of near line segments and of chain short line segments

2.2 直线参数化

(11)

式中：和表示齐次因子；是一个6×1向量，包含和，是直线的方向向量，是过该直线且通过原点的平面的法向量，垂直于，即=0，如图8所示。

普吕克坐标六参数的形式大于空间直线四自由度，依然是一种过参数化的表达方式，且存在正交约束，会增加后端优化求解的难度，故参考Bartoli和Sturm提出的四参数最小化表示方法，在后端优化中采用正交表示法。对普吕克坐标进行QR分解，其正交表示为(,)∈SO(3)×SO(2)。

图7 线特征重投影残差Fig.7 Re-projection error of line feature

图8 普吕克线坐标系Fig.8 Plücker line coordinates

(12)

(13)

正交表示与普吕克坐标之间的转换关系有

(14)

式中：表示矩阵的第列。

2.3 直线的观测模型和重投影残差

空间直线的观测模型存在3D-3D和3D-2D等多种定义方式，通常采用精度更高的3D-2D方式将空间直线重投影到图像上，然后计算投影直线和图中匹配直线间的误差。类比于空间中三维点到图像坐标系的转换方式，将直线转换到相机坐标系，记为，并将用普吕克坐标表示，

(15)

式中：为直线变换矩阵(由旋转矩阵和平移向量构成)；()^表示平移向量的反对称矩阵。将投影到图像平面得到，其普吕克坐标表示为

(16)

式中：表示直线的投影矩阵，、和、分别为相机的焦距和主点。

(17)

式中：=[,,]。

3 基于多源信息融合的后端滑窗优化

(18)

通过最小化目标函数(包括IMU残差、点线特征残差和先验残差)来优化滑动窗口中的所有状态变量：

(19)

(20)

4 多地图构建

SLAM算法自建立之初便以定位和建图作为其两大核心任务，对于建图部分，传统基于特征点的视觉SLAM算法，每帧仅采用几百个点的像素信息来计算空间路标点，导致建图稀疏，所建地图无法有效表示周围环境，且点云存储规模较大，无法在有效的内存中建模较大的环境。本文多地图构建模块有助于解决传统SLAM算法后端建图样式单一，可视化程度与可用性能力不足的缺陷，构建多种能适应不同应用需求的地图样式，包括点线特征结构地图、稠密面元地图、八叉树地图和ESDF地图。加入线特征后，在提升系统精度的同时构建更高层次的环境点线结构地图，主要用于SLAM系统重定位和回环检测时的位姿校正，但所建地图仍为稀疏特征地图。考虑到双目相机左右目视差可以恢复图像深度，以及无人机后续导航的需要，构建易于可视化场景外貌特征的稠密面元地图和导航地图。在导航地图的表达方式中，选择存储容量更小的八叉树地图和ESDF地图。八叉树地图使用分层的八叉树结构存储占据的概率大小，然而多数路径规划方法要求直接给出到障碍物的距离而不仅仅是前方障碍物占据的概率大小，因而采用ESDF进行地图构建。使用Voxblox给出的Grouped Raycast方法，可以实现从TSDF(Truncated Signed Distance Functions)地图到ESDF地图的快速构建。TSDF是一种将结构化点云数据映射到一个预先定义的三维立体空间中，并用截断符号距离函数表示真实场景表面附近区域的地图重建算法。由TSDF融合后的ESDF地图可以快速对障碍物进行距离和梯度信息的查询，并解决了原生TSDF需要已知地图大小且内存开销过大的问题，长期在路径规划中被用于碰撞检测以及查找较大的空置区域。

考虑到建图算法的计算量会影响SLAM系统的实时性，在后端开辟一个新的线程接收来自前端传感器的左右相机图像和后端优化后的位姿数据，并将系统统一集成到ROS机器人操作平台下，便于后续无人机导航工作的开展。

5 实验与分析

为验证线特征提取的快速性和SLAM算法的有效性，对线特征提取的耗时进行统计，算法有效性的验证分为2部分：① 在公开数据集进行仿真并与一些优秀的算法进行对比；② 在真实楼道环境中进行实验验证。PC平台使用Intel NUC8i7BEH迷你主机，具体配置如下：CPU为IntelCorei7-8559U处理器，主频为2.7 GHz-4.5 GHz，内存为8 G，无GPU，系统版本为Ubuntu 16.04 LTS 64位，ROS Kinetic。

5.1 线特征提取算法性能比较

为验证线特征提取算法的性能，选择EuRoC数据集中4个场景提取难度最大的子序列MH_04_difficult、MH_05_difficult、V1_03_difficult、V2_03_difficult，分别进行3次实验，统计线特征提取算法LSD、FLD和改进FLD平均每帧的耗时和线特征提取数量，结果如表1所示。本文算法(改进FLD)，在参数调整的基础上融合了长度抑制、近线合并和断线拼接策略，相比使用原始FLD算法(平均每帧约21.6 ms)的线特征提取耗时有所增加，平均每帧耗时约为23.1 ms，平均增幅在7%左右，但仍处于较低水准。LSD线特征提取算法耗时最长，平均每帧约49 ms，算法耗时是本文的2倍多。本实验中EuRoC数据集输出频率约为20 Hz，特征点提取耗时约为15 ms，本文算法的点线特征提取线程耗时共计约为38 ms，满足实时运行要求，基于LSD的线特征提取明显超时。

由表1中线特征提取数量可以看出，LSD算法提取线段数量最多，FLD算法次之，本文算法最少，相对于FLD算法平均降幅在77.8%左右，提取效果如图9所示，其中图9(a)、图9(c)、图9(e) 为Machine Hall场景下测试结果对比，图9(b)、图9(d)、图9(f)为Vicon Room场景下测试结果对比。可以看出，使用LSD和FLD等传统算法提取线段时，会有大量短线段特征存在，增加线段检测和匹配的计算成本，且一些长线段被分割成许多短线段，以及邻近相似线段的存在使后续的线段匹配任务变得复杂，增加了线段检测和系统定位精度的不确定性。改进后的FLD算法使用长度抑制策略，大大减少了短线段特征的数量，且保留了对系统定位精度影响较大的长线段特征。使用近线合并和断线拼接方法，再次改良了线段提取的质量，有效地提升了系统精度。

表1 3种线特征提取算法性能对比

实验结果表明，改进后的FLD算法，线段提取时间少，减少了后端线特征重投影残差的求解时间，保证了算法的实时性。且提取到的长线段特征明显，线段质量高，利于系统定位精度的提升，鲁棒性更好。

图9 3种线特征提取算法效果对比Fig.9 Comparison of results of three line feature extraction algorithms

5.2 公开数据集EuRoC仿真实验

为验证双目视觉惯性SLAM算法的有效性和准确度，使用公开的EuRoC数据集对算法进行仿真实验对比，该数据集由无人机挂载VI-Sensor记录200 Hz的IMU数据和20 Hz的双目相机数据。图10为本文算法在EuRoC数据集MH_04_difficult序列运行过程中的效果图及构建的特征地图与轨迹的显示界面。

图11为EuRoC数据集序列中室内弱纹理场景下本文算法的特征提取对比图，其中图11(a)为点特征提取图，图11(b)为线特征提取图。可以看出，在光线较强或较弱以及特征不丰富的弱纹理场景下，点特征提取较为稀疏。而室内多为人造结构化场景，具有丰富的边缘、直角等线性结构，利用线特征提取算法仍能获得丰富的线特征，点线特征互补的方法相较于单一特征点提取，可为后续运动估计提供更加丰富且鲁棒的特征信息。

VINS-Fusion算法是香港科技大学开源的基于优化的多传感器状态估计器，前端采用光流法，支持双目视觉惯导SLAM方案；PL-VIO算法是贺一家博士开源的使用点和线特征的紧耦合单目视觉惯性里程计，在VINS-mono的基础上增加LSD线特征提取器。不同算法在EuRoC数据集(共11个子序列)下的运动估计误差如表2所示，采用绝对轨迹误差(Absolute Trajectory Error，ATE)评估算法的准确性，即计算估计位姿与真实位姿之间欧氏距离的均方根误差(Root Mean Square Error，RMSE)和最大值。在与真实轨迹作对比时使用EVO(Evaluation of Odometry and SLAM)工具进行数据对齐和误差计算，其中均方根误差为同时考虑平移和旋转两部分得到的最终误差，表中加粗值为当前数据集序列下3种算法中的最小均方根误差值。

图10 MH_04_difficult数据集仿真界面图Fig.10 Simulation interface of MH_04_difficult dataset

图11 弱纹理场景下特征提取对比Fig.11 Comparison of feature extraction in weakly textured environment

从表2可以看出，加入线特征后算法准确度明显优于纯点特征视觉SLAM算法，本文采用点线VIO系统，因线特征提取质量高，定位精度优于同类型PL-VIO算法，除了MH_01_easy和MH_02_easy两个简单序列下的精度略输于PL-VIO，其余场景均为最优，在场景纹理缺失的MH_04_difficult和相机快速运动的V2_03_difficult序列下，算法均表现出优异的稳定性，没有出现PL-VIO在V1_02_medium中特征丢失的情形(如表中“×”所示)。系统采用双目相机也有效避免了单目相机尺度不确定的缺陷，在提升系统精度的同时，更加鲁棒。由表2中数据和图12(a)各算法在EuRoC数据集下的均方根误差柱形图可以得出，本文算法与仅使用点特征的VINS-Fusion比较，平均定位误差下降了68%；与使用单目点线特征的PL-VIO比较，平均定位误差下降了30%。图12(b)为MH_04_difficult序列下，各算法平面轨迹与真实轨迹对比图，可以看出本文算法与真值间的差距十分接近，精度较高。

不同算法在EuRoC数据集下各模块平均每帧的计算时间如表3所示，其中点特征提取与追踪、线特征提取和线特征追踪模块均属于图2系统框架的前端线程，位姿优化属于后端线程，多地图构建则属于建图线程。从表3可以看出，本文采用改进FLD的线特征提取算法计算时间约为PL-VIO算法的一半，平均每帧耗时为25 ms，前端点线特征提取与追踪线程耗时共计50 ms，而EuRoC数据集输出频率为20 Hz，系统满足实时运行要求。多地图构建线程相对比较耗时，平均每帧约为60 ms，其余各模块的计算时间，3种算法基本保持一致。

表2 不同算法在EuRoC数据集下的运动估计误差Table 2 Motion estimation errors of different algorithms in EuRoC dataset

图12 不同算法仿真结果对比Fig.12 Comparison of simulation results of different algorithms

表3 不同算法在EuRoC数据集下的平均计算时间

系统在定位的同时，后端建图模块同步构建环境地图，如图13所示，为算法在运行EuRoC数据集MH_01_easy、V1_01_easy和V2_01_easy子序列下的建图结果，包括稀疏的点线特征结构地图、稠密面元地图、稠密八叉树地图和ESDF地图。线特征的加入为稀疏的点特征地图增加了场景的结构化特征，如边缘、直角等，也便于系统建立更高层次环境地图。稠密面元地图则能直观恢复EuRoC数据集录制地点的真实场景外貌。与多数SLAM系统加入深度相机后构建的稠密点云地图不同，稠密八叉树地图和ESDF地图存储规模更低，稠密八叉树地图以体素信息来表示空间中障碍物占据的概率大小，ESDF地图则能快速对障碍物进行距离和梯度信息的查询，针对应用需求的不同均可为后续无人机的自主导航提供先决地图数据。对图13中同一数据集序列下的四中建图结果进行横向对比，可以看出面元地图和八叉树地图在障碍物表现上做到了一致，轮廓清晰，地面的呈现也较为平整，与ESDF地图中占有和空置区域形成了对应。

图13 地图重建结果Fig.13 Map reconstruction results

5.3 室内弱纹理长廊环境测试

为验证算法在实际弱纹理环境中的性能，使用由移动平台TurtleBot搭载Intel NUC主机和Mynteye双目惯性导航相机(相机帧率30 Hz，IMU帧率200 Hz)采集的实际场景数据对算法进行测试，实验采集设备如图14(a)所示，实验轨迹如图14(b)所示。该数据集主要分布在北京理工大学中关村校区宇航楼长走廊环境下，走廊内白墙特征明显，特征点提取困难，且光照变化差异大，对算法性能具有较大挑战性，如图14(c)所示。经测试，本文算法在白墙较多、特征点提取不足的长走廊环境下，能够提取到丰富的线特征，与PL-VIO算法提取到的线特征相比，滤除了较短的线段和相似的邻近线段，对于过分割产生的线段也进行了拼接，如图14(d)、图14(e)所示，算法提取线段质量更高，大大降低了线特征的误匹配率，提升了系统定位精度。

图14 弱纹理长廊环境下不同算法仿真结果对比Fig.14 Comparison of simulation results of different algorithms in weakly textured corridor environment

不同算法在室内长廊数据集下的运动估计误差如表4所示，采用与第5.2节一致的绝对轨迹误差(ATE)来评估算法的准确性。从表中可以看出，本文算法均方根误差和误差最大值均为3种算法中的最小值，定位精度最高。图15为本文算法与真值间的绝对轨迹误差显示，从图中可以看出本文算法全段均位于较低误差区间内，与真值间的差距十分接近。