PL-VINS:实时基于点线的单目惯导SLAM系统

点云PCL免费知识星球，点云论文速读。

标题：PL-VINS: Real-Time Monocular Visual-Inertial SLAM with Point and Line

作者：Qiang Fu12∗, Jialong Wang1∗, Hongshan Yu1, Islam Ali2, Feng Guo1, Hong Zhang

编译：点云PCL

欢迎各位加入免费知识星球，获取PDF论文，欢迎转发朋友圈分享快乐。

论文阅读模块将分享点云处理，SLAM，三维视觉，高精地图相关的文章。公众号致力于理解三维视觉领域相关内容的干货分享，欢迎各位加入我，我们一起每天一篇文章阅读，开启分享之旅,有兴趣的可联系微信dianyunpcl@163.com。

摘要

利用线特征来提高基于点特征的视觉惯性SLAM（VINS）的定位精度越来越重要，因为它们提供了结构化场景中规则性的额外约束，然而，实时性能一直没有得到关注。本文介绍了PL-VINS，一种基于实时优化的具有点和线的单目VINS方法，它是在最新的基于点的VINS Mono的基础上发展起来的。观察到目前的工作是使用LSD算法来提取直线，但是LSD是为场景的形状结构表示而设计的，而不是针对特定的姿态估计问题，由于其昂贵的成本成为实时性能的瓶颈。本文通过对隐参数调整和长度抑制策略的研究，提出了一种改进的LSD算法。改进后的LSD的运行速度至少是LSD的三倍。此外，通过用pucker坐标表示线地标，线重投影残差被建模为中点到线的距离，然后通过迭代更新pucker坐标的最小四参数正交表示来最小化。在公共euro-benchmark数据集上的实验表明，在相同工作频率下，在低功耗CPU@1.1ghz下，该方法的定位误差比VINS-Mono算法低12-16%。

开源代码：https://github.com/cnqiangfu/PL-VINS。

vins-mono和PL-VINS的特征比较

相关工作与主要贡献

本文提出了PL-VINS，其特点包括：

• 据我所知，PL-VINS是第一个基于实时优化的单目点和线VINS方法（见表）。
• 针对特定的位姿估计问题，通过研究隐式参数调整和长度抑制策略，提出了一种改进的LSD算法，其速度至少是LSD算法的三倍。
• 点、线和IMU信息在基于优化的滑动窗口中有效融合，实现高精度姿态估计。线重投影残差被建模为中点到线的距离，然后通过迭代更新最小的四参数正态表示来最小化。
• 在基准数据集EuRoc上的定性和定量实验表明，在低功耗CPU Intel Core i7-10710U@1.10GHz上，在相同工作频率下，方法比VINS Mono获得了更高的性能。

该表显示了具有代表性的单目VINS方法与点、线或平面方法的比较。

本文提出了一种新的方案，它扩展了传统的SfM算法，使之适用于立体相机和LiDAR传感器。这项工作基于一个简单的想法，即激光雷达的远距离能力可以用来抑制图像之间的相对运动。更具体地说，我们首先实现了一个立体视觉SfM方案，它计算摄像机的运动并估计视觉特征（结构）的三维位置。然后将激光雷达点云和视觉特征融合到一个单一的优化函数中，迭代求解该优化函数以最优化相机的运动和结构。在我们的方案中，LiDAR数据从两个方面增强了SfM算法：

1）LiDAR点云用于检测和排除无效的图像匹配，使基于立体相机的SfM方案对视觉模糊具有更强的鲁棒性；

2）LiDAR点云与视觉特征在联合优化框架中相结合，以减少运动漂移。我们的方案可以实现比最先进的SfM算法更一致和更精确的运动估计。

本文的工作主要有以下几个方面：

1）将全局SfM技术应用于立体摄像系统，实现了摄像机在真实尺度下的运动初始化。

2） 激光雷达数据被用来排除无效的图像匹配，进一步加强了方案的可靠性。

3） 通过联合立体相机和激光雷达的共同的数据，扩展了我们先前提出的联合优化方案，提高了所建模型的精度和一致性。

根据多信息融合框架的不同，目前的VINS方法分为：

A，基于Kalman滤波的VINS方法

第一个基于紧耦合Kalman滤波（KF）的VINS方法可以追溯到MSCKF，一个多状态约束KF框架。最近，Patrick等人提出了OpenVINS，一个开源的基于扩展卡尔曼（EKF）的视觉惯性估计平台。这两个方案都使用了Shi Tomasi作为视觉特征。基于这个平台，Yang等人引入了线特征或平面特征或线与平面来提高OpenVINS的性能。

B、 基于非线性优化的VINS方法

在实践中，基于非线性优化的方法是基于捆集调整（BA）的方法，其中姿态是通过优化视觉和IMU等多个约束获得的。本文重点研究了基于优化的VINS方法，将其分为：

基于点的方法。第一种基于紧耦合非线性优化的方法可以追溯到OKVIS，它基于关键帧和BA实现。在此基础上，提出了一些基于优化的工作，其中，VINS Mono似乎是一个单目VINS基准，因为它具有很高的鲁棒性，特征跟踪使用Lucas-Kanade跟踪器（KLT），回环闭合使用DBoW2，4自由度姿态图优化和地图合并。它最近被扩展到立体视觉和立体+惯导。值得注意的是，目前的工作大多采用ShiTomasi作为特征提取工具，KLT作为特征跟踪器，这些工作可能会在具有挑战性的场景中产生低精度的点特征提取。在一些情况下，较差的角点和IMU信息不足以满足高精度定位的要求。

基于点和线的方法。在基于点的VINS系统中利用几何信息越来越重要，例如直线和平面。本文从实时应用的角度出发，重点研究了直线特征。如何将线特征引入到基于点的VINS方法中，是保证该方法有效性的关键。目前的工作大多直接使用OpenCV中的LSD进行直线提取，但是LSD是为结构化环境而设计的，而不是姿态估计问题，其中大量的直线可以被视为异常值，既不浪费计算资源，又容易产生异常值。

实际上，对于具体的问题，我们不需要用线条来精细地描述场景，而是提取明显的线条特征。这些VINS方法在检测出直线后，采用LBD和KnnMatch算法进行描述子计算和匹配。关于线匹配的工作Gomez-Ojeda等人采用几何约束来匹配和剔除线异常值。一旦建立了直线对应，就可以进行姿态估计。在基于BA的优化框架下，通过联合最小化点、线和IMU约束的三个残差项，可以估计出VINS方法中的摄像机姿态。

主要内容

系统概述

本文提出了一种基于实时优化的单目视觉惯导点线SLAM方法：PL-VINS，该方法有效地利用了线特征，改善了现有VINS-Mono方法的性能。本文重点研究了线特征融合的过程。pl-vins的一般结构如图所示。

系统概述。PL-VINS是在VINS Mono在以前的工作的基础上发展起来的，它实现了三个线程：观测 预处理，局部VIO和回环检测。蓝色矩形表示与VINS-MONO区别之处。

A，观测值预处理

PL-VINS系统是从这个线程开始，它的功能是提取和对齐相机和IMU两种测量的原始信息。对于相机捕获的输入帧（RGB图像），在图像中并行地检测、跟踪和优化点和线特征。这里使用Shi-Tomasi来检测点特征，KLT来跟踪，基于RANSAC的几何约束来保留最佳的点和线。对于线特征直接使用OpenCV中的LSD进行检测，并对LSD进行了修改，实现了实时应用，基于LBD算法（descriptor）和KnnMatch（match）跟踪直线特征。

对于IMU测量的原始陀螺仪和加速度计信息，这里遵循VINS Mono的工作，在两个连续帧之间对它们进行预积分。初始化：利用预处理的测量信息进行初始化系统，初始化后触发下一个线程所需的值和输入。首先，在多个帧中估计一个由高比例相机姿态和点、线地标组成的图结构。接下来，该图与IMU预积分值（包括速度、重力矢量和陀螺仪偏差）进行对齐。请注意，IMU和相机之间的外部参数是预先给定的。

B，局部VIO

初始化完成后，通过最小化所有测量残差，开启基于紧耦合优化的局部视觉惯导里程计（VIO）线程，用于高精度六自由度相机姿态估计。首先，通过对新帧和旧帧之间的点和线特征对应关系进行三角化来构建三维（3D）点和线特征，并分别通过逆深度和Plcker坐标进行参数化。其次，采用固定大小的滑动窗口，通过联合最小化多残差函数，找到最优状态向量，包括姿态、速度、3D特征、加速度和陀螺仪偏差。在输入新的图像帧时，我们将滑动窗口中的最后一帧边缘化以保持窗口大小。关键帧：遵循VINS Mono的关键帧选择标准：当前帧和最后一个关键帧之间的视差大于某个值或跟踪的特征数小于某个值。

C，闭合回环

当前帧被选为关键帧时，闭合回环线程被激活，它的功能是搜索和决定轨迹是否形成闭合回环，也可以用于重新定位。

线特征

A ，线特征检测

目前基于点和线的VINS方法直接采用OpenCV中的LSD进行线提取，由于其计算量大，实验中每帧60~80ms的耗时，已经成为实时应用的瓶颈。观察到LSD被设计用于场景形状表示，没有参数调整，而不是针对特定的位姿估计问题。对于检测到大量短长度线段，但是根据姿势估计问题，它们可以被直接视为异常值。对于特定的问题，我们不需要用线来精细地描述场景，而是检测出明显的线段。有趣的是，我们进一步发现可以调整LSD中的一些隐藏参数来加速检测过程。本文基于OpenCV中的源代码对LSD进行了修改，从隐藏参数调整两个方面加快了提取过程。

1，隐藏参数调整。虽然LSD没有参数调整，但仍有一些隐藏参数可以优化以加快检测速度，本文将其显式化。首先，OpenCV使用生成的N层高斯金字塔来表示原始图像，其中图像被下采样N-1次，模糊N次，然后使用LSD提取每层图像中的线段。我们简化了金字塔的尺度和层，实验上，尺度=0.5，N（层）=2效果良好。接下来，如果封闭矩形中的对齐区域点小于阈值，则LSD设置最小密度阈值以排除线段，其中我们将其设置为0.6以加快处理过程。请注意，提取过程也很耗时，但有必要剔除不可靠的线段。为了简化调优过程，所有的值都可以在我们的开源代码中找到。

2，排除长度不符合的线。以前的参数调整有助于简化算法的检测过程，我们使用长度约束来排除其中的一些参数。例如。min（WI；HI）=480，η=0:125表示Lenmin=60。适用于帮助提取系统η=125:0。因此，修改后的LSD的运行速度至少是LSD的3倍。

B, 线段跟踪和内点保留

我们采用的流行直线跟踪策略：使用LBD计算每个线段的描述子，然后使用KnnMatch进行匹配。然后建立帧间的初始线特征对应关系。对于可能出现的线异常点，我们采用了基于几何约束的线性细化方法，将异常点过滤掉。

C，线特征三角化路标点

在前面的步骤中，我们在图像平面上建立（2D）线特征对应，现在我们通过三角化对应来估计相应的3D线路标。（这段可查看具体论文中的介绍）

D,线重投影残差模型

直线重投影残差模型为中点到直线的距离即线段到投影线的中点距离。在图像中中的线重投影误差可以定义为：

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

通过最小化以下目标函数，优化状态向量以获得最大后验估计：（详情查看论文）

实验对比

这里评估了PL-VINS在公共EuRoc数据集上的定位精度和实时性能。所有实验均在英特尔酷睿i7-10710U处理器@1.10ghz上进行。PL-VINS是使用带有ROS-Melodic的ubuntu18.04实现的。

A、 精度比较

这里测试了PL-VINS的定位精度，通过绝对轨迹误差（ATE）和相对姿态误差（RPE）的均方根误差（RMSE）来评估精度：给定基于VINS-Mono的PL-SLAM，我们首先在EuRoc数据集最具挑战性的序列中比较它们。下表提供了ATE比较，其中粗体表示更好的结果。

从这个表中，我们可以得出结论：

1. PL-VINS除了V1-02数据集，其精度更高。
2. 闭合环路（Loop）是消除累积误差的必要步骤，适用于所有序列。以VINS-Mono为例，在MH-04-hard序列中将0.375降为0.220。
3. 从最后一行总结，PL-SLAM是更好的方法，因为通过额外利用线路特性，与VINS Mono相比，位置误差平均降低16%。

下图提供关于3D运动轨迹的直观比较。图5提供了MH-04中的一个可视化示例，其中，PL-VINS还重建了线地标地图，并通过跟踪它来恢复运动。

V2-03困难数据集中VINS-Mono和PL-VINS环的三维轨迹比较。

EuRoc数据集MH-04困难数据集的轨迹和地标图比较

B、 实时分析

将PL-VINS与基于优化的单目VINS方法进行比较，讨论PL-VINS的实时性。下表提供了三种方法的平均执行时间比较。其中线程1、2和3分别表示测量预处理、局部VIO和回环闭合。

总结

本文介绍了PL-VINS，这是第一个基于实时优化的单目点和线VINS。其中，针对特定的姿态估计问题，通过研究隐参数调整和线段长度抑制策略，提出了一种改进的LSD算法来加速直线检测过程。改进的LSD可用于现有的基于直线对应的姿态估计方法。此外，利用滑动优化窗口中的直线约束，通过四参数正交表示和中点到直线距离度量，有效地实现了高精度的姿态估计。因此，在相同的运行速率下，PL-VINS比最先进的VINSMono具有更高的定位精度。