vslam论文10：PL-VINS:具有点和线特征的实时单目视觉惯性SLAM

摘要

        PL-VINS是基于最先进的基于点的VINS- mono，开发的一种基于点和线特征的实时、高效优化的单目VINS方法。我们观察到，目前的作品使用LSD算法提取线条特征;然而,LSD是为场景形状表示而设计的，而不是为姿态估计问题设计的，由于其高昂的计算成本，这成为了实时性能的瓶颈。在本文中，我们通过研究隐藏参数调整和长度抑制策略来改进LSD算法。改进后的LSD算法的运行速度至少是LSD的三倍。此外，通过用Plucker坐标表示空间线，根据点到线的距离对线估计中的残差¨进行建模，然后通过迭代更新最小的四参数正交表示的Plucker坐标来最小化残差。开源:https://github.com/cnqiangfu/PL-VINS。

I. 引言

        据我们所知，PL-VINS是第一个具有点和线特征的基于实时优化的单目VINS方法。

（1）通过研究隐参数调整和长度抑制策略，提出了一种改进的LSD姿态估计算法。改进后的LSD算法的运行速度至少是LSD的三倍。

（2）点、线和IMU信息在基于优化的滑动窗口中有效融合，实现高精度姿态估计。

（3）在基准数据集EuRoc上的实验表明，在相同的姿态更新频率下，我们的方法比最先进的(SOTA) VINS-Mono方法产生更高的定位精度。

III. 系统概述

图2：系统概述。PL-VINS是基于VINS-Mono和我们之前的工作开发的，它实现了三个线程，包括测量预处理、本地VIO和环路闭合，蓝色矩形表示与VINS-Mono的差异。 

A.测量预处理

        PL-VINS从这个线程开始，它的功能是分别提取、对齐相机和IMU测量的两种原始信息。对于相机捕获的每个传入图像，点和线特征被同时检测、跟踪和细化。对于点特征，我们使用Shi-Tomasi进行检测，使用KLT进行跟踪，使用基于ransac的极极几何约束识别内层。对于线特征，我们针对姿态估计问题修改了原始LSD算法，其运行速度至少是原始LSD算法的3倍。然后用LBD描述线条特征，用KNN进行匹配。为IMU的原始陀螺仪和加速度计测量，我们遵循vins - mono的工作，在两个连续帧之间预积分它们。

        通过预处理信息，PL-VINS初始化触发下一个线程所需的一些值。首先，在几帧内估计出一个由比例姿势、空间点和线组成的图形结构。然后，图形与预积分IMU状态对齐。

B.局部VIO

        初始化后，PL-VINS激活基于紧密耦合优化的局部视觉惯性测程(VIO)线程，通过最小化所有测量残差来实现高精度姿态估计。首先，通过对帧间的二维点和线特征对应关系进行三角剖分，重构空间点和线;空间点由逆深度参数化，空间线由Plucker¨坐标参数化；接下来，采用固定大小的滑动窗口，通过联合最小化由式定义的几个残差函数，找到最优状态向量，包括位姿、速度、空间点线、加速度和陀螺仪偏差，如式所示。在输入新帧时，我们将滑动窗口中的最后一帧边缘化，以保持窗口大小不变。

C.环路闭合

        局部VIO后，PL-VINS判断当前帧是否为关键帧，取决于当前帧与最后一帧之间的视差是否大于某个阈值，或者跟踪的特征数量是否小于某个阈值。一旦当前帧被选为关键帧，PL-VINS就会激活循环关闭线程来搜索并确定是否发生了循环关闭。这个线程遵循原来的VINS-Mono的工作。

IV. 线特征融合

A. 线特征检测

        目前基于点和线的VINS方法直接使用OpenCV中的LSD进行线特征提取，如[10]，[21]- [23]，[27]，[28];然而，由于LSD的高计算成本，它已经成为实时性能的瓶颈。我们观察到原始的LSD算法是为场景形状表示而不是姿态估计问题而设计的。如图3所示。

        在这项工作中，我们基于OpenCV的源代码修改了原来的LSD算法来解决位姿估计问题。该改进过程首先通过研究隐藏参数调整来简化线特征检测，然后通过长度抑制策略过滤掉短线特征。因此，改进后的LSD算法的运行速度至少是LSD的三倍。

图3：LSD是一种无参数调整的线段检测器，专为场景形状表示而设计。在这种情况下，在782×480像素的图像中检测647个行特征需要62 ms;然而，对于姿态估计问题来说，大量的短线特征可以被视为离群值，因为它们很难匹配，并且可能在下一帧中消失，更不用说剔除局外人的困难了。在647个线特征中，长度超过20像素的线特征有107个，长度超过40像素的线特征有50个。上，长线条特征比短线条特征更容易匹配，更有可能出现在下一帧中。 

隐藏参数研究：LSD是无参数可调的线段检测器;然而，有趣的是，我们发现仍然有几个隐藏的参数可以调整以加快姿态估计问题的线检测。在这项工作中，我们使这些参数可见，所有这些参数都可以在我们的开源代码中找到。

        OpenCV使用N层高斯金字塔来创建多尺度表示，其中原始图像以固定比例r下采样N-1次，并使用高斯滤波器模糊N次。上述两个参数r和N在原始OpenCV函数中是可用的和可见的。在这项工作中，我们遵循一个标准设置r = 0:5和N = 2，之后，OpenCV设计一个图像尺度参数s = (0;1]对每层图像进行缩放，然后使用LSD提取线特征。

        LSD从一个线支持的区域划分操作开始，在这个操作中，图像被划分为许多区域，其中所有的点共享大致相同的图像梯度，每个区域都近似为一个矩形。随后，LSD定义了一个最小密度阈值d来拒绝线特征:矩形中对齐的点数[2]需要小于阈值。

        注意，我们观察到两个隐藏参数s和d对于减少线特征检测的时间至关重要。图4为研究过程。注意，减小s和d可以显著减少时间，而精度损失几乎可以忽略不计。在这项工作中，考虑到效率和普遍性，我们设置s = 0.5(默认为0.8)和d = 0.6(默认为0.7)。其余隐藏参数包括角度容差、图像梯度范数量化误差容差、细化触发等均采用默认值。

 图4：隐藏参数研究。结果通过在MH-04-Difficult序列上运行PLVINS w/o循环得到。Time表示修改后LSD算法的平均执行时间。

长度拒绝策略：前一个过程简化了检测过程。现在我们使用最小长度阈值Lmin来拒绝短线特征:

B. 线跟踪和内线细化

        在进行线特征检测后，我们分别采用LBD和KNN对线特征进行描述和匹配。特别地，对于可能存在的线异常点，我们执行内线细化来识别线特征内线

(1)由KNN计算的汉明距离需要大于30

(2)匹配线特征之间的角度需要小于0.1 rad。

C.空间线

        在前面的步骤中，我们在图像平面上建立了线特征对应关系。现在我们通过三角化这些对应来初始化πw中的空间线(地标)。

空间线表示：给定一条空间线Lw 2 πw，我们用Plucker坐标¨Lw = (n > w;d > w) > 2r6，其中nw2r3表示由Lw和πw原点所确定的平面的法向量，dw2r3表示由Lw的两个端点所确定的方向向量。然后，我们初始化Plucker¨坐标。假设Lw分别由相机位置为c1和c2的两帧在πw中观测。

        因此我们有两个平面由两个位置和Lw决定:π1 = (c1;Lw)， π2 = (c2;Lw)。根据[30]，对偶Plucker矩阵¨L w可以描述为:

正交表示： 的正交表示

可以用QR分解计算： 我们将其用于优化过程

 其中U和W分别表示三维和二维旋转矩阵。设R(θ) = U, R(θ) = W为对应的旋转变换，有:

D. 线重投影残差模型

        线重投影残差根据点到线的距离建模。首先，定义线几何变换，给定一个变换矩阵从πw到πc，其中Rcw和tcw 分别定义旋转和平移。有了这个矩阵，我们可以用πw变换Lw到πc

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

 E.基于点、线和IMU的滑动窗口优化

首先，我们定义与点、线和IMU测量信息相关的滑动窗口中的全状态向量X:

 接下来，我们定义滑动窗口中的目标函数:

         与VINS-Mono相比，本工作用一个额外的线重投影残差项eline修改了其目标函数，我们将定义它。设L为滑动窗口中观测到的线特征集合，(i;j) 2l表示滑动窗口内第i个相机帧ci观测到的第j条空间线Lj。根据式(9)，直线可以定义为

        最后，利用目标函数，从初始猜测X0迭代更新得到最优状态向量X: Xt+1 = Xt⊕δX;其中t表示第t次迭代，δ表示增量。我们特别介绍了如何更新线路。根据式(3)-(5)，可以通过迭代更新最小四参数标准正交表示来最小化直线: 

 V. 实验

        我们在基准数据集EuRoc上评估PL-VINS在定位精度和实时性能方面的性能。PL-VINS是基于Ubuntu 18.04和ROS Melodic实现的，所有实验都是在低功耗Intel Core i7-10710U CPU @1.10 GHz上进行的。

A. 准确性比较

ATE比较：

图7：VINS-Mono和PL-VINS在MH-04-Difficult序列上的轨迹和空间特征图比较。这两张图是ROS Rviz窗口的截图，其中黄线表示运动轨迹，白点表示空间点，橙线表示空间线。定量结果见表2。

 RPE比较：

        RPE包括平移误差和旋转误差，其评估结果取决于时间增量的设置。我们检查两种设置:固定时间增量= 1s和无时间固定增量。在我们的实验中，前者通常可以找到1000对姿势，而后者可以找到9000多对。

B. 实时性分析

 表4给出了三种方法在MH-04-difficult序列上的平均执行时间比较。