vslam论文19：利用结构信息改进点线视觉惯性里程计（RAL 2022）

摘要

       利用直线特征来提高SLAM系统的精度已经得到了很多研究。然而，充分利用不同线特征(平行、非平行)的特点来改进SLAM系统却鲜有提及。在本文中，我们设计了一个基于点和直线的VIO系统，将直线分为结构(即相互平行的直线)和非结构。为了有效地优化直线特征，我们对结构直线和非结构直线使用了双参数表示方法。此外，我们设计了一种基于帧对帧(2D-2D)和帧对地图(2D-3D)策略的稳定线匹配方法，可以显著提高系统的轨迹精度。我们在合成数据和公共数据集上进行了消融实验，并将我们的方法与目前最先进的算法进行了比较。实验验证了不同线特征的组合可以提高VIO系统的精度，也证明了我们系统的有效性。

开源：boxuLibrary/Structural-and-Non-structural-line: comparison of plucker parameter representation and the parameter representation proposed by ours (github.com)

I. 引言 

       总的来说，非结构线和结构线有潜力以不同的优势提高VIO系统的性能，通过利用它们，估计器将在复杂环境中更加鲁棒和准确。本文提出了一种紧密耦合的单目VIO系统PLS-VIO (Point, Non-structural line, Structural line VIO)，包括视觉点特征、非结构线特征和结构线特征，实现准确的姿态估计和点线图构建，如图1所示。

图1所示。提出了一种基于线图构建的单目VIO系统。图中的红、绿、蓝线为X、Y、Z方向结构线特征的标志，紫色线为非结构线特征的标志。三个子图像显示了图像中检测到的线条。

本文的主要贡献包括:

（1）我们将VIO系统观察到的线条划分为结构线条和非结构线条，与单一类型的线条特征相比，我们的方法利用了不同的线条约束，提高了复杂环境下姿态估计和映射的准确性和鲁棒性。

（2）我们设计了2D-2D和2D-3D的线匹配算法，以减少长时间线特征的不匹配和跟踪丢失。为了有效地优化线条特征，我们引入了非结构线条的2参数表示，将线条特征有效地融合到基于优化的估计中。

II. 系统概述

       本文提出的系统基于VINSMono[2]。VINS-Mono采用优化方法将IMU观测与点特征的目视观测紧密耦合。我们的系统增加了非结构线和结构线，并构建了相应的约束条件。

       如图2所示，我们的系统包含两个模块:前端和后端。在前端，对IMU和图像的原始测量数据进行预处理，包括IMU预积分、点检测与匹配、直线检测和垂直线分类。

图2：PLS-VIO系统概述前端模块用于从原始测量中提取信息;后端采用滑动窗口优化方法对线路分类和状态变量进行估计。

       后端主要介绍了非结构线和结构线的操作。我们将与重力方向和非垂直方向对齐的垂直线传递到后端，X、Y方向和非结构线从非垂直方向的直线进一步分类，这将在III-B中描述。接下来，运行两种不同的行匹配策略，为了提高代码运行效率和简化行数据管理，我们将行匹配移到后端，这个过程将在III-C中描述。在此之后，我们初始化线条以获得3D线条地标，这将在III-D中介绍。最后，通过最小化IMU残差、先验残差、点重投影残差和线重投影残差的总和来优化地图中的IMU体状态和3D地标，所有这些残差将在IV中介绍.

III. 结构和非结构方法学

       在本节中，我们将介绍VIO系统中非结构线和结构线的实现细节。首先，描述了点地标和线地标的参数表示。其次，我们介绍了线路的分类。最后给出了不同的线条匹配方法以及非结构线条和结构线条的初始化。

A. 地标表示

1)点表示:

       我们使用逆深度λ∈R来参数化观察到的第一个关键帧中的点地标。给定归一化图像平面上的点观测值z = [u, v, 1] T，通过得到地标的三维位置。

2)非结构线表示:

       对于非结构线，如图3所示，平面π由三维线L的两个端点与相机光学中心O组成，三维线L可以表示为 

 图3

       由于锚定线所在主机帧的噪声较小，我们固定线所在平面的法向量cn，用2自由度参数表示4自由度线。π平面上的局部坐标系定义为{P}，为了简化L的表示，设{P}的原点为s`，然后设y轴方向与从O到s`的射线对齐，设z轴方向平行于平面π的法向量cn。由于坐标轴的正交性，x轴垂直于y轴和z轴。O到s的距离是d∈R。

       为了在优化过程中减少线路参数的数量，我们建议使用只有两个参数的紧凑参数化:θ和ρ = 1/d。θ为局部坐标{P}的直线方向与{P}的x轴之间的夹角。可由得到，其中RP C∈R3×3为相机坐标w.r.t局部坐标{P}的旋转矩阵。

3)结构线表示:

       我们以不同的方向表示曼哈顿多个局部世界中的结构线。具体来说，如图4所示，每条结构线都锚定在其首次被观测到的局部坐标系上，我们将这个锚定坐标系定义为起始帧{S}。起始坐标系{S}在世界坐标下的旋转和平移为。是关联的局部曼哈顿世界坐标系w.r.t世界坐标系的旋转矩阵，它从世界坐标系绕φ旋转。等于第一次观测到直线的世界帧中相机坐标的位置 . 

 图4

       为了表达一致的X, Y, Z方向线在{S}中的,我们我们再次定义一个参数空间{L}，从参数空间{L}到起始坐标系{S}的变换是一个纯旋转 .在参数空间的结构线可以表示成在XY平面上交点,我们使用逆深度代表了交点,即,我们采用这种表示是为了加快结构线的收敛速度.

 B. 结构线和非结构线分类

       我们使用LSD算法检测图像中的直线，然后将这些直线分为结构线和非结构线。利用图像中的消失点来识别结构线。我们探测到的消失点Z方向借助IMU信息，对于Z方向的结构线，我们从Z方向的消失点到线段S的中间点绘制一条射线，进一步计算射线与S之间的夹角Aerr和距离Derr。 如果Aerr小于角度阈值Ath, Derr小于距离阈值Dth，则认为S为Z方向的结构线。对于剩余的线，我们首先根据角度和距离的一致性将线附加到检测到的曼哈顿世界上，当未分类线的数量大于检测到的线总数的60%时，我们使用RANSAC方法检测X和Y方向的消失点，然后将线分类到新的曼哈顿世界中，类似于Z方向线分类的方法，最后，未分类的线被认为是非结构线。

C.  2D-2D和2D-3D的线匹配

       为了提高直线跟踪的稳定性和精度，我们结合了两种跟踪策略，即帧到帧的直线跟踪和帧到地图的直线跟踪。一般情况下，我们采用帧对帧的跟踪方法来跟踪新检测到的行，如果匹配的行数太少，我们将采用帧对地图的方法来增加匹配的行数。与文献[16]中的匹配方法相比，首先进行帧间匹配方法的主要目的是避免由于三维线地标位置估计不准确而导致的不匹配。

1) 帧到帧的线追踪方法: 

       对于帧到帧的直线跟踪方法，我们对前一帧的所有直线进行采样，得到采样点集合，然后使用极极搜索方法在当前帧中找到相应的候选匹配点。使用ZMSSD (Zero-mean Sum of Squared Differences)模板计算两点的匹配分数。我们选择匹配分数最高的候选点作为跟踪点，得到跟踪点集合。如果跟踪点到当前帧线的距离小于阈值m_th，则跟踪点有效，在我们的实现中m_th被设置为5个像素。如果有效点的数量大于前一帧线上采样点的0.8倍，我们认为这条线是最佳匹配。

2)帧到地图的直线跟踪方法:

       对于帧到地图的线跟踪，我们使用ZNCC(零归一化相互关系)匹配方法。对于一条三维线，我们在该线对应的历史帧中得到最新观测帧Fi, Fi与当前帧之间的线使用ZNCC方法进行匹配。由于摄像机的快速运动和场景中的遮挡，需要匹配的线条长度相差较大，影响了匹配的精度。我们使用对极几何约束来确定线的采样范围以辅助ZNCC匹配，即在计算Fi与当前帧之间的本质矩阵E后，根据极极几何将线在Fi中的端点投影到当前帧中，通过将当前帧中的两条极极线与匹配线相交来确定相应的采样范围，从而提高了ZNCC匹配方法的成功率。

D.初始化结构线和非结构线

       直线初始化的稳定性和精度对姿态估计有很大的影响。对于结构线和非结构线两种参数化表达式，我们采用不同的初始化方法来确定合理的初始值。

1)非结构性线的初始化:

线段在归一化图像平面上可以表示为两个端点三个非共线点，包括一个线段的两个端点和相机的光学中心O，确定一个平面π=考虑到两个平面π1和π2在摄像机坐标系c1,可以计算:

 2)结构线初始化:

       结构线的初始化也需要先计算直线的Plucker坐标，然后得到直线在世界坐标下的三维端点表达式。为了得到世界坐标系中wL平面与XY平面的交点，我们将lp转移到世界坐标系中，通过公式(2)得到wp:

 然后将wL与平面wp相交得到交点，将传递到参数空间得到点llp。

        我们使用交点

去初始化结构线，θ被初始化为逆深度也被初始化为

 IV. 线和点的VIO

       在本节中，我们将融合IMU和视觉信息与滑动窗口优化来构建VIO系统，该系统可以估计自身状态和3D地标。

A. VIO系统公式

       我们通过最小化IMU残差、视觉残差和先验残差的代价项之和来优化滑动窗口中的所有状态变量：

       其中rb为IMU测量残差，为点的重投影残差，分别为非结构线和结构线的重投影。rp和Jp分别是由边缘化算子[2]得到的先验残差和雅可比矩阵。ρ(·)是用于抑制异常值的柯西鲁棒函数。是测量值的协方差矩阵。通过对IMU测量噪声的协方差矩阵传播计算出IMU的协方差矩阵;视觉测量的协方差矩阵由先验知识确定。

B.点特征测量模型

       对于点特征，我们使用归一化图像平面中投影点与观测点之间的距离来表示残差，该距离定义为重投影误差。给定第k个点特征在帧cj处的测量, ，则重投影误差定义为:

 其中为特征第一观测帧的投影点。

C.非结构线测量模型

       对于非结构线，我们通过将在第一个关键帧中观察到的线段参数传递到观察该线的另一个关键帧来表示测量模型。直线在局部坐标系下的方向矢量可以用来计算，我们可以将dv从局部坐标系转移到摄像机坐标系，得到直线方向cv。

D.结构线测量模型

       结构线的残差形式是将参数空间{L}中的线参数转移到起始帧{S}中，再转移到观察三维线的另一个关键帧中。在目标关键帧中，构造直线重投影残差。利用式(12)，将直线的参数从第一观测关键帧的参数空间转移到目标关键帧的摄像机坐标，为了简化公式，我们吸收了从IMU到摄像机的变换。

 V. 实验

A. 综合数据

       为了验证非结构线参数表达式的有效性，我们生成了一个合成环境，其中所有3D线组成一个房间作为groundtruth。如图5所示，(a)中的绿线是模拟的80个地标，蓝色轨迹由600个VIO位姿组成，每个位姿在相机帧内形成线观测值，(b)中的红线是我们的方法重建的，线观测值没有添加噪声。在实验中，我们分别使用我们的2参数表达式、[30]中的2参数表达式和[7]中的4参数表达式对所有的3D直线地标进行了优化。通过将重建直线端点与模拟的三维直线地标进行比较，获得重建精度。如表1所示，我们的2参数表达式与4参数表达式相比精度相当，但计算时间减半，效率大大提高。

 图5：用该方法合成数据并重建三维线条。(a)真值；(b)我们的方法。

 B. EuRoc数据集

1)定位精度:

 如表2所示，PLS-VIO在几乎所有序列上的平移误差都最小，说明在VIO系统中同时使用结构线和非结构线可以提高姿态估计的精度和鲁棒性，同时也验证了我们的线匹配方法的有效性。

2) 建图质量: 

       我们将本文提出的PLS-VIO绘制的线状图与PL-VIO绘制的线状图进行比较，以评价地图质量。图6(a)显示了PLS-VIO地图的鸟瞰图，以及三个详细的子地图以及它们对应的真实场景图像。地图中的线路地标能很好地反映人工机器大厅的线路特征。为了更好地体现我们系统的地图质量，我们还运行PL-VIO来构建线图，如图6(b)所示，我们可以看到，PLS-VIO的地图具有更多的线特征，线的位置也更加准确，同时，VIO系统构建的局部地图精度的提高也会影响姿态估计，从而提高姿态估计的精度。

图6：(a) PL-VIO生成的EuRoc MH_05数据集线状图，图中的红、绿、蓝线为X、Y、Z方向结构线状特征的地标，紫色线为非结构线状特征的地标，(b) PL-VIO，图中的红线为线状特征的地标。

C. TUM VI 基准数据集

图7：(a) Corridor1数据集、(b) Corridor2数据集、(c) Corridor4数据集上的BASALT、openVINS、PL-VINS和PLS-VIO运行轨迹。十字是轨迹的起点，圆是轨迹的终点。走廊沿水平方向，初始时刻的4条轨迹与地面真值对齐，PLS-VIO的结束位置接近地面真值，方向漂移较小。

       从表III可以看出，我们的系统在几乎所有序列上都实现了最小的平移误差，同时，在PLS-VIO的地图中，X、Y、Z方向的结构线特征地标与曼哈顿世界的主方向平行。

       从表4可以看出，与LBD匹配方法相比，我们使用2D-2D和2D-3D匹配方法提高了匹配精度，但耗时与LBD基本相同。对于2参数和4参数的PL-VIO，本文的2参数线表达式在优化器中加入的状态变量较少，因此优化和边缘化所需的时间较少。

VI. 结论

       本文提出了一种充分利用点、非结构线和结构线特征的新型VIO系统，称为PLS-VIO。同时使用结构线和非结构线来提高映射和姿态估计的精度和鲁棒性，优于单一类型的线。线的2参数表示可以加快线地标的优化速度，同时，2D-2D和2D-3D的线匹配方法减少了线的不匹配。在大规模场景和走廊环境中对所提出的状态估计器进行了测试。实验表明，该方法的轨迹精度和映射质量都优于目前最先进的视觉惯性里程计。

       在未来，我们将通过深度学习获取系统中的结构信息，提高结构线检测的稳定性。此外，还将引入平面约束进行映射。