一种基于点和线的视觉惯性SLAM算法

A Point and Line Based Visual Inertial SLAM Algorithm

摘要: 传统的视觉惯性SLAM (Simultaneous Localization and Mapping)算法，在弱纹理场景下，往往存在定位精度差甚至失效的问题，本文提出一种基于点和线特征的视觉惯性SLAM算法。该算法以开源的VINS-Mono (Monocular Visual-Inertial Systems)系统为基础，在此基础上增加了线特征，结合点线特征各自独有的特性，并且提出一种新的数据选择策略，减少了因为线特征的加入而增加的计算量，保证了系统的实时性。实验采用开源数据集Euroc，通过与其他开源算法做对比，对本文算法进行评估，实验结果表明了本文算法的有效性。

Abstract: Traditional visual inertial SLAM (Simultaneous Localization and Mapping) algorithms often have poor localization accuracy or even failure in weak texture scenes. In this paper, a visual inertial SLAM algorithm based on point and line features is proposed. The algorithm is based on the open source VINS-Mono (Monocular Visual-Inertial Systems) system, which adds line features, combines the unique features of point and line features, and proposes a new data selection strategy, which reduces the computational burden caused by line features. The real-time performance of the system is guaranteed. The experiment uses open source data set Euroc and evaluates the algorithm in this paper by comparing it with other open source algorithms. The experimental results show the effectiveness of the algorithm in this paper.

1. 引言

视觉惯性系统VINS (Visual-Inertial System)，是融合相机和IMU (Inertial Measurement Unit)数据实现SLAM的算法，是机器人领域的一大关键技术。目前已有的视觉惯性SLAM系统，主要采用特征点法进行跟踪，首先从图像中选取特征点，常用的特征点包括著名的PTAM (Parallel Tracking And Mapping) [1]、SIFT (Scale Invariant Feature Transform) [2]、SURF (Speeded Up Robust Features) [3]、ORB (Oriented Fast and Rotated Brief) [4] 等等。基于点特征的SLAM系统依赖于点特征的数量和质量，当在走廊、窗户等弱纹理场景下，往往检测不到足够数量的点特征，因此会影响系统的精度，甚至出现跟踪失败的情况。为了改善基于点特征SLAM系统的性能，越来越多的人开始在点特征系统中加入线特征。Pumarola等 [5] 在ORB-SLAM [6] 的基础上加入线特征，提出了PL-SLAM (Visual SLAM With Points and Lines)，并提出利用线特征进行系统初始化的方法，是一种点线结合的单目视觉SLAM算法。HE Yijia等 [7] 提出的基于点线特征结合的视觉惯导联合里程计PL-VIO (Point-Line VIO)将线特征集成到优化框架中，以图优化的方法实现了更高的精度。Gomez-Ojeda等 [8] 在SVO (Semi-Direct Monocular Visual Odometry) [9] 的基础上提出了基于点线结合的半直接法单目视觉里程计PLSVO (Point-Line SVO)。谢晓佳 [10] 则采用普吕克坐标参数化空间直线，构建了一个点线综合的双目视觉SLAM系统。Qiang Fu等 [11] 在VINS-Mono [12] 的基础上增加了线特征，针对线特征的提取算法，进行了隐参数调整并提出长度抑制策略，改进最小二乘法，设计出一种具有点和线特征的实时单目视觉惯性系统。

以上是目前比较流行的点线结合的SLAM系统，线特征提取大多采用LSD (Line Segment Detector) [13] 算法，但由于LSD算法，运行较慢，不适用于SLAM这种对实时性要求较高的系统。Cuneyt Akinlar等 [14] 在2011年提出一种快速线段特征提取算法EDlines，其能够在线性时间内给出精确的结果，并拥有比LSD检测算法更快的提取速度，但精度与LSD算法持平。因此本文对EDlines算法进行改进，在保证原有运行速度的基础上，提高了算法精度；线段匹配采用LBD (Lind Band Descriptor) [15] 描述子方案，得到点、线特征数据后，提出一种数据选择策略，减少因为线特征的加入而增加的计算量，保证了系统的实时性，并以VINS-Mono系统为基础搭建了本文的SLAM系统。

2. 系统框架

本文SLAM系统共分为四大线程：测量数据预处理、局部视觉–惯性BA与重定位、回环检测和全局位姿图优化，如图1所示。首先对相机和IMU数据经行预处理，包括点线特征的提取与匹配、IMU预积分，得到点线特征和IMU预积分数据后进行数据选择，然后采用基于滑动窗口模型的非线性估计器，根据视觉约束、IMU约束以及回环约束构建联合优化函数并求解出滑动窗口内所有帧间位置、速度、旋转及偏置等，通过DboW词袋模型和BRIEF描述子经行回环检测，建立滑动窗口与回环候选帧之间的链接，最后是为了确保基于重定位的结果，对过去的位姿进行全局一致配置。该系统以开源的VINS-Mono系统架构为基础，在此基础上增加了线特征，并结合点线特征各自独有的特性。本研究将着重阐述关于线特征的主要算法。

Figure 1. System frame

图1. 系统框架图

3. 线特征提取

3.1. EDlines直线提取算法

EDLines是一种高效且快速的线段检测算法，无需参数调整只需为所有类型的图像运行一组默认参数即可，是一种快速、无参数的线段检测器，详细原理如下：首先提取边缘，具体分为以下4 步：1)将灰度图通过滤波器进行处理后输出平滑图像；2) 计算图像中各像素的梯度方向、梯度值；3) 将梯度图中梯度值的峰值点设为锚点；4) 连接锚点并绘制成边。

将上一步由锚点连接而成的边缘，利用最小二乘线拟合的方法进行线段的拟合提取，其原理是偏差绝对值之和最小：

$\min {\displaystyle \underset{i=1}{\overset{m}{\sum }}\left|{\delta }_i\right|}={\displaystyle \underset{i=1}{\overset{m}{\sum }}\left|\phi \left(x_i\right)-y_i\right|}$ (1)

其中 $x_i,y_i$ 是像素点坐标， $y=\phi \left(x\right)$ 是近似曲线， ${\delta }_i=\phi \left(x_i\right)-y_i$ 是近似曲线在点 $\left(x_i,y_i\right)$ 出的偏差。

得到的线段需要经过评估才能被认定为线段，评估的方法采用Helmholtz原理，通过计算NFA (Number of False Alarms)来确定提取的线段是否有效，具体原理为：对于一段由n个锚点组成的线段，其中k个点的梯度方向与线段的方向垂直，若其NFA计算的结果小于1，则认为线段有效，否则判定为无效线段，将之剔除。

$\text{NFA}\left(n,k\right)=N^4{\displaystyle \underset{i=k}{\overset{n}{\sum }}{p}^i}{\left(1-p\right)}^{n-i}$ (2)

3.2. EDlines算法改进

由于提取出的线特征存在多种干扰点，最小二乘法拟合直线时，因其需要尽可能包含全部局外点，会造成直线与局内点存在偏差，所以本文使用随机抽样一致性(RANSAC) [16] 算法解决线特征含有高噪声点的问题，RANSAC算法的原理是通过局内点的数据得出模型，拟合出的直线包含局内点的概率高。

RANSAC算法拟合像素链的步骤如下：首先假定单个需要拟合的像素链中有N个像素点，随机选择两个像素点，通过选择的两个点计算出两点所表示直线的模型方程 $y=ax+b$。然后设定阈值T，计算其余 $N-2$个像素点与模型方程的距离，统计出距离小于阈值的点的数量M，将这些点称为内点。最后按以上步骤迭代K次，M值最大时所得到的直线即为目标直线。

Figure 2. Comparison of linear fitting methods

图2. 直线拟合算法对比

使用RANSAC算法拟合直线，需要找出合适的算法参数，提高检测的准确度与合理概率。图2所示为一组数据分别使用不同的算法作对比，其中RANSAC的参数设定：模型阈值 $T=2$ ；迭代次数 $K=25$。在图2中就可以明显看出直线位置，但最小二乘法生成的结果却是错误的，这是由于图中存在较多局外点，当只有部分的数据符合模型时，最小二乘法拟合作用较差，无法使用，RANSAC算法可以有效解决此弊端。

由于噪声的存在，最小二乘法得到的拟合结果与预期结果相差甚远，使用RANSAC算法在生成的边缘图中拟合直线，减少了局外噪声点对结果产生干扰，在实际SLAM系统中经过多次测试发现，当 $T=3,K=28$ 时直线拟合效果最好，运行效果图如图3所示，图3(a)是改进前的运行效果图，图3(b)是改进后的运行效果图，改进后的EDLines算法，能够剔除低图像梯度的锚点，减少许多无效的短线段，提取出更多有效的长线段。

(a) 改进前 (b) 改进后

Figure 3. Comparison of actual effects

图3. 实际效果对比

4. 线特征匹配

提取到线特征之后要对线特征进行描述并加以匹配。线特征的描述和匹配方法有Zhiheng Wang等人提出的MSLD描述子 [17]，该描述子具有旋转、光照不变性等优点。Lilian Zhang等人在此基础上提出了LBD描述子 [15]，LBD描述子引入了全局、局部的高斯权重系数，相对于MSLD具有更好的匹配效果，同时拥有更快的计算速度。

LBD描述子首先会在线段处建立一个矩形的线段支持域LSR (Line Support Region)。利用条带 $\left\{B_1;B_1;\cdots ;B_m\right\}$ 形式表示，用m表示LSR条带数目，w表示条带像素宽度。在这个矩形支持域内，定义直线所朝的方向 $d_L$，与直线方向顺时针垂直的方向 $d_{\perp }$，这两种方向构成局部2D坐标系，局部坐标系的原点是该线段的中点。将LSR中的每个像素的梯度投影到上述局部坐标系中。

$g^{\prime }={\left(g^{\text{T}}\cdot d_{\perp },g^{\text{T}}\cdot d_L\right)}^{\text{T}}\triangleq {\left({g^{\prime }}_{d_{\perp }},{g^{\prime }}_{d_L}\right)}^{\text{T}}$ (3)

g是图片坐标系中的像素梯度， $g^{\prime }$ 是局部坐标系中的像素梯度。引入全局高斯函数 $f_g$ 来缓和在线段垂直方向上微小变化的敏感度，以及引入局部高斯函数 $f_l$ 降低边缘效应，避免描述符的突变。计算每个条带 $B_j$ 对应的特征向量 $B{D}_j$，将所有特征向量合并，形成LBD描述子：

$\text{LBD}={\left(B{D}_1^{\text{T}},B{D}_2^{\text{T}},\cdots ,B{D}_m^{\text{T}}\right)}^{\text{T}}$ (4)

将 $B_j$ 与相邻条带每一行的局部梯度进行分别求和。对于k行，有：

$\begin{array}{l}v{1}_j^k=\lambda {\displaystyle \underset{{g^{\prime }}_{d\perp }>0}{\sum }{g^{\prime }}_{d\perp }},v{2}_j^k=\lambda {\displaystyle \underset{{g^{\prime }}_{d\perp }<0}{\sum }{g^{\prime }}_{d\perp }}\\ v{3}_j^k=\lambda {\displaystyle \underset{{g^{\prime }}_{dL}>0}{\sum }{g^{\prime }}_{dL}},v{4}_j^k=\lambda {\displaystyle \underset{{g^{\prime }}_{dL}<0}{\sum }{g^{\prime }}_{dL}}\end{array}$ (5)

其中， $\lambda =f_g\left(k\right)f_l\left(k\right)$ 为高斯系数。每行的和放在一起，可以构成 $B{D}_j$ 对应的特征描述矩阵：

$\begin{array}{l}BD{M}_j=\left(\begin{array}{cccc}v{1}_j^1& v{1}_j^2& \cdots & v{1}_j^n\\ v{2}_j^1& v{2}_j^2& \cdots & v{2}_j^n\\ v{3}_j^1& v{3}_j^2& \cdots & v{3}_j^n\\ v{4}_j^1& v{4}_j^2& \cdots & v{4}_j^n\end{array}\right)\in R^{4\times n}\\ n=\{\begin{array}{l}2w,j=1,m\\ 3w,\text{else}\end{array}\end{array}$ (6)

最后对 $BD{M}_j$ 的均值向量 $M_j^{\text{T}}$ 和标准方差 $S_j^{\text{T}}$ 分别进行归一化，得到特征向量：

$B{D}_j={\left(M_j^{\text{T}},S_j^{\text{T}}\right)}^{\text{T}}\in R^S$ (7)

最后得到LBD特征：

$\text{LBD}={\left(M_1^{\text{T}},S_1^{\text{T}},M_2^{\text{T}},S_2^{\text{T}},\cdots ,M_m^{\text{T}},S_m^{\text{T}}\right)}^{\text{T}}\in R^{Sm}$ (8)

5. 数据选择策略

线特征的加入，无疑会增大计算量，降低系统实时性。为提高系统的实时性，文献 [18] 针对点线特征SLAM算法中，存在图像局部密集区域提取大量相似线特征、同一直线上的线段过度分割等弊端，利用梯度密度滤波器剔除图像中特征密集区域，降低了线特征的误匹配率加速了特征提取过程；文献 [19] 提出一种最优候选帧的关键帧选取策略，首先将疑似关键帧作为候选帧，并从多个方面评估选出最优帧作为关键帧，提高了整个系统的鲁棒性。文献 [20] 通过引入线段分组策略，合并潜在的同源线段、生成高质量长线段，提高了系统精度和实时性。为了降低引入线特征后的计算量，本节提出一种针对点特征与线特征的数据选择策略，对线特征的数量设置阈值，以点特征为主，线特征为辅，当点特征较多时，将线特征阈值减小，取少量线特征；当遇到弱纹理情况，对应点特征较少时，相应增大线特征阈值，进而提取出更多线特征，如此既保证了精度，亦保证了系统实时性。在实际系统中，阈值设置如表1所示，点特征阈值设置为固定数值150，线特征数目阈值上下限分别为100、20，其数目随点特征的数目变化而变化。

Table 1. New data selection strategy

表1. 新数据选择策略

(a) 点特征较多改进前 (b) 点特征较多改进后 (c) 点特征较少改进前 (d) 点特征较少改进后

Figure 4. Improved renderings

图4. 改进效果

图4展示了点特征较多和较少，这两种情况下，改进前后的实际运行效果图。其中图4(a)、图4(b)是点特征较多时改进前后效果图，图4(a)是改进前，此时追踪到的点线特征数目较多，但为了系统的实时性，利用数据选择策略，保持点特征阈值不变，减小线特征阈值，因此将减少提取到的线特征，改进后如图4(b)所示；图4(c)、图4(d)是点特征较少时改进前后效果图，图4(c)是改进前，此时图中光线较暗，属于典型弱纹理场景，点线特征较少，此时增大线特征阈值，获得更多的线特征，提高系统精度，改进后如图4(d)所示。利用此策略能够在保证统精度的前提下，同样保证系统实时性。

6. 实验结果分析

为了评估本文算法的性能，使用标准视觉惯性数据集Euroc [21]，该数据集中有许多弱纹理场景。因为本文算法以VINS-Mono系统为基础开发，所以将本文算法与VINS-Mono及以VINS-Mono为基础开发的点线特征系统PL-VIO、PL-VINS在同一台机器上进行对比。

测试平台为CPU为Intel i7-8700@3.2 GHz，8 GB内存，系统为ubuntu18.04。

利用SLAM精度评定工具EVO (Evaluation of Odometry)，对比内容是多种SLAM算法的均方根误差(Root Mean Square Error, RMSE)与轨迹。轨迹对比如图5所示，以MH-02-easy、V1-02-mdeium、V1-03-difficlut、V2-03-difficlut数据集为例，SLAM算法轨迹与真实轨迹越接近，精度越高，RMSE则选择Euroc全部的数据集，RMSE值越小说明系统的整体定位精度越高。

(a) MH-02-easy (b) V1-02-mdeium (c) V1-03-difficult (d) V2-03-difficult

Figure 5. Running track

图5. 运行轨迹图

通过与其他几种算法的轨迹对比，结果如图5所示，本文算法在不同难度数据集上均有良好的表现，由于MH-02-easy这个数据集比较简单，本文算法轨迹与其它3种SLAM算法轨迹基本一致，但是在V1-02-mdeium、V1-03-difficult、V1-03-difficult这三个数据集上，本文算法轨迹与真实轨迹更接近，精度要高于其它3种算法。

Table 2. RMSE comparison of various SLAM algorithms

表2. 多种SLAM算法的RMSE对比

表2表明，将使用改进的EDLines算法提取到的线特征整合到视觉惯性里程计中可以有效提高SLAM系统运动估计精度。本文提出视觉惯性SLAM系统，基于改进的EDLines算法，并通过点特征的数量对线特征设置不同阈值，在除MH_04_difficult以外的数据集中都得到了更高的精度。

7. 结论

本文以VINS-Mono为基础，提出了一种点线结合的视觉惯性SLAM系统，在原本点特征的基础上，增加线特征，并对线特征提取算法进行改进，得到了更高的精度，并提出了一种新的数据选择策略，既保证了精度，也保证了系统运行的实时性，解决了传统视觉惯性里程计在弱纹理场景下，由于特征信息缺失，导致定位精度差甚至失效的问题。并在开源视觉惯性数据集EuRoc上的实验结果表明本文算法能够获取更多有效的待匹配线段，提高了线特征匹配的准确度，同时具有很高的实时性，相比其它算法更具优越性。本文只是将线特征加入到后端优化中，基于此研究方向，可以继续改进算法，构建线特征词袋库，将线特征加入回环检测中，进一步提高精度。

致谢

首先感谢香港科技大学科研团队开源了VINS-Mono系统，让我学习到了一种非常优秀的视觉惯性SLAM框架，本文就是以VINS-Mono为基础，设计的点线结合的视觉惯性SLAM系统；感谢我的导师，本文是在导师耐心的指导下完成的，每一步都倾注了导师的心血和汗水；感谢所有引用文献的作者，正是他们的文章让我有了新的构思和设想；最后感谢在本文写作过程中所有提供帮助的老师、同学、朋友，是你们的帮助让我顺利完成文章，谢谢你们！