PL-VINS:实时基于点线的单目惯导SLAM系统
目录
基本情况
摘要
相关工作与主要贡献
主要内容
系统概述
A，观测值预处理
B，局部VIO
C，闭合回环
线特征
A，线特征检测
B，线段跟踪和内点保留
C，线特征三角化路标点
D，线重投影残差模型
E，基于点、线和IMU的滑动窗口优化
实验对比
A，精度比较
B，实时分析
总结
基本情况
- 标题：PL-VINS: Real Time Single-Camera Visual-Inertial Simultaneous Localization and Mapping with Point and Line Features
- 作者：Qiang Fu12∗, Jialong Wang1∗, Hongshan Yu1, Islam Ali2, Feng Guo1, Hong Zhang
- 出处：2020, arXiv
- 论文地址：https://arxiv.org/pdf/2009.07462.pdf
视频演示：https://www.bilibili.com/video/BV1464y1F7hk/
开源代码：https://github.com/cnqiangfu/PL-VINS
摘要
基于点特征的视觉惯性定位系统（VINS）的定位精度提升变得愈发关键,因为它们在结构化场景中提供了额外的结构性约束,然而实时性能方面却一直未能得到足够重视.本研究提出了一种改进型PL-VINS算法,该方法基于最新的单目VINS Mono框架进行开发,旨在通过引入实时优化技术来提高定位精度.研究者发现目前采用LSD算法提取直线段,但该算法虽然适用于场景形状结构表示,却不适合特定的姿态估计问题,其高昂的计算成本成为了制约实时性能的关键因素.本论文通过深入分析隐参数调节策略与长度抑制机制,提出了一种优化后的LSD优化算法 。实验表明该改进方案运行速度至少是传统LSD算法的三倍.此外,通过引入折痕坐标系表示线特征,将线重投影残差建模为线段中点至直线的距离,并采用迭代更新最小四参数正交表示的方式进行优化求解.在公共euro-benchmark数据集上的实验证明:在同一工作频率下,在低功耗环境下运行于1.1GHz CPU时,该方法相比VINS-Mono算法定位误差降低了12-16%。
。
vins-mono和PL-VINS的特征比较
相关工作与主要贡献
本文提出了PL-VINS，其特点包括：
- 从公开资料显示可知, PL-VINS作为首个基于实时优化框架的单目视觉惯性导航系统,实现了对点和线数据的有效融合（见表）。
- 为解决特定姿态估计挑战,本研究通过优化隐式参数并引入长度缩减策略,提出了一种改进型LSD算法,显著提升了计算效率(约三倍)。
- 点、线及IMU信息得以在基于优化设计的滑动窗口内精准融合,从而实现了系统的高精度定位与姿态估计。具体而言,将线重投影误差定义为空间中点到直线的最短距离,并通过迭代更新最小化的四参数正态表示来实现误差最小化。
- 经过系统评估,本方法在基准数据集EuRoc上表现优异,能够在低功耗环境下运行于Intel Core i7-10710U@1.10GHz处理器上,并在相同工作频率下显著超越了传统VINS Mono方案。
表显示了具有代表性的单目VINS方法与点、线或平面方法的比较。
本文提出了一种创新方案,该方案将传统SfM算法扩展至支持立体相机与激光雷达传感器的场景.该研究建立在基础概念之上:通过利用激光雷达的远距测距能力来抵消图像间的相对运动差异.具体而言,在实现过程中我们首先开发了一个基于立体视觉的SfM系统,该系统能够计算摄像机运动并估计视觉特征（结构）的空间位置.随后,在系统设计中我们整合了激光雷达生成的点云数据与视觉特征提取的结果,并将这两者纳入同一个优化框架中进行迭代求解.在此过程中,优化函数的目标是最优地确定摄像机运动参数以及三维空间中的物体结构信息.值得指出的是，默认情况下LiDAR数据通过双重途径增强了SfM算法的表现:
第一,它提供了丰富的三维几何信息;
第二,它能够有效抑制由于相对运动导致的数据不确定性.
LiDAR点云被用来识别并剔除无效图像匹配，从而通过增强基于立体相机的SfM方案对视觉模糊的能力来提高其鲁棒性。
LiDAR点云与视觉特征通过协同工作在联合优化框架中融合,从而有效地降低了运动漂移现象。我们的方法在运动估计方面表现出了比现有最先进的SfM算法更高的准确性和一致性。
本文的工作主要有以下几个方面：
- 1）将全局SfM技术集成到立体摄像系统中，并成功实现了各摄像头在实际尺寸下的初始运动估计。
- 2）激光雷达采集的数据用于筛选出有效的图像匹配对，并进一步保证了该方案的整体可靠性。
- 3）通过融合立体相机与激光雷达所提供的多源信息，并结合我们先前提出的一种联合优化方法（即联合优化方案），显著提升了所建模型的精度和一致性。
根据多信息融合框架的不同，目前的VINS方法分为：
A，基于Kalman滤波的VINS方法
第一个源自紧耦合Kalman滤波（KF）的方法即为VINS算法，在MSCKF的基础上发展而来。近期Patrick等人提出了一种开源的基于扩展卡尔曼（EKF）视觉惯性估计平台OpenVINS。该方案采用了Shi Tomasi算法作为视觉特征提取工具。以这一为基础Yang团队开发了多种改进型特征用于提升系统性能：包括线特征、平面特征以及线与平面结合使用的创新性解决方案。
B，基于非线性优化的VINS方法
在实际应用中采用非线性优化方案时会涉及 bundle adjustment （BA），其中姿态参数是经过优化处理以整合视觉信息与IMU数据等多方面的约束条件。本文致力于深入探讨基于优化算法的VINS方法，并对其进行了系统分类分析。
基于特征的方法
基于点和线的方法 。随着基于点的VINS系统的发展，在其中运用几何信息愈发显得关键。如直线和平面等几何元素，在本文中得到了特别的关注。本文主要从实时应用的角度探讨了基于点的方法，并重点研究了直线这一特征。如何将线这一特征成功地融入基于点的方法中，则是保证该技术有效性的关键所在。目前的研究多依赖于OpenCV中的LSD算法来提取直线，然而LSD算法主要用于结构化环境下的处理，在非结构化场景下存在诸多不足之处：一方面它无法有效地捕捉动态变化的情况；另一方面它对大量冗余线条表现出明显的敏感性，在实际应用中容易导致计算资源的巨大浪费以及异常值的产生。
实际上，在具体问题中，并非必须用线条来精确描绘场景环境；而是应重点提取出显著的直线特征。这些基于VINS的方法，在检测到直线后会运用LBD算法和KnnMatch技术来进行描述子计算与匹配配准。Gomez-Ojeda等人则采用几何约束条件来实现线段匹配并剔除离群异常值点。当完成直线配准之后，则可进行姿态估计工作。基于BA优化框架下实施的方法，则是通过同时最小化点位、直线方向及IMU约束的三个残差项来实现对VINS方法中摄像头姿态信息的有效估计
主要内容
系统概述
该文提出了一种基于实时优化的单目视觉惯导点线SLAM算法命名为PL-VINS。该算法通过有效提取和利用线特征信息，在性能指标上优于现有的VINS-Mono算法。本研究重点分析了不同线特征之间的融合机制及其对整体系统性能的影响。如图所示的是PL-VINS算法的整体架构示意图。
图 系统概述 基于对VINS-Mono先前工作的继承, PL-VINS实现了三个主要功能模块:首先是观测预处理阶段,其次是局部VIO算法,最后是回环检测机制.特别地，在图形化界面中使用不同颜色标记出其与传统方法的差异.
A，观测值预处理
PL-VINS系统基于这个线程启动，并旨在获取并对齐来自相机和IMU的原始测量数据。该系统能够并行地精确提取、追踪并优化图像中的点与线条特征。其中，在图像处理阶段采用Shi-Tomasi算法进行点特征检测，在运动估计阶段采用KLT方法追踪目标运动轨迹。为了提高系统的几何精度，在点与线条优化过程中引入了基于RANSAC算法的最佳几何约束机制。此外，在线特征提取部分利用OpenCV中的LSD算法直接实现线特征检测，并在此基础上进行了改进以实现实时性应用。该系统还结合了LBD算法（descriptor）与KnnMatch（match）方法来进行直线特性的持续跟踪与更新。
对于IMU测得的原始陀螺仪与加速度计数据而言，在遵循VINS Mono算法的基础上，在两个连续帧间完成其预积分计算。初始化阶段：通过预处理的数据对系统进行初始化配置，并在初始化完成后提供所需的状态参数及输入信号给后续线程运行支持。随后，在多帧数据中构建一个包含高精度相机姿态以及点特征与直线标记的整体图状模型结构。接着将此构建好的图状模型与包含速度信息、重力向量以及陀螺仪偏置项等在内的IMU预积分值实现精确配准对齐过程。特别地需要指出的是：在本算法中所涉及的IMU与相机之间的外部参数均为预先设定好的常数值或已知参数
B，局部VIO
初始化完成之后，在实现全局优化的过程中开启基于紧密耦合优化算法的局部视觉惯性里程计（VIO）线程
C，闭合回环
在选定当前帧作为关键帧时，会启动闭合循环线程。该线程的主要职责是搜索并判断轨迹是否构成闭合回环，并且同样可用于重新定位。
线特征
A，线特征检测
目前基于点和线的VINS方法直接采用OpenCV中的LSD进行线提取操作。因为该算法具有较大的计算复杂度，在实验中每帧60~80ms的时间开销已经成为了实时应用中的性能瓶颈问题。值得注意的是尽管LSD算法被设计用于场景形状表征但其核心设计并非专注于特定于位姿估计问题而是一体化解决方案。在实际应用中当系统探测到大量短长度线条时这些线条往往能够被直接归类为异常观测结果而对于那些具有明显特征的问题我们则倾向于通过直接提取显著线条来简化建模过程无需过多关注细节描述工作有趣的是我们还发现可以通过调节LSD算法内部的一些隐式参数来加速关键步骤的操作流程本文参考了OpenCV源码对其中的关键部分进行了优化改进实现了两方面的性能提升一方面是对隐式参数设置进行了优化另一方面则是通过筛选与长度不符的线条从而显著降低了计算负担使得整体算法效率得到明显提升
1, 隐藏参数优化调整。尽管LSD算法本身没有提供可调节的参数设置功能, 但仍有一些未被显式使用的隐含参数能够通过优化来加快检测速度, 本文将这些隐含的参数进行显式化处理。具体而言, 在OpenCV库中使用生成N层高斯金字塔来表示原始图像, 经过N-1次下采样处理后, 再随后又被模糊N次处理, 最后利用LSD算法提取每层图像中的线段特征。为了进一步提高算法效率, 我们对金字塔结构进行了简化设计: 实验表明, 在缩放比例设定为0.5且层数设置为2的情况下取得了良好的效果（尺度=0.5,N（层）=2）。此外, 在封闭矩形区域内的点集满足一定条件时, LSD算法会自动排除线段以避免噪声干扰; 其中我们将最小密度阈值设定为0.6以加速计算效率。需要注意的是, 在提取过程中计算量较大, 因此必须对生成的所有线段进行筛选以确保最终结果的质量与可靠性。
2. 排除不符合长度要求的线段。通过调整参数设置，可以简化算法的检测流程。通过施加长度约束条件来筛选和剔除不满足要求的参数。其中min(WI, HI)=480, η∈[0, 1.25]表示L_{min}=60。适用于帮助提取系统中η∈[1.25, +∞)的情况。因此，修改后的LSD运行速度至少是原速度的3倍。
B，线段跟踪和内点保留
我们所采用的流行直线跟踪策略包括以下步骤：首先通过应用LBD算法来计算各个线段的特征描述符；随后利用KnnMatch算法进行匹配；进而构建起帧间的基本线特征对应关系；在识别出可能出现的线性异常点时，在遵循几何约束条件的基础上应用了基于细化的线性方法来去除这些异常数据。
C，线特征三角化路标点
在之前的步骤中，在平面图像区域内创建二维平面内的线性特征匹配，在当前阶段我们采用多边形分解技术来推导出相应的三维特征参数。（这段可参考论文中的相关内容）
D，线重投影残差模型
基于直线重投影残差的模型即表示为：该模型将中点到给定直线的距离与该线段在其投影方向上的中点距离进行比较。在图像处理领域中的线条重投影误差可被定义为：
E，基于点、线和IMU的滑动窗口优化
通过最小化所述目标函数，调整状态向量从而获得最大后验估计：（详情参见论文）。
实验对比
本研究关注PL-VINS在公共EuRoc数据集中的定位精度及实时性能表现。所有实验均运行于配备英特尔酷睿i7-10710U处理器（频率：1.10GHz）的计算环境中。基于带有ROS-Melodic的操作系统下的Ubuntu 18.04操作系统平台构建的PL-VINS实现了高效的定位解决方案。
A，精度比较
为了评估PL-VINS的定位精度, 采用绝对轨迹误差均方根值 (RMSE_AT)、相对姿态误差均方根值 (RMSE_PO) 以及相对位置误差均方根值 (RMSE_PL) 作为评估指标. 基于VINS-Mono实现的PL-SLAM系统, 我们将该算法应用于EuRoC数据集中最具挑战性的轨迹片段进行验证. 表1展示了各项性能指标对比结果, 其中数值加粗显示了性能优势.
从这个表中，我们可以得出结论：
- 相对于VINS-Mono而言，在V1-02数据集之外的环境中表现更为出色。
- 闭合环路（Loop）是消除累积误差的关键步骤，在任何序列中都不可或缺。以VINS-Mono为例，在MH-04-hard序列中将位置精度从0.375优化至0.220。
- 综合对比结果可知，在多数场景下采用线路特性的辅助定位策略能够显著提升定位精度。
下图展示了3D运动轨迹的对比分析。图5中展示了一个MH-04的具体可视化案例，在此基础上，PL-VINS构建了线地标地图，并通过追踪该地图来推演运动轨迹。
V2-03困难数据集中VINS-Mono和PL-VINS环的三维轨迹比较。
EuRoc数据集MH-04困难数据集的轨迹和地标图比较
B，实时分析
对PL-VINS与基于优化的单目VINS方法进行对比分析，探讨其实时性能。该表格展示了三种方法在平均执行时间上的统计结果对比。其中线程1、2及3分别对应测量预处理、局部VIO以及回环闭合过程。
总结
该文阐述了PL-VINS系统及其核心创新之处：它是首个采用实时优化技术的单目视觉惯性导航系统（VINS）。在其中的研究框架下，在针对特定的姿态估计挑战时, 本研究通过探讨隐参数调节与线段长度抑制策略, 开发了一种改进型LSD算法以加速直线检测流程, 该改进型LSD算法适用于现有基于直线对应的技术方案。随后, 采用滑动优化窗口内的直线约束条件, 借助四参数正交表示法与中点至直线距离测量方法, 达到了高精度的姿态估算目标. 因此, 在同等运行速度条件下, PL-VINS系统较目前最先进的VINSMono方案展现出更高的定位精度