PL-VIO（点线slam）学习笔记（持续更新）

论文翻译

PL-VIO:使用点和线特征的紧耦合单目视觉惯性里程计

收稿日期:2018年3月23日;录用日期:2018年4月9日;发布日期:2018年4月10日

摘要

摘要: 为解决这一问题的估计相机轨迹和建立一个结构三维(3 d)地图根据惯性测量和视觉的观察,本文提出点线路visual-inertial测程法(PL-VIO),紧耦合的单眼visual-inertial测距系统利用点和线的特性。与点特征相比，线提供了更多的环境几何结构信息。为了获得三维空间线的计算简洁性和表示的紧致性，使用Plücker坐标和直线的标准正交表示。为了紧密有效地融合来自惯性测量单元(IMU)和视觉传感器的信息，我们通过最小化成本函数来优化状态，该函数将预先集成的IMU误差项与点和线重投影误差项结合在一个滑动窗口优化框架中。在公共数据集上进行的实验表明，结合点和线特征的PL-VIO方法优于仅使用点特征的几种最先进的VIO系统
关键词: 传感器融合;visual-inertial测程法;紧耦合;点和线特征

1引言

近年来，定位与导航技术因其广泛的应用而备受关注，尤其是在无人驾驶汽车、服务机器人、无人机等领域。定位和导航使用了几种类型的传感器，如全球导航卫星系统(GNSSs)[1]、激光激光雷达[2,3]、惯性测量单元(imu)和相机[4,5]。然而，它们各自都有明显的缺点:gnss只有在天空晴朗[6]时才能提供可靠的定位信息;激光激光雷达对玻璃表面[7]的物体有反射问题;民用imu的测量值有噪声，惯性导航系统可能由于误差累积[8]而快速漂移;而单目同步定位与测绘(SLAM)只能恢复一定规模的运动轨迹，当相机移动快或光照变化剧烈时，容易丢失[9-11]。因此，传感器融合方法，尤其是视觉惯性导航系统的传感器融合方法，受到了广泛的关注。来自IMU的加速度和角速度信息可以显著改善单目SLAM系统[13,14]。此外，imu和相机重量轻、成本低，因此在民用应用中得到广泛应用。

根据传感器直接或间接的融合测量结果，视觉惯性里程计(VIO)系统可以分为两大主流:松耦合和紧耦合方法。松散耦合方法[15,16]通过两个估计器分别估计相对运动，并融合两个估计器的估计来处理图像和IMU测量结果.

紧耦合方法[17,18]使用一个估计器，通过直接融合来自相机和IMU的原始测量数据来找到最优估计。与松散耦合方法相比，紧密耦合方法通常更准确、更健壮。本文提出的PL-VIO方法是一个紧耦合的VIO系统。关于紧密耦合VIO方法的相关工作可以根据测量模型[14]中线性化的数量进行分类。基于扩展卡尔曼滤波(EKF)的方法在更新步骤中只处理一次测量，而批量非线性优化在优化步骤中多次线性化。基于滤波的方法[19,20]集成IMU测量来传播/预测状态，然后用视觉测量更新/修正最新的状态。由于状态向量中包含了三维地标的坐标，因此EKF的计算复杂度随地标数量的增加而二次增加。为了解决这个问题，Mourikis和Roumeliotis[21]提出了多状态约束卡尔曼滤波器(multi-state constraint Kalman filter, MSCKF)，它将状态向量中的地标坐标边缘化。这种方法的一个缺点是，用于更新状态的地标测量需要移出相机的视图，这意味着不是所有当前的视觉测量都在过滤器中使用。此外，线性化误差使滤波器不一致[14]。

基于优化的方法通过最小化带有IMU测量残差和视觉重投影残差的联合非线性代价函数来获得最优估计。因此，基于优化的方法可以在不同的点上重复线性化状态向量，从而达到比基于滤波的方法[14]更高的精度。IMU测量约束是通过整合帧间IMU测量值来计算的。然而，标准的IMU集成方法与第一帧IMU体的初始状态密切相关。当估计的状态发生变化时，需要重新计算所有集成的IMU测量值。Lupton和Sukkarieh[22]提出了IMU预集成技术，避免了这种重复集成。IMU预集成在基于优化的VIO中得到了广泛的应用[18,23,24]。Forster等人[14]通过在流形上处理旋转组而不是使用欧拉角重新制定了IMU预积分。Liu等人[13]提出了连续预积分法。虽然基于优化的方法已经达到了很高的精度，但随着越来越多的里程碑被添加到优化中，计算变得昂贵。OKVIS[18]采用先入后出滑动窗口法进行边界计算，边沿最老状态的测量值。Shen等人[23]提出了一种双向边际化，选择性地边际化身体状态和标志物。

尽管在VIO领域已经取得了重大成就，但大多数VIO系统只使用点特性作为可视化信息。然而，在无纹理环境下的点检测和在光照变化场景下的点跟踪具有挑战性[25,26]。相反，线段在这些场景中是一个合适的替代方案。此外，线段比[27]点提供更多关于环境的结构信息。对于纯视觉SLAM，有一些结合点和线特征来估计相机运动的工作[28,29]。在SLAM系统中集成直线特征的最简单方法是使用两个端点来表示直线。从不同的视图匹配一条线的端点是很困难的。此外，三维空间线只有4个自由度，而两个三维端点引入了6个参数，导致过度参数化。Bartoli和Sturm[30]提出了标准正交表示，在优化过程中使用一个三自由度旋转矩阵和一个单自由度旋转矩阵来更新线参数。一些立体视觉SLAM系统已经使用了标准正交表示[27,31]。对于VIO方法，Kottas和Roumeliotis[26]只使用线特征来研究VIO的可观测性。Kong等[25]利用三焦点几何构造了一个结合点和线特征的立体VIO系统。然而，这些工作涉及到基于过滤的VIO。在我们提出的PL-VIO方法中，我们将线特征集成到优化框架中，以获得比基于滤波的方法更高的精度。

自己的补充: 直线的自由度：4，一个法向量可以确定一簇平面（2自由度），平面上的任意一条直线有2自由度，因此确定了一个法向量，过其与平面垂足做一条直线，所得到的结果是唯一确定的。另一方面：直线的参数方程为ax+by+cz+d=0,显然四个参数可以描述一条直线。

为了构建结构三维地图并获取摄像机的运动，我们提出了PL-VIO系统，该系统通过联合最小化IMU预积分约束以及滑动窗口的点和线重投影误差来优化系统状态。与传统的仅利用点特征的方法相比，该方法利用了附加的线特征，旨在提高光照变化环境下的鲁棒性和准确性。我们的主要贡献如下:

• 据我们所知，拟议的PL-VIO是第一个使用点和线作为地标的基于优化的单目VIO系统。
•为了紧密有效地融合来自视觉和惯性传感器的信息，我们引入了带有IMU预集成约束和点/线特征的滑动窗口模型。为了在优化中紧凑地表示三维空间直线，采用了直线的标准正交表示。为了有效地求解滑动窗口优化问题，导出了与IMU体状态相关的所有误差项的雅可比矩阵。
•在EuRoc数据集和PennCOSYVIO数据集上，我们比较了提出的PL-VIO与三种最先进的单目VIO方法(ROVIO[17]、OKVIS[18]和VINS-Mono[32])的性能，并给出了详细的评价结果。

本文的其余部分组织如下。首先，我们在第2节中描述了数学基础，然后在第3节中给出了基于滑动窗的视觉惯性融合方法。接下来，我们将在第4节中描述PL-VIO系统和实现细节。第5节给出了实验结果。最后，第六部分给出了结论和未来可能的工作。

2 数学公式

2.1 符号

图1展示了视觉惯性传感器，以及点和线特征的视觉观测结果。我们将ci作为表示为t = i时刻的相机坐标系，同时bi作为IMU坐标系。w是地球的惯性系。(·)c表示向量(·)在坐标系c中表示。四元数qxy用来将一个向量从坐标系y旋转到坐标系x，对应的矩阵形式为Rxy。我们使用pxy将一个向量从帧y平移到帧x。四元数qbc和向量pbc表示 相机帧和机身帧之间的外部参数，这些外部参数在提供的数据集中是已知的，或者用Kalibr校准工具箱[33]校准。fj 和 Lj分别是地图中的第j个点地标和线地标。Z表示测量值;具体来说，
是第i个相机帧观测到的第j个点特征，zbibj表示两个关键帧之间的预积分IMU测量。

图1所示。视觉惯性传感器、点观测和线观测的说明。IMU:惯性测量单元。

2.2 IMU Pre-Integration

一个六轴IMU，包括一个三轴加速度计和一个三轴陀螺仪，可以测量身体框架相对于惯性框架[14]的加速度A和角速度ω。原始测量ˆω和ˆa受到偏置和白噪声的影响:

式中

分别为陀螺仪的偏差和加速度计的白噪声。

为坐标系w中的重力矢量。对于imu驱动系统[34]，我们使用如下运动学:

⊗表示四元数乘法运算。
已知t = i时刻的IMU体态，即

，以及t∈[i, j]期间ω和a的值，对式(3)积分可得t = j时刻的体态:

∆t 为i与j的时间差，式(4)中，IMU状态传播从第i帧bi开始。当bi状态改变时，我们需要重新传播所有的测量值。由于优化过程中每次迭代都要调整身体状态，因此式(4)比较耗时。将qwbj分解为qwbi⊗qbibt，式(4)可表示为:



称为预积分测量[22]，可以直接计算，不需要知道bi的体态，这意味着当体态发生改变时，不需要再复制。我们将预集成的测量值作为连续关键帧之间的约束因素。

自己的补充:说人话就是,imu预积分量是两帧之间的相对变化量,用这个量作为约束,就不存在重新计算的问题了.

由连续时间推导出的预积分模型(式(6))忽略了偏差和噪声。在实践中，IMU测量是离散时间采集的，应该考虑噪声。在这项工作中，我们使用中点积分集成IMU测量。IMU体传播使用离散矩k和k + 1的测量值计算如下:

一开始，k = i，我们有qbibi= [0,0,0,1]，

是零向量。
在式(7)中，为了有效计算预积分测量值，我们假设两个关键帧之间的偏差是恒定的:

在实践中，偏差变化缓慢。我们用随机行走噪声建模偏差:

其中高斯白噪声定义为
当偏差以很小的增量变化时，我们使用一阶近似更新
[14]，而不是迭代计算预积分测量值:


是预先集成的测量bias的雅可比矩阵。他们可以派生误差状态转换矩阵,如附录A所示的协方差矩阵预先集成测量
能够迭代计算IMU传播, 附录A提供了更多的细节。

自己的补充: 可以这么理解,就是在一开始计算预积分的时候,也就是公式7,我们认为bias是不变的.
但是随着时间的推移,bias肯定有变化,我们不用公式7重新计算预积分,而是用一阶线性近似更新,
也就是用bias的jacobin来更新预积分量.

2.3 直线的几何表示
一条直线只有四个自由度。因此，直线的紧凑参数化是有四个参数的。在我们的系统中，我们将三维空间中的直线视为无限的直线，并对三维直线采用两种参数化方法，如[27]。Plücker线坐标由6个参数组成，便于变换和投影。由四个参数组成的标准正交表示法由于其紧致性而被用于优化。

2.3.1 Plücker 线坐标
在图2a中，Plücker坐标下的三维空间直线L表示为

其中

为直线方向向量，

为直线与坐标原点所决定平面的法向量。
Plücker坐标是过度参数化的，因为在向量n和d之间存在一个隐含的约束，
即

因此，Plücker坐标不能直接用于无约束优化。然而，用法向量和方向向量表示的三维直线，可以简单地从两个视图进行几何三角剖分，也可以方便地建模直线几何变换。

图2。Plücker直线特征的坐标。(a) Plücker线坐标;(b)新观测线的初始化

对于直线几何变换，给出变换矩阵

从世界坐标系w到相机坐标系c，我们可以通过[30]变换一条直线的Plücker坐标

其中 [·]× 为三维向量的斜对称矩阵，Tcw为将直线从坐标系w变换到坐标系c的变换矩阵。

当在两个不同的摄像机视图中观察到一个新的线地标时，Plücker坐标很容易计算出来。如图2b所示，摄像机c1和c2分别为


捕捉3D线条L。
归一化图像平面上的线段

可以用两个端点来表示，sc1= [us,vs, 1]T和ec1= [ue,ve, 1]T。三个非共线点，包括线段的两个端点和坐标原点C = [x0,y0,z0]T，确定了三维空间中的平面π = [πx，πy，πz，πw]T

自己的补充：很简单，其实就是下面的蓝色这个面（或者红色面）的构建方法


给定两个平面π1和π2和相机帧c1，可以计算出对偶Plücker矩阵L∗

相机框架c1中的Plücker坐标L很容易从对偶Plücker矩阵L∗中提取出来。可以看出，n和d不需要是单位向量

自己的补充：这里其实是在讲怎么计算一个相机坐标下的直线Plücker矩阵L。
可以这么想，我们首先拿到两个相机C1 、C2下的匹配的两队点，在各自相机坐标系下构建平面π1和π2
（利用公式12，13），然后用公式14计算对偶L*，之后从L*矩阵提取出L坐标。
有了各个坐标系下的L，后面就是坐标变换了，对吧，很简单，道理和点之间的坐标变换类似（参考公式11）。

2.3.2 正交表示

由于三维空间线只有4个dof，所以在优化时，标准正交表示 (U,W)∈SO(3) × SO(2) 比Plücker坐标更合适。此外，标准正交表示和Plücker坐标可以相互转换，这意味着我们可以在SLAM系统中采用它们来实现不同的目的。在这一节中，我们将介绍标准正交表示的细节。如图2a所示，在三维线上定义了一个坐标系。标准化的法向量和标准化的方向向量是坐标系的两个轴。第三个轴是通过与另外两个轴向量相交而确定的。我们可以将线坐标与相机帧之间的旋转矩阵定义为U:

ψ = [ψ1，ψ2，ψ3]T 是相机坐标系x轴、y轴和z轴的旋转角。Plücker坐标与U的关系为:

由于式(16)中的

组合只有一个DoF，可以用三角函数表示:

其中

为旋转角度（旋转向量/角轴）。回想一下，从坐标原点到3D直线的距离是

因此W包含距离d的信息。根据U和W的定义, 这四个自由度包括三个来自旋转矩阵的自由度(它将线坐标转换为相机框架)和一个来自距离d的自由度。,在优化过程中，我们使用O = [ψ，φ]T作为三维空间线的最小表示。
一旦用标准正交表示对一条3D直线L进行优化，则可通过以下方式计算直线对应的Plücker坐标:

其中ui是矩阵U的第i列，w1= cos(φ)， w2= sin(φ)。L‘ 和L之间有一个比例因子，但它们代表相同的3D空间线。

自己的补充：首先是引出直线正交表示方法 （公式15 17）
然后引出正交法和Plücker坐标转换（公式18， L是真正想要的Plücker坐标）
同时给出优化的最小表达（ O = [ψ,φ]T，ψ代表旋转 ，φ指的是角轴的角度（旋转向量） 这里有点不太懂）

3.紧耦合Visual-Inertial融合

在视觉SLAM中，通过最小化图像平面上的重投影误差来优化相机的姿态和三维地图点。通过非线性优化进行的束调整可以视为一个因子图[35]，如图3a所示:圆形节点是需要优化的摄像机姿态或3D地标;带有方框的边表示作为节点之间约束的视觉测量值。对于视觉-惯性融合，我们也可以使用紧密耦合的基于图的框架来优化视觉-惯性系统[23]的所有状态变量。如图3b所示，因子图中不仅包含视觉测量值，还将预先集成的IMU测量值作为边来约束连续的IMU体状态。

图3。(a)视觉同步定位和测绘(SLAM)问题与(b)视觉惯性SLAM问题的因子图。

3.1 滑动窗口方程
在提高计算效率的同时提高计算精度，我们采用滑动窗口公式进行因子图优化。滑动窗口在i时刻的完整状态变量定义为:

其中xi是第i个IMU体在世界框架中的位置、速度和方向描述，以及IMU体框架中的偏差。下标n、m和o分别是body状态、点标志和线标志的开始索引。N是滑动窗口中的关键帧数。
M和O分别为滑动窗口中所有关键帧观察到的点标志点和线标志点的个数。我们只使用一个变量，即逆深度λk，来参数化第k个从其第一个观察关键帧开始的点地标。Ol是世界坐标系w中第 l 条线特征的标准正交（U，W）表示。
我们通过最小化所有测量残差的成本项之和来优化滑动窗口中的所有状态变量:

式中

为body state xi和xi+1之间的IMU测量残差。B是滑动窗口中所有预先集成的IMU测量值的集合。

分别为点特征重投影残差和线特征重投影残差。
F和L为相机帧观测到的点特征和线特征集。
{rp,Jp}是在滑动窗口[23]中剔除一帧后可以计算出的先验信息，
Jp是之前优化后得到的Hessian矩阵的先验雅可比矩阵。
ρ是用来抑制异常值的柯西鲁棒函数。
我们使用Levenberg-Marquard算法来解决非线性优化问题(20)。通过迭代更新初始X0，可以得到最优状态估计X:

其中⊕是用增量δX来更新参数的操作符。对于位置、速度、偏差和逆深度，更新操作符和增量δ很容易定义:

然而，状态姿态q的更新算子和增量δ更为复杂。四元数中使用了四个参数来表示三自由度旋转，因此过于参数化。旋转的增量只能是三维的。与[18]类似，我们在切空间中使用 δθ∈R3 扰动作为旋转增量。因此，旋转q可以通过四元数乘法来更新:

我们也可以把它写成旋转矩阵形式:

I是一个3× 3的单位矩阵。同样，我们可以将标准正交表示更新为:

标准正交表示的增量为δO = [[δψ]×，δφ]T。最后，优化过程中的增量δX可定义为:

每一次迭代，增量δX可由式(20)求解:

其中Hp、Hb、Hf和Hl 分别为先前残差的Hessian矩阵、IMU测量残差以及点和线重投影残差。对于残差r(·)，有

其中J(·)是残差向量r(·)相对δX的雅可比矩阵，Σ(·)是观测的协方差矩阵。残差和雅可比矩阵的公式将在下面的小节中定义。

３.2 IMU测量模型
由于由式(10)计算的预积分IMU测量是两个连续关键帧bi 和 bj之间的约束因子，因此IMU测量残差可定义为:

其中[·]xyz提取四元数的实部，用于近似三维旋转误差[18]。

在非线性优化过程中，IMU测量残差相对于body state xi 和 xj 的雅可比矩阵计算为:

其中

为左四元数积矩阵和右四元数积矩阵，分别为[36]。
算子 [.]3x3 用于从(·)的右下块中提取一个3×3矩阵。
雅可比矩阵由

计算得到

3.3 点特征测量模型
对于点特征，三维点的重投影误差是投影点与观测点之间的图像距离。在这项工作中，我们处理点和线的特征测量归一化图像平面。给定帧 cj 的第k个点特征测量，

则重投影误差定义为:

其中

为该特征在相机帧ci中的首次观测，λkis的反深度也在相机帧ci中定义。为了最小化点的再投影误差(30)，我们需要优化帧bi,bj的旋转和位置和特征逆深度λ。通过链式法则可得到相应的雅可比矩阵:


其中

是第i个IMU体帧中的三维点向量。我们假设被测点特征在图像平面的垂直和水平方向上都有像素噪声，定义点测量协方差矩阵

为2× 2对角矩阵。

3.4 线特征测量模型

直线测量的重投影误差定义为从端点到投影线的距离。对于针孔模型相机，通过[27]可以将三维空间线L = [n,d]T投影到相机图像平面:

其中K是直线特征的投影矩阵。当将直线投影到归一化像平面时，K为单位矩阵。由投影式(33)可知，一条3D直线所投影的线段坐标仅与法向量n有关。

给定一个三维线

和在世界帧上的标准正交表示Ol，我们首先将其转换为相机帧ci(公式11)。然后将其投影到图像平面上，得到投影线

摄像机帧i中的重投影误差定义为：

用d(s,l)表示端点s到投影线l的距离函数:

自己的补充：这里我们把直线投影到相机平面上，然后算出平面上匹配线段的两个端点到投影线的距离
这个距离理想情况是0

通过最小化线重投影误差

优化第i个body state和第 l 个线参数。通过链式法则可得到相应的雅可比矩阵:

与点测量协方差矩阵相似，直线测量协方差矩阵

是通过假设线段的端点存在像素噪声来定义的。
其实这么来看，这个内部的重点其实是构建线匹配的误差，并加入到整体的Hessian矩阵里面。

后面就是介绍框架和实验啦

4. 具有点和线特征的单目视觉惯性里程计

如图4所示，拟议的PL-VIO系统有两个主要模块:前端和后端。前端模块用于IMU和摄像头测量数据的预处理。后端模块用于估计和优化body状态。我们将在以下小节中介绍详细内容。

图4。我们的点线视觉-惯性里程计(PL-VIO)系统概述。A是前端模块，用于从原始测量数据中提取信息;B是用来估计和优化身体状态的后端模

4.1 前端

前端从相机和IMU的原始测量数据中提取信息。随着每次新的IMU测量值的增加，body state通过传播更新，并以最新的body state作为滑动窗口优化的初始值。此外，新的IMU测量值是预先集成的，以约束IMU body state在优化期间的连续状态。

在图像处理方面，在两个独立的线程中检测点和线的特征。当新帧出现时，利用KLT光流算法[37]将点特征从前一帧跟踪到新帧。然后，我们使用RANSAC框架和必要的矩阵测试来去除异常值。如果在剔除离群点后跟踪的点特征个数小于阈值，则会添加FAST检测器[38]检测到的新的角点特征。对于线段特征，由LSD检测器[39]检测新帧中的线段，并由基于外观的描述符LBD[40]与前一帧中的线段进行匹配。我们使用几何约束来去除线匹配的离群值。例如，两条匹配线的中点之间的距离应该不超过δth distpixels，角度差应该不超过δth角度。特征检测和匹配后，将点特征和线段端点投影到归一化后的图像平面上。此外，如果跟踪点特征的平均视差大于阈值，则一帧被选为新的关键帧。

4.2 后端

在后端线程中，首先对点和线进行三角剖分，建立重投影残差。为了得到较好的地标估计，利用所有的观测数据估计一个点特征的逆深度。对于线三角剖分，我们只选择滑动窗口中空间距离最远的两帧来初始化Plücker坐标。

在我们得到地图点/线的初始估计和从IMU测量预测的IMU体状态后，使用第3节中描述的滑动窗口优化来寻找最优状态。为了限制状态向量X的大小，采用了双向边缘化策略

从滑动窗口[23]中删除状态。当第二个最新帧xn+N−1是关键帧时，我们忽略了最早的帧xn及其所有度量值。否则，如果最新的第二帧不是关键帧，我们将丢弃该帧的视觉测量值，并在预积分测量中保留其IMU测量值。在边缘测度的基础上获得新的先验信息，保留了被移除状态的约束信息。

最后，剔除无效的地图点和线。如果一个点的反深度是负的，我们将从地图中删除这个点。如果一条线的重投影残差超过一个阈值，它将从地图中删除。

4.3 实现细节

为了引导VIO系统，我们采用视觉-惯性对准方法[41]来恢复IMU初始体态的尺度、重力矢量、初始偏差和速度。滑动窗口有N = 10个关键帧。每帧最多有150个点特征和150个线段。在线匹配中使用的阈值设置δth dist= 60像素和δth角度= 30◦。由于视觉惯性系统只有4个不可观测的自由度(偏航方向和绝对位置)，6个自由度的优化方法可能会在滚转和俯仰方向上引入虚假信息，自动沿着这些方向采取步骤以最小化成本函数。在滑动窗口优化之后，我们通过沿着滚动和俯仰方向的增量旋转它来重置身体状态。所有的数值优化都使用Ceres求解程序库[42]中的Levenberg-Marquardt方法进行求解。线路检测和匹配代码由opencv3[43]提供。

5. 实验结果

我们使用两个公共基准数据集评估PL-VIO系统:EuRoc MA V数据集[44]和PennCOSYVIO数据集[45]。将PL-VIO方法与3种最先进的单目VIO方法进行了比较，验证了PL-VIO方法的优势:ROVIO[17]和OKVIS[18]单目模式下，以及VINS-Mono[32]无闭环模式下的优势。ROVIO是基于扩展卡尔曼滤波(EKF)的VIO的紧耦合形式。它直接利用图像的强度误差在更新步骤中找到最优状态。OKVIS是一种具有点特征的滑动窗口优化算法，可以在单目或立体模式下工作。VINS-Mono是一个完整的VIO-SLAM系统，采用点特性在滑动窗口中优化IMU身体状态，并执行循环关闭。所有的实验都是在Intel Core i7-6700HQ CPU (2.60 GHz, 16 GB RAM)和ROS Kinetic[46]上进行的。

5.1 EuRoc飞行器数据集
EuRoc微型飞行器(MA V)数据集由MA V在两个室内场景中收集，其中包含来自全球快门相机在20FPS下的立体图像和同步IMU在200hz[44]下的测量数据。每个数据集提供由VICON运动捕捉系统给出的地面真实轨迹。数据集中还提供了所有的外部和内部参数。在我们的实验中，我们只使用左边相机的图像。

线条特征的主要优点是它们提供了与环境相关的重要的几何结构信息。作为一个例子，我们展示了由PL-VIO根据图5中的MH_05_difficult序列构建的重建映射。图5a-d中的四幅图像是由一架MA V在一个机器大厅中飞行拍摄的，它显示了房间的结构。如图5d所示，在弱光照场景下，线段检测比点特征检测更具有鲁棒性。从重建的三维地图中可以看出，环境的几何形状是由线段描述的，从而可以从地图中提取语义信息。这对机器人导航很有用。

图5。映射结果为MH_05_difficult序列。(a-d)检测不同帧的点和线特征。(e)重建线图(红色)和轨迹(绿色)。从地图上清晰地重建了线条特征丰富的场景(如窗户、栏杆、橱柜等)。(f)重建的点图(蓝色)。与线状图相比，点状图的结构难以识别。

为了进行定量评估，我们将PL-VIO与三种最先进的单目视觉惯性方法进行了比较:ROVIO [17]， OKVIS[18]单目模式下，以及VINS-Mono[32]无环封闭。为了进行公平比较，使用了这些比较方法的作者提供的默认参数。我们选择绝对位姿误差(APE)作为评价指标，直接比较了估计值与地面真实值[47]之间的轨迹误差。开源包evo (https://michaelgrupp.github.io/evo/)提供了一个易于使用的接口来评估轨迹测量和SLAM算法。在本节中，我们使用了这个工具来评估这些方法。表1给出了所有轨迹平移和旋转的均方根误差(RMSE)，其直方图如图6所示。

表1。对多个EuRoc MA V数据集进行了均方根误差(RMSE)分析。下面列出了平移(m)和旋转(rad)误差。加粗的数字表示估计的轨迹更接近基准轨迹


图6。针对ROVIO、OKVIS-Mono和Vins-Mono的rmse，不带循环关闭，以及针对使用EuRoc数据集的拟议PL-VIO。(a)翻译中的rmse。(b)轮调rmse。ROVIO的旋转误差比其他三种方法高一个数量级，所以在(b)中没有报告结果

表1显示，除了V1_01_easy之外，联合优化点和线的PL-VIO在8个旋转序列上提供了最佳性能。除了V1_01_easy、V1_02_medium和V1_03_difficult，我们的方法对6个序列的翻译效果也最好。然而，与最佳结果的差异仅在亚毫米水平。表1的结果表明，将线特征集成到VIO中可以提高运动估计的精度。为了直观地展示结果，图7显示了PL-VIO和VINS-Mono估计的几个轨迹热图。颜色越红，transaltion误差越大。通过对三种轨迹的比较，我们得出结论，当相机快速旋转时，具有线特征的PL-VIO比VINS-Mono误差更小。此外，我们发现这些快速旋转的序列会导致观察方向发生很大的变化，而且光照条件对跟踪点特征尤其具有挑战性[25,26,28]。如图8所示，27个点对(包括10个离群点)匹配成功，33条线匹配成功，匹配全部正确。

图7。将该方法与VINS-Mono进行比较。在(a) MH_05_difficult上用VINS-Mono运行左列的三个彩色轨迹;© MH_04_difficult;(e) V1_03_difficult序列。右边三个轨迹是拟议的PL-VIO在(b) MH_05_difficult上的结果;(d) MH_04_difficult;和(f) V1_03_difficult序列。颜色编码相应的绝对位姿错误。

图8。跟踪点/线特征与快速旋转在V1_03_difficult。光照条件和视图方向在连续的帧中改变。在帧(a,b)中用相同颜色标记匹配点特征。在帧(c,d)中用相同颜色标记匹配的线段特征。

不同方法的计算次数如表2所示。基于滤波器的ROVIO计算效率最高，准确率最低。提出的PL-VIO系统是最耗时的方法，但其计算时间主要受线路检测和匹配步骤的限制。在5.3节中，我们在V1_03_difficult序列中独立估计了PL-VIO中不同模块的计算次数，发现PL-VIO系统的计算效率直接取决于线路检测和匹配。

表2。不同方法在EuRoc数据集上的平均运行时间(单位:毫秒)。

5.2 PennCOSYVIO Dataset

PennCOSYVIO数据集是最近的一个VIO基准，它从室外到室内收集带有手持设备的大型玻璃建筑的同步数据(参见图9)[45]。具有挑战性的因素包括光照变化、快速旋转和重复结构。给出了所有的内、外参数以及地面真轨迹。我们使用的数据是由vi传感器收集的，该传感器也用于EuRoc MA V数据集。

9。来自PennCOSYVIO数据集的图像。红线是检测到的线。(a)玻璃大厦外。(b)玻璃建筑内部。

我们比较了提出的PL-VIO和没有环路闭合的VINS-Mono。为了公平地评估，PL-VIO和VINS-Mono使用了相同的参数。这里使用5.1节中使用的相同的度量和评估工具来评估轨迹。表3列出了结果。

表3。给出了不同算法的绝对位姿误差的均方根误差。加粗的数字表示估计的轨迹更接近基准轨迹

此外，在PennCOSYVIO数据集中还提供了评估工具https://daniilidis-group.github.io/penncosyvio/，并采用了APE和相对位形误差(RPE)两个指标。对于RPE，它通过将该值除以路径长度[45]来表示误差的百分比。PennCOSYVIO的创建者谨慎地选择了评估参数，因此他们的工具适合在这个数据集中评估VIO方法。因此，我们在实验中采用了该评价工具，结果如表4所示。

表4。采用PennCOSYVIO评估工具对不同算法的结果进行了评估。旋转误差和相对位姿误差(RPE)用度表示。平移误差表示为x轴、y轴和z轴。APE用米表示，RPE用百分比(%)表示。加粗的数字表示估计的轨迹更接近基准轨迹。

从表3和表4可以看出，PL-VIO在旋转部分获得了最好的性能。PennCOSYVIO工具评估的APE的平移提供了更多细节。与VINS-Mono相比，PL-VIO在y轴和z轴上的误差更小，三轴的误差之和也更小。VINS-Mono仅在x轴上获得了更好的性能。

5.3 计算时间

最后，我们评估PL-VIO在V1_02_medium序列上运行的平均执行时间，因为这个图像序列是从一个典型的室内场景中收集的。表5显示了每个块的执行时间。我们可以看到，线路检测和匹配，运行在11hz的前端，是效率的瓶颈。最先进的线路检测和匹配方法，如LSD和LBD的组合，对于VIO/SLAM系统并不令人满意。需要注意的是，我们的方法独立于直线特征检测和匹配，因此提高它们的效率超出了本文的范围。后端边缘化是另一个耗时的部分。我们观察到边缘化的低效是由于边缘化特征时的填充造成的，这使得Hessian矩阵变得更稀疏。这个问题可以通过在执行边际化以保持稀疏Hessian矩阵[18]时丢弃一些特征来潜在地解决。

表5 PL-VIO按V1_02_medium顺序运行的平均执行时间。

6. 结论

本文提出了一种新的单目视觉-惯性里程计紧密耦合算法PL-VIO，该算法在具有点和线特征的滑动窗口中优化系统状态。拟议的PL-VIO系统有两个主要模块:前端和后端。前端模块用于传播IMU体状态，检测和匹配点/线特征。后端模块用于估计和优化主体状态。在后端模块中，将线标视为无限的三维空间线，并在优化过程中采用其标准正交表示对其进行紧凑参数化。详细给出了所有误差项的雅可比矩阵，从而有效地求解滑动窗口优化问题。我们还提供了拟议的PL-VIO与三种最先进的单目VIO方法(包括ROVIO[17]、OKVIS[18]和VINS-Mono[32])在EuRoc数据集和PennCOSYVIO数据集上的比较结果。根据分析和结果，进一步得出以下两个结论:

1. 重建的具有线特征的三维地图可以提供与环境相关的几何信息，从而从地图中提取语义信息。这对机器人导航很有用。
2. 线特征可以提高系统的平移和旋转精度，特别是在光照变化的场景中。但是线路的检测与匹配非常耗时，成为系统效率的瓶颈。
   在未来，我们计划通过引入三维空间线之间的结构约束来改进我们的系统，例如在曼哈顿世界场景[48]中引入平行线或共面线。这些线之间的几何约束有进一步提高定位精度和减少旋转累积误差的潜力。

致谢

本研究受到国家自然科学基金(批准号:61421004)的资助。感谢杨丁测试线路匹配方法。最后，何宜佳特别感谢唐强、秦方波的支持。

作者贡献

何宜嘉、赵冀构思并设计了算法;Yijia He进行实验，分析数据，起草论文;Guo Yue和Wenhao He贡献了分析工具;季昭和奎元对手稿进行了修改。

附录

参考文献

Groves, P .D. Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems; Artech House:
Norwood, MA, USA, 2013.
2.
Martínez, J.L.; Morán, M.; Morales, J.; Reina, A.J.; Zafra, M. Field Navigation Using Fuzzy Elevation Maps
Built with Local 3D Laser Scans. Appl. Sci. 2018, 8, 397.
3. Hess, W.; Kohler, D.; Rapp, H.; Andor, D. Real-time loop closure in 2D LIDAR SLAM. In Proceedings
of the 2016 IEEE International Conference on Robotics and Automation (ICRA), Stockholm, Sweden,
16–21 May 2016; pp. 1271–1278.
4. Liu, L.; Mei, T.; Niu, R.; Wang, J.; Liu, Y .; Chu, S. RBF-Based Monocular Vision Navigation for Small V ehicles
in Narrow Space below Maize Canopy . Appl. Sci. 2016, 6, 182.
5.
Valiente, D.; Gil, A.; Payá, L.; Sebastián, J.M.; Reinoso, Ó. Robust Visual Localization with Dynamic
Uncertainty Management in Omnidirectional SLAM. Appl. Sci. 2017, 7, 1294.
6. Borraz, R.; Navarro, P .J.; Fernández, C.; Alcover, P .M. Cloud Incubator Car: A Reliable Platform for
Autonomous Driving. Appl. Sci. 2018, 8, 303.
7. W ang, X.; W ang, J. Detecting glass in simultaneous localisation and mapping. Robot. Auton. Syst. 2017, 88, 97–103.
8. Titterton, D.; Weston, J.L. Strapdown Inertial Navigation T echnology; The Institution of Engineering and
Technology: Stevenage, UK, 2004; V olume 17.
9. Cadena, C.; Carlone, L.; Carrillo, H.; Latif, Y .; Scaramuzza, D.; Neira, J.; Reid, I.; Leonard, J.J. Past, present,
and future of simultaneous localization and mapping: Toward the robust-perception age. IEEE T rans. Robot.
2016, 32, 1309–1332.
10. Mur-Artal, R.; Tardós, J.D. ORB-SLAM2: An open-source SLAM system for monocular, stereo, and RGB-D
cameras. IEEE T rans. Robot. 2017, 33, 1255–1262.
11.
Engel, J.; Koltun, V .; Cremers, D. Direct sparse odometry . IEEE T rans. Pattern Anal. Mach. Intell. 2018, 40,
611–625.
12. Gui, J.; Gu, D.; Wang, S.; Hu, H. A review of visual inertial odometry from filtering and optimisation
perspectives. Adv. Robot. 2015, 29, 1289–1301.
13.
Liu, Y .; Chen, Z.; Zheng, W.; Wang, H.; Liu, J. Monocular Visual-Inertial SLAM: Continuous Preintegration
and Reliable Initialization. Sensors 2017, 17, 2613.
14. Forster, C.; Carlone, L.; Dellaert, F.; Scaramuzza, D. On-Manifold Preintegration for Real-Time Visual–Inertial
Odometry . IEEE T rans. Robot. 2017, 33, 1–21.
15. Weiss, S.; Siegwart, R. Real-time metric state estimation for modular vision-inertial systems. In Proceedings of
the 2011 IEEE International Conference on Robotics and Automation (ICRA), Shanghai, China, 9–13 May 2011;
pp. 4531–4537.
16. Kneip, L.; Weiss, S.; Siegwart, R. Deterministic initialization of metric state estimation filters for
loosely-coupled monocular vision-inertial systems. In Proceedings of the 2011 IEEE/RSJ International
Conference on Intelligent Robots and Systems (IROS), San Francisco, CA, USA, 25–30 September 2011;
pp. 2235–2241.
17. Bloesch, M.; Burri, M.; Omari, S.; Hutter, M.; Siegwart, R. Iterated extended Kalman filterbasedvisual-inertial
odometry using direct photometric feedback. Int. J. Robot. Res. 2017, 36, 1053–1072.
18. Leutenegger, S.; Lynen, S.; Bosse, M.; Siegwart, R.; Furgale, P . Keyframe-based visual–inertial odometry
using nonlinear optimization. Int. J. Robot. Res. 2015, 34, 314–334.
19. Jones, E.S.; Soatto, S. Visual-inertial navigation, mapping and localization: A scalable real-time causal
approach. Int. J. Robot. Res. 2011, 30, 407–430.
20. Bloesch, M.; Omari, S.; Hutter, M.; Siegwart, R. Robust visual inertial odometry using a direct EKF-based
approach. In Proceedings of the 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems
(IROS), Hamburg, Germany , 28 September–2 October 2015; pp. 298–304.
21. Mourikis, A.I.; Roumeliotis, S.I. A multi-state constraint Kalman filter for vision-aided inertial navigation.
In Proceedings of the 2007 IEEE International Conference on Robotics and Automation, Roma, Italy ,
10–14 April 2007; pp. 3565–3572.
22.
Lupton, T.; Sukkarieh, S. Visual-inertial-aided navigation for high-dynamic motion in built environments
without initial conditions. IEEE T rans. Robot. 2012, 28, 61–76.
23. Shen, S.; Michael, N.; Kumar, V . Tightly-coupled monocular visual-inertial fusion for autonomous flight of
rotorcraft MA Vs. In Proceedings of the 2015 IEEE International Conference on Robotics and Automation
(ICRA), Seattle, WA, USA, 26–30 May 2015; pp. 5303–5310.
24. Mur-Artal, R.; Tardós, J.D. Visual-inertial monocular SLAM with map reuse. IEEE Robot. Autom. Lett. 2017,
2, 796–803.
25. Kong, X.; Wu, W.; Zhang, L.; Wang, Y . Tightly-coupled stereo visual-inertial navigation using point and line
features. Sensors 2015, 15, 12816–12833.
26. Kottas, D.G.; Roumeliotis, S.I. Efficient and consistent vision-aided inertial navigation using line
observations. In Proceedings of the 2013 IEEE International Conference on Robotics and Automation
(ICRA), Karlsruhe, Germany , 6–10 May 2013; pp. 1540–1547.
27. Zhang, G.; Lee, J.H.; Lim, J.; Suh, I.H. Building a 3-D Line-Based Map Using Stereo SLAM. IEEE T rans. Robot.
2015, 31, 1364–1377.
28. Pumarola, A.; Vakhitov , A.; Agudo, A.; Sanfeliu, A.; Moreno-Noguer, F. PL-SLAM: Real-time monocular
visual SLAM with points and lines. In Proceedings of the 2017 IEEE International Conference on Robotics
and Automation (ICRA), Singapore, 29 May–3 June 2017; pp. 4503–4508.
29.
Gomez-Ojeda, R.; Moreno, F.A.; Scaramuzza, D.; Gonzalez-Jimenez, J. PL-SLAM: A Stereo SLAM System
through the Combination of Points and Line Segments. arXiv 2017, arXiv:1705.09479.
30.
Bartoli, A.; Sturm, P . The 3D line motion matrix and alignment of line reconstructions. Int. J. Comput. Vis.
2004, 57, 159–178.
31.
Zuo, X.; Xie, X.; Liu, Y .; Huang, G. Robust Visual SLAM with Point and Line Features. arXiv 2017,
arXiv:1711.08654.
32. Qin, T.; Li, P .; Shen, S. VINS-Mono: A Robust and V ersatile Monocular Visual-Inertial State Estimator. arXiv
2017, arXiv:1708.03852.
33. Furgale, P .; Rehder, J.; Siegwart, R. Unified temporal and spatial calibration for multi-sensor systems.
In Proceedings of the 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS),
Tokyo, Japan, 3–7 November 2013; pp. 1280–1286.
34. Kok, M.; Hol, J.D.; Schön, T.B. Using inertial sensors for position and orientation estimation. arXiv 2017,
arXiv:1704.06053.
35. Kaess, M.; Johannsson, H.; Roberts, R.; Ila, V .; Leonard, J.J.; Dellaert, F. iSAM2: Incremental smoothing and
mapping using the Bayes tree. Int. J. Robot. Res. 2012, 31, 216–235.
36. Sola, J. Quaternion kinematics for the error-state Kalman filter. arXiv 2017, arXiv:1711.02508.
37. Lucas, B.D.; Kanade, T. An iterative image registration technique with an application to stereo
vision. In Proceedings of the 7th International Joint Conference on Artificial Intelligence (IJCAI),
Vancouver, BC, Canada, 24–28 August 1981.
38. Rosten, E.; Porter, R.; Drummond, T. Faster and better: A machine learning approach to corner detection.
IEEE T rans. Pattern Anal. Mach. Intell. 2010, 32, 105–119.
39. V on Gioi, R.G.; Jakubowicz, J.; Morel, J.M.; Randall, G. LSD: A fast line segment detector with a false
detection control. IEEE T rans. Pattern Anal. Mach. Intell. 2010, 32, 722–732.
40. Zhang, L.; Koch, R. An efficient and robust line segment matching approach based on LBD descriptor and
pairwise geometric consistency . J. Vis. Commun. Image Represent. 2013, 24, 794–805.
41. Yang, Z.; Shen, S. Monocular visual–inertial state estimation with online initialization and camera–imu
extrinsic calibration. IEEE T rans. Autom. Sci. Eng. 2017, 14, 39–51.
42. Agarwal, S.; Mierle, K. Ceres Solver. Available online: http://ceres-solver.org (accessed on 9 April 2018).
43. Kaehler, A.; Bradski, G. Learning OpenCV 3: Computer Vision in C++ with the OpenCV Library; O’Reilly Media,
Inc.: Sebastopol, CA, USA, 2016.
44. Burri, M.; Nikolic, J.; Gohl, P .; Schneider, T.; Rehder, J.; Omari, S.; Achtelik, M.W.; Siegwart, R. The EuRoC
micro aerial vehicle datasets. Int. J. Robot. Res. 2016, 35, 1157–1163.
45. Pfrommer, B.; Sanket, N.; Daniilidis, K.; Cleveland, J. PennCOSYVIO: A challenging visual inertial odometry
benchmark. In Proceedings of the 2017 IEEE International Conference on Robotics and Automation (ICRA),
Singapore, 29 May–3 June2017; pp. 3847–3854.
46.
Quigley , M.; Conley , K.; Gerkey , B.; Faust, J.; Foote, T.; Leibs, J.; Wheeler, R.; Ng, A.Y . ROS: An Open-Source
Robot Operating System; ICRA Workshop on Open Source Software; ICRA: Kobe, Japan, 2009; p. 5.
47.
Sturm, J.; Engelhard, N.; Endres, F.; Burgard, W.; Cremers, D. A benchmark for the evaluation of RGB-D
SLAM systems. In Proceedings of the 2012 IEEE/RSJ International Conference on Intelligent Robots and
Systems (IROS), Vilamoura, Portugal , 7–12 October 2012; pp. 573–580.
48. Lu, Y .; Song, D.; Yi, J. High level landmark-based visual navigation using unsupervised geometric constraints
in local bundle adjustment. In Proceedings of the 2014 IEEE International Conference on Robotics and
Automation (ICRA), Hong Kong, China, 31 May–7 June 2014; pp. 1540–1545.
49. Yang, Z.; Shen, S. Tightly-Coupled Visual–Inertial Sensor Fusion Based on IMU Pre-Integration; Technical Report;
Hong Kong University of Science and Technology: Hong Kong, China, 2016.

上面关于线特征看不太懂的可以看下面的论文。

基于点线综合特征的双目视觉SLAM方法 谢晓佳 浙江大学

3.1 图像中的线特征

3.2 环境地图中的线

直接分解很直接
公式3.12 和公式3.13一样的

3.3 点线综合的后端优化框架

上面的雅可比矩阵 14讲也推导过。

下面的是直线投影雅可比矩阵推导，重点！！！

这个推导其实也还好，仔细看应该是可以看懂的，实在看不懂直接看结果把，也就是公式3.43和公式3.44

3.4 点线综合的视觉词典

点线综合的词典，可以在vins上试试