vslam论文18：S-VIO:利用的结构约束的RGB-D视觉惯性里程计（RAL 2023）

摘要

       基于视觉的定位是自主系统的一个关键问题，而基于视觉的里程计在具有挑战性的场景下性能会下降。这篇文章介绍了S-VIO，一种RGB- D视觉惯性里程计(VIO)，它充分利用多传感器测量(即深度，RGB和IMU)，异质地标(即点，线和面)和环境的结构规则来获得鲁棒准确的定位结果。为了检测环境的潜在结构规律，提出了一种两步亚特兰大世界推理方法。利用VIO系统估计的重力方向，该算法首先从一组最近优化的平面地标生成水平亚特兰大轴假设。然后，根据观测值的持久性，使用以下平面地标和线簇来过滤掉偶尔观测到的轴。剩下的坐标轴将保存在亚特兰大地图中，以备将来重新观测。特别地，采用了一种高效的 mined-and-stabbed(MnS)方法对结构线进行分类，并从每个线簇中提取消失点。此外，提出了一种结构线特征的闭式初始化方法，利用已知方向获得较好的初始估计。S-VIO集成了上述新特性，在两个公开的真实世界RGB-D惯性数据集上进行了测试。实验表明，S-VIO算法比现有的VIO算法和RGB-D VIO算法具有更好的精度和鲁棒性。

开源：oranfire/SVIO: a tightly-coupled optimization-based RGB-D VIO system (github.com)

I. 引言

       一些研究人员提出整合RGB-D相机的深度信息来提高纯VIO/VI-SLAM系统的性能。研究表明，深度信息对VIO的初始化和优化过程都有好处。其他研究人员利用线和平面特征来提高低纹理环境的鲁棒性。此外，一些工程利用人造环境的结构规律来纠正旋转漂移。与传统的基于点的VIO/VI-SLAM系统相比，上述工作显示出更好的鲁棒性和准确性。然而，尽管RGB-D VI传感器套件被广泛使用，但仍然很少有作品充分利用这类传感器套件提供的信息。

       基于上述理解，本研究提出在RGB-D惯性设置下，利用异质地标(即点、线和面)与结构规则(即亚特兰大世界假设)相结合的优势。来自不同模态(即深度、RGB和IMU)的测量被充分利用，并以紧密耦合的方式融合在一起，以提高性能。此外，垂直主导方向和重力方向之间的巧合被利用来促进对亚特兰大世界的推断。

综上所述，本文的主要贡献有:

（1）第一个开源的紧密耦合RGB-D VIO算法，结合了环境的点、线、平面地标和结构规则性，实现了较高的定位精度和鲁棒性。

（2）利用估计的重力方向和序列信息从RGBD图像序列中检测稳定的主导方向，提出了一种两步亚特兰大世界推断方法。

（3）一种利用已知直线方向的闭式结构直线初始化方法，以获得较好的初始估计。

       此外，作为提出的亚特兰大世界推断方法的一部分，我们实现了一种改进的mined-and-stabbed算法用于结构线聚类。与原算法[5]不同的是，我们的实现避免了繁琐的系数计算和高次代数方程求解，从而提供了精确且易于计算的解。

III. S-VIO概述

 图1：S-VIO的框架。对于vins- mono和DUI-VIO，新增加的组件颜色为橙色。

A.系统概述

       所提出的S-VIO系统的总体框架如图1所示。我们的系统采用同步的RGB-D图像和IMU测量作为输入。S-VIO的基本结构在VINS-Mono和DUI-VIO上进行了修改。特别地，S-VIO中并行运行两个主线程:特征跟踪和状态估计。同步的RGB和深度图像被传递给特征跟踪线程进行特征检测和跟踪。平面上检查模块根据平面分割结果为平面分配共面点和线特征。同时，对两个相邻帧之间的IMU测量值进行预集成。然后，状态估计线程总结特征信息和IMU预积分结果，用于估计器初始化和姿态估计。对于估计量初始化部分，我们采用了DUI-VIO提出的方法。对于状态估计部分，执行基于滑动窗口的优化以获得最大后验估计(章节III-C)。优化后，采用离群值去除方法对重投影误差较大的特征进行识别和丢弃。然后，我们检测亚特兰大世界模型(Section IV-A)，并将特征与先前检测到的优势方向相关联

       其中，特征跟踪线程负责点、线、面三种特征。为了提高效率，它们在三个并行线程中被检测和跟踪。用Shi-Tomas角检测器检测点特征，用Kanade-Lucas-Tomasi (KLT)光流法跟踪点特征。

       对于线特征的检测和跟踪，我们实现了一种将FLD与基于klt的样本点跟踪相结合的方案。此外，属于同一行的折线段被合并为一条长线，以便更好地估计。使用AHC算法在深度图像上分割潜在的多个平面区域，然后使用最小二乘法拟合到三维平面中。为了平面匹配，计算平均点面距离和面面法线夹角，并与阈值(距离为5厘米，角度为5度)进行比较，以识别匹配的平面。

B. 状态变量

我们系统中的状态变量定义如下:

       式(1)中，X为状态变量的整个集合。D、n、m、k分别为滑动窗长度、点特征个数、结构线特征个数、平面特征个数。xi是在滑动窗口中捕获第i张图像时IMU的状态。是IMU在世界坐标系中的第i个平移、速度和方向。为IMU本体框架中的加速度偏差和陀螺仪偏差。λ是点特征在其第一观测帧中的逆深度。l是世界框架中结构线特征的二自由度表示，如第IV-C节所述。π是世界坐标系中平面特征的3自由度最近点(CP)向量[11]。

C. 基于滑窗的优化

在紧耦合融合中，对上述状态变量进行了基于滑动窗口的优化估计。总体目标函数L定义如下:

       式(2)中，α、β、γ分别是点、线、面特征的指标。ρ(·)是coauthy鲁棒函数异常值测量的影响。z, P分别为测量值和协方差。rp, Hp为边缘化后的先验信息[2]。rB是IMU测量的残差。rP, rL分别为点特征和线特征在像面上的重投影误差。对于线特征，以观测线段的两个端点与投影线之间的距离作为测量值[25]。rP,D, rΠ，D分别为点、面特征深度误差。对于点特征，我们采用文献[8]中使用的基于gmm的深度误差项。对于平面特征，对[29]中提出的点到平面的代价进行加权，以纳入深度不确定性。另外，rΠ，P, rΠ，L为共面点线特征的重投影误差，公式如[15]。利用精确的平面估计有利于共面点和线特征的估计。rΠ，S为平面特征的结构误差项。它要求根据结构类型，平面的法线与某一方向平行/正交。特别是,

       其中z是单位矢量，表示世界坐标系中的重力方向，而dγ是另一个单位矢量，表示与γ-th平面正交的亚特兰大方向。

IV. 方法

A.两步亚特兰大世界推理

       在本节中，我们提出了一种两步法进行AW推理。给定一系列RGB-D图像，自动识别推理过程旨在识别场景的潜在亚特兰大结构，即主要方向。应该注意，一旦VIO初始化完成，垂直主导方向(VDD)就已经已知了。因此，问题简化为给定已知的垂直优势方向，求水平优势方向(hdd)。

       以前的工作是从线簇、平面和表面法线的消失点来检测主导方向。然而，它们在评估的质量上可能有所不同。在RGB-D相机的存在下，可以在近距离内进行高质量的深度测量，因此平面估计是准确的。另一方面，线特征在现实世界混乱的环境中显得嘈杂，并且可能在特定视点提供模糊的测量。基于这一观察结果，我们设计了一种可靠的方法，分两步推断出AW。在假设步骤中，首先根据观测到的垂直平面，根据线性极大值原理生成假设hdd。然后在验证步骤中，根据观测的持久性，使用以下平面和直线来验证新提出的假设。注意，偶尔观察到的水平方向在验证步骤中被过滤掉。因此，两个步骤的结合保证了亚特兰大主导方向的准确和有效的检测。接下来，我们将详细介绍拟议的两步方案。

1)假设步骤:

       该过程从假设步骤开始。建立一个时间窗口来存储短时间内最近观测到的平面特征，其中选择与之前任何HDD都没有关联的垂直平面进行HDD检测。具体来说，我们使用了在[5]中首次提出的用于结构线聚类的加权挖刺(MnS)算法，从一组垂直平面的法线中检测假设的hdd。硬盘检测的主要步骤如下。首先，计算平面法线与x轴之间的夹角，生成可能的HDD的一维候选间隔;

       然后，给定一组候选区间，使用MnS算法寻找与候选区间重叠最多的最优子区间。我们还为每个候选区间附加一个表示其可靠性的权重，即飞机的观测时间。选择权值之和大于阈值的子区间。最后，生成假设的hdd作为这些子区间的中心。

2)验证步骤:

       验证步骤的目的是利用平面和直线特征消除偶尔观察到的假想hdd。对于每个新检测到的假设HDD，我们根据观察时间检查其持久性。如果一个假设的HDD在积累最少N个关键帧观测之前遇到最大长度为M的关键帧观测中断，它将被丢弃(我们所有的实验中M = 150, N = 300)。对于每一个平面特征，我们首先计算法线方向与重力方向之间的夹角来确定其结构类型。如果它是一个垂直平面，那么我们遍历hdd以找到具有最小角差的匹配的hdd。对于线部分，我们使用从单帧线观测中提取的消失点与HDD进行匹配。它有三个步骤。首先，根据线的法向量与重力方向之间的夹角来识别与重力矢量平行的直线。然后，使用IV-B节中的方法将其余的线特征分类到不同的聚类中。最后，计算每个行簇的消失点来搜索匹配的HDD。值得注意的是，每条线集群的消失点，而不是每条线的法向量被用来检索匹配的硬盘，因为从单线观察中，线的方向可能是模糊的。

B.高效的结构线聚类

       特别是，我们实现了一种改进的MnS方法用于结构线聚类。基本思想与[5]中提出的算法相同(后扩展为[30]中的MW)。然而，我们证明了计算效率可以进一步提高。MnS算法的基本过程可以简单概括如下(详细描述请参考[5])。在图2中，重力方向表示为单位向量δz =(0,0,1)，位于圆心为O的高斯球上的球点S表示直线观测的单位法向量δn。如果它是一条与某个硬盘平行的无噪声线，则S应该位于与该硬盘正交的水平主导平面π上。然而，在现实中有噪声的情况下，S位于一个球形帽ω上。

       值得注意的是，水平优势面与球帽ω的几何关系决定了可能的HDD。只有正交平面与球面cap ω相交的硬盘才能在有限的噪声范围内产生线测量δ OS。如图2所示，这一观察结果得出了相关HDD的候选区间[β1， β2]。利用MnS算法，即在假设步骤中也使用了相同的方法，可以根据线性极大值原理识别具有共同水平方向的直线。

       图2：候选区间计算的两种情况。在大多数情况下(右)，与以Q为中心的圆相切的两个垂直平面的法向量(红色箭头)构成了可能的水平方向的候选间隔。在某些情况下(左)，球面点S太靠近z轴，这导致候选区间从0到2π。

C.封闭式结构线初始化

       在利用(2)所示的结构线特征进行优化之前，需要确定初始值。已经提出了几种结构线初始化的方法[25]，[31]。然而，它们既不能充分利用先前的信息来有利于初始化，也不能提供封闭形式的解决方案。在本节中，我们提出了一种利用已知结构线方向的封闭形式初始化方法。

1)结构线参数化:

       与[25]、[31]类似，结构线采用二自由度参数化。如图3所示，l为世界坐标系中的一条结构线，πl为经过原点O且与l正交的平面，一般称为l的投影平面，我们用l与πl的交点Pl表示该结构线。由于πl已知结构线的方向，因此只需要两个额外的参数来参数化Pl。图3描述了参数化的两种情况。在情形1中，l是平行于z轴，用世界坐标系中交点Pl的x和y坐标来参数化直线。在情况2中，l平行于某个亚特兰大硬盘，建立一个局部帧Fl，并使用Fl中交点Pl的y和z坐标进行线的参数化。

 图3：结构线参数化

2)结构感知的封闭形式初始化:

我们使用多视图线观测来获得结构线的初始估计:

 V. 实验

       在本节中，我们给出了建议的候选区间计算方法和建议的里程计方法的实验结果。对于前者，我们使用了[30]作者提供的开源代码来证明我们方法的计算效率。其中，本文提出的候选区间计算方法与文献[30]提出的MnS算法相结合，用于结构线聚类和摄像机旋转估计。还有，原来的候选人开源代码中包含的间隔计算方法进行了测试，提供了一个基线。这两种方法都是在MATLAB中实现的，并在具有英特尔酷睿i7 (2.8 GHz) CPU和32 GB RAM的笔记本电脑上运行。对于后者，我们在两个现实世界的RGB-D惯性数据集VCU-RVI和OpenLORIS-Scene上评估了所提出的S-VIO。由于我们的系统建立在vins - mono和DUI-VIO之上，因此选择它们作为基线。此外，我们还比较了著名的RGB-D惯性算法VINS-RGBD、利用MW假设的RGB-D惯性算法MSC-VO和利用AW假设的VIO算法StructVIO。对于所有评估的算法，环路关闭模块被禁用，仅评估里程计性能。所有实验都是在英特尔酷睿i9 CPU (3.6 GHz)和32gb RAM的台式机上使用开源代码进行的。

A.候选区间计算的评价

       表1给出了两种方法在ICL-NUIM数据集上的定量评价结果，包括旋转估计的平均误差和每行候选区间计算的平均计算时间。实验结果表明，两种方法均能获得准确的摄像机旋转结果。特别是，两种方法的旋转估计结果基本一致，证明了候选区间计算方法的正确性。在计算速度方面，本文提出的候选区间计算方法的执行时间比原方法低一个数量级。这主要是因为我们采用了不同的方法来建立方程，从而避免了原方法中繁琐的系数计算。表2显示了对方程系数进行一次计算所需的操作数。可以观察到，我们的方法比原来的方法需要更少的操作。此外，我们的方法只需要求解一个二阶方程，从而消除了求解高阶方程的需要。

B.基于VCU-RVI数据集的S-VIO评估

       表III显示了评估算法在VCU-RVI手持数据集上的精度比较，总共包括24个序列。在这六种算法中，S-VIO实现了在大多数序列中ATE最低或第二低。与没有结构约束的算法相比，在短距离序列中，S-VIO比DUI-VIO、VINS-RGBD和VINS-Mono的平均误差率(w/o失败率)提高了25%、35%和46%。这种优势在远端序列上更为明显，S-VIO比DUI-VIO、VINS-RGBD和VINS-Mono分别高出58%、75%和67%。这主要是因为利用自适应假设减少了姿态估计误差，从而有利于测程精度的提高。如图4所示，在长期运行中，S-VIO与其他算法相比具有更低的定位误差。我们还进行了消融研究，以证明获得结构约束的必要性。S-VIO中的AW检测模块关闭，导致结构线地标和平面地标上的结构约束缺失。如表III所示，与S-VIO相比，精度有所下降。

图4：廊道上评价算法的定性结果。轨迹的起点和终点应该在同一个房间。

       在结构里程计算法中，S-VIO的精度略高于StructVIO，比MSC-VO有较大的提高。具体来说，可以观察到，与StructVIO相比，利用平面特征的S-VIO在走廊4等低纹理环境中具有更低的漂移，在这些环境中，无纹理的墙壁在一段时间内主导着视野。实验结果也说明了IMU在鲁棒性和准确性方面的重要性。虽然MSC-VO也以紧耦合的方式利用了环境的结构规律性，但它仍然存在快速运动和弱纹理带来的困难。

C. S-VIO在OpenLORIS数据集上的评估

       OpenLORIS-Scene数据集[33]是一个真实世界的室内数据集，其中包含几个具有挑战性的因素，包括弱纹理、动态/可变形物体和光照差。它由两个配备了多个传感器的地面机器人收集估计。所有结果均由作者提供的OpenLORIS-SceneTools计算。

 图5：OpenLORIS-Scene数据集上的评估结果

D.运行时间分析

       表IV报告了S-VIO在VCU-RVI手持数据集上的运行时分解。特性跟踪线程和状态优化线程的平均运行时间都低于30毫秒，支持至少30 fps的实时处理图像输入。需要注意的是，点、线、面特征的检测和跟踪是在三个并行线程中进行的，这节省了大量的计算时间。

 VI. 结论

       这篇文章提出了一种紧密耦合的RGB-D惯性里程计，即用于未知室内环境的S-VIO。在优化框架中，充分利用了异构地标(点、线、面)和多模态测量(RGB、depth和IMU)，有利于里程测量。环境中可能的结构规则(即亚特兰大世界)也被用于该系统。利用VIO估计的重力方向，提出了一种两步自适应推理方法，从线和平面检测水平主导方向。此外，提出了一种包含已知方向信息的结构线初始化的封闭式方法。在两个公开的真实数据集上进行的实验表明，与现有的VIO算法和RGB-D VIO算法相比，S-VIO算法具有更好的性能。