PL-VIO: Tightly-Coupled Monocular Visual–Inertial Odometry Using Point and Line Features
摘要：为了解决基于惯性测量和视觉观测的相机轨迹估计和构建结构三维（3D）地图的问题，本文提出了点线视觉惯性里程计（PL-VIO），一种紧耦合利用点和线特征的单目视觉惯性里程计系统。与点特征相比，线提供了更多关于环境的几何结构信息。为了获得3D空间线的计算简单性和表示紧凑性，采用普吕克坐标和线的正交表示。为了紧密有效地融合来自惯性测量单元（IMU）和视觉传感器的信息，我们通过最小化成本函数来优化状态，该成本函数将预先集成的IMU误差项与点和线重新投影误差项组合在滑动窗口中 优化框架。在公共数据集上评估的实验表明，结合点和线特征的PL-VIO方法优于仅使用点特征的几个最先进的VIO系统。
1. Introduction
近年来，定位和导航在广泛的应用中引起了很多关注，特别是对于自动驾驶汽车，服务机器人和无人驾驶飞行器等。几种类型的传感器用于定位和导航，例如全球导航卫星系统（GNSS）[1]，激光雷达[2,3]，惯性测量单元（IMU）和摄像机[4,5]。然而，它们具有明显的各自缺点：如果有清晰的天空视图，GNSS仅提供可靠的定位信息[6]; 激光雷达遇到玻璃表面物体的反射问题[7]; 来自民用IMU的测量结果是有噪声的，因此惯性导航系统可能由于误差累积而快速漂移[8]; 单眼同时定位和制图（SLAM）只能恢复到一定比例的运动轨迹，当摄像机快速移动或光照变化剧烈时，它往往会丢失[9-11]。因此，传感器融合方法，特别是视觉惯性导航系统，已引起广泛关注[12]。 来自IMU的加速度和角速度信息可以显着改善单眼SLAM系统[13,14]。 此外，IMU和相机都重量轻且成本低，因此它们广泛用于民用领域。
根据传感器的直接或间接融合测量，视觉惯性里程计（VIO）系统可分为两个主要流：松耦合和紧耦合方法。松散耦合方法[15,16]通过两个估计器处理图像和IMU测量，这两个估计器分别估计相对运动并融合来自两个估计器的估计以获得最终结果。紧耦合方法[17,18]使用一个估计器通过直接融合来自摄像机和IMU的原始测量来找到最佳估计。 与松散耦合方法相比，紧耦合方法通常更准确和更健壮。 在本文中，提出的PL-VIO方法是一个紧耦合的VIO系统。 关于紧耦合VIO方法的相关工作可以通过测量模型中的线性化数量来分类[14]。这些基于扩展卡尔曼滤波器（EKF）的方法仅在更新步骤中处理一次测量，而批次非线性优化在优化步骤期间多次线性化。 基于滤波的方法[19,20]整合IMU测量以传播/预测状态，然后通过视觉测量更新/校正最新状态。由于三维（3D）地标的坐标包括在状态向量中，因此EKF的计算复杂度随着地标的数量而呈二次方增加。 为了解决这个问题，Mourikis和Roumeliotis [21]提出了多状态约束卡尔曼滤波器（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系统中集成线要素的最简单方法是使用两个端点来表示线。 匹配来自不同视图的线的端点是困难的。 此外，3D空间线仅具有四个自由度（DoF），而两个3D端点引入六个参数，这导致过度参数化。Bartoli和Sturm [30]提出了正交表示，它使用三自由度旋转矩阵和一个多自由度旋转矩阵来更新优化期间的线参数。 正交表示已经用于一些立体视觉SLAM系统[27,31]。 对于VIO方法，Kottas和Roumeliotis [26]仅使用线路特征研究了VIO的可观察性。 Kong等人[25]利用三焦几何构建了一个结合点和线特征的立体声VIO系统。 但是，这些工作涉及基于过滤的VIO。 在我们提出的PL-VIO方法中，我们将线要素集成到优化框架中，以实现比基于过滤的方法更高的精度。
为了构建结构3D地图并获得相机的运动，我们提出了PL-VIO系统，它通过联合最小化IMU预积分约束以及滑动窗口中的点和线重新投影误差来优化系统状态。 与仅使用点特征的传统方法相比，我们的方法利用附加线特征，旨在提高照明变化环境中的鲁棒性和准确性。 我们的主要贡献如下：
据我们所知，拟议的PL-VIO是第一个使用点和线作为地标的基于优化的单目VIO系统。
为了紧密有效地融合来自视觉和惯性传感器的信息，我们引入了具有IMU预积分约束和点/线特征的滑动窗口模型。 为了在优化中紧凑地表示3D空间线，采用线的正交表示。导出关于IMU体状态的所有误差项的雅可比矩阵，以有效地求解滑动窗口优化。
我们在EuRoc数据集和PennCOSYVIO数据集上比较了所提出的PL-VIO与三种最先进的单眼VIO方法的性能，包括ROVIO [17]，OKVIS [18]和VINS-Mono [32]， 报告详细的评估结果。
本文的其余部分安排如下。 首先，我们在第2节中描述了数学预备，然后在第3节中制定了基于滑动窗口的视觉 - 惯性融合方法。接下来，我们将在第4节中描述我们的PL-VIO系统和实现细节。第5节显示实验结果。 最后，第6节给出了结论和未来的潜在工作。
2. Mathematical Formulation
2.1. Notations
图1显示了视觉惯性传感器，以及点和线特征的视觉观察。我们将c i表示为时间t = i时的相机帧，并且b i同时表示为IMU体帧。
2.2. IMU Pre-Integration
一个6轴IMU，包括一个3轴加速度计和一个3轴陀螺仪，可以测量车体框架相对于惯性框架的加速度a和角速度ω[14]。原始测量ω和â是 受偏见和白噪声的影响：
where ⊗ denotes the quaternion multiplication operation.
预积分模型（等式（6））源自连续时间并忽略偏差和噪声。 在实践中，IMU测量是从离散时间收集的，应考虑噪声。 在这项工作中，我们使用中点集成来集成IMU测量。 使用离散时刻k和k + 1处的测量的IMU体传播通过以下公式计算：
在实践中，偏见变化缓慢。 我们用随机游走噪声模拟偏差：
2.3. Geometric Representation of Line
直线只有四个DoF。 因此，直线的紧凑参数化具有四个参数。 在我们的系统中，我们将3D空间中的直线视为无限直线，并采用两个参数化作为3D线，如[27]中所述。由六个参数组成的Plücker线坐标由于其简单性而用于变换和投影。 由四个参数组成的正交表示由于其紧凑性而用于优化。
2.3.1. Plücker Line Coordinates
2.3.2. Orthonormal Representation
由于3D空间线仅具有四个自由度，因此在优化期间，正交表示（U，W）∈SO（3）×SO（2）比Plücker坐标更合适。另外，正交表示和Plücker坐标可以相互转换，这意味着我们可以在SLAM系统中采用它们用于不同的目的。 在本节中，我们将介绍正交表示的细节。 如图2a所示，在3D线上定义坐标系。 归一化法向量和归一化方向向量是坐标系的两个轴。 通过交叉其他两个轴向量确定第三轴。 我们可以将线坐标和相机框架之间的旋转矩阵定义为U：
3. Tightly-Coupled Visual–Inertial Fusion
在视觉SLAM中，通过最小化图像平面中的重新投影误差，使用束调整来优化相机姿势和3D地图点。 通过非线性优化进行的束调整可以视为因子图[35]，如图3a所示：圆形节点是需要优化的相机姿态或3D地标; 带方框的边表示视觉测量值作为节点之间的约束。 对于视觉惯性融合，我们还可以使用紧耦合的基于图的框架来优化视觉惯性系统的所有状态变量[23]。 如图3b所示，因子图不仅包含视觉测量，还将预先集成的IMU测量作为边缘来约束连续的IMU体状态。
3.1. Sliding Window Formulation
为了实现计算效率和高精度，我们使用滑动窗口公式进行因子图优化。 在时间i的滑动窗口中的完整状态变量被定义为：
其中x i由世界框架中的第i个IMU主体位置，速度和方向以及IMU主体框架中的偏差来描述。 下标n，m和o分别是身体状态，点地标和线地标的起始索引。 N是滑动窗口中关键帧的数量。M和O分别是滑动窗口中所有关键帧观察到的点标志和线标志的数量。 我们仅使用一个变量，即逆深度λk，从其第一个观察到的关键帧参数化第k个点地标。 O l是世界框架w中第l行特征的标准正交表示。
我们通过最小化所有测量残差的成本项之和来优化滑动窗口中的所有状态变量：
我们使用Levenberg-Marquard算法来解决非线性优化问题（20）。通过从初始猜测X 0迭代更新，可以找到最优状态估计X：
其中⊕是用于以增量δX更新参数的运算符。 对于位置，速度，偏差和反向深度，更新运算符和增量δ很容易定义：
然而，更新运算符和状态态q的增量δ更复杂。四元数中使用四个参数来表示三自由度旋转，因此它被过度参数化。旋转的增量应该只是三维的。 与[18]类似，我们在切线空间中使用扰动δθ∈R3作为旋转增量。 因此，可以通过四元数乘法更新旋转q：
3.2. IMU Measurement Model
由于通过等式（10）计算的预积分IMU测量值是两个连续关键帧b i和b j之间的约束因子，因此IMU测量残差可以定义为：
其中[·] xyz提取四元数的实部，用于逼近三维旋转误差[18]。
在非线性优化期间，IMU测量残差的雅可比矩阵相对于身体状态x i和x j通过以下公式计算：
3.3. Point Feature Measurement Model
3.4. Line Feature Measurement Model
4. Monocular Visual Inertial Odometry with Point and Line Features
如图4所示，建议的PL-VIO系统有两个主要模块：前端和后端。 前端模块用于预处理来自IMU和摄像头的测量。后端模块用于估计和优化身体状态。 我们将在以下小节中介绍细节。
4.1. Front End
前端从摄像机和IMU的原始测量值中提取信息。 通过每次新IMU测量的传播来更新身体状态，并且最新的身体状态被用作滑动窗口优化中的初始值。 另外，新的IMU测量被预先集成以在优化期间约束连续的IMU身体状态。
对于图像处理，在两个单独的线程中检测点和线特征。当新帧到来时，通过KLT光流算法[37]从前一帧到新帧跟踪点特征。然后，我们使用RANSAC框架和基本矩阵测试来删除异常值。如果离群值抑制后跟踪点要素的数量小于阈值，则将添加由FAST检测器[38]检测到的新拐角特征。对于线特征，LSD检测器[39]检测新帧中的线段，并通过基于外观的描述符LBD [40]与前一帧中的线段匹配。我们使用几何约束来删除线匹配的异常值。例如，两条匹配线的中点之间的距离应不大于dist像素，并且角度差应不大于δth角度。在特征检测和匹配之后，将线段的点特征和端点投影到归一化图像平面上。另外，如果跟踪点特征的平均视差大于阈值，则选择帧作为新关键帧。
4.2. Back End
在后端线程中，首先对点和线进行三角测量以构建重新投影残差。为了获得良好的地标估计，利用所有观测值估计点特征的逆深度。 对于线三角测量，我们只在滑动窗口中选择具有最远空间距离的两个帧来初始化Plücker坐标。
在我们获得地图点/线的初始估计以及从IMU测量预测的IMU体状态之后，使用第3节中描述的滑动窗口优化来找到最佳状态。 为了限制状态向量X的大小，采用双向边缘化策略从滑动窗口中去除状态[23]。当第二个最新的帧x n + N - 1是一个关键帧时，我们将所有测量结果边缘化出最早的帧x n。 否则，如果第二个最新帧不是关键帧，我们将丢弃此帧中的可视测量值，并在预积分测量中保留其IMU测量值。 基于边缘化测量获得新的先验信息，从被移除的状态保留约束信息。
最后，我们剔除无效的地图点和线。 如果点的反深度为负，我们将从地图中删除此点。 如果线的重新投影残差超过阈值，则将从地图中移除。
4.3. Implementation Details
5. Experimental Results
我们使用两个公共基准数据集评估了我们的PL-VIO系统：EuRoc MAV数据集[44]和PennCOSYVIO数据集[45]。将PL-VIO方法的准确性与三种最先进的单眼数据进行比较 用于验证PL-VIO方法优点的VIO方法：单眼模式下的ROVIO [17]和OKVIS [18]，以及没有闭环的VINS-Mono [32] .ROVIO是一种基于扩展的紧密耦合形式的VIO 卡尔曼滤波器（EKF）。 它直接使用图像中的强度误差来找到更新步骤中的最佳状态。 OKVIS是一种滑动窗口优化算法，具有单点或立体模式的点特征.VINS-Mono是一个完整的VIO-SLAM系统，采用点特征在滑动窗口中优化IMU体状态，并执行循环闭合。 所有的实验都在计算机上进行，配备Intel Core i7-6700HQ CPU，2.60 GHz，16 GB RAM和ROS Kinetic [46]。
5.1. EuRoc MAV Dataset
EuRoc微型飞行器（MAV）数据集由MAV在两个室内场景中收集，其中包含来自20FPS的全局快门相机的立体图像和200Hz的同步IMU测量[44]。 每个数据集提供由VICON运动捕捉系统给出的地面实况轨迹。 所有的外在和内在参数也在数据集中提供。在我们的实验中，我们只使用左相机的图像。
线要素的主要优点是它们提供了与环境有关的重要几何结构信息。 作为一个例子，我们展示了由PL-VIO从图5中的MH_05_difficult序列构建的重建地图。图5a-d中的四个图像由机房大厅中的MAV捕获，其显示了房间的结构。 如图5d所示，弱照明场景中的线段检测比点特征检测更稳健。从重建的3D图可以看出环境的几何形状由线段描述，因此语义信息 可以从地图中提取。 这对机器人导航很有用。
为了进行定量评估，我们将PL-VIO与三种最先进的单眼视觉惯性方法进行了比较：单眼模式下的ROVIO [17]，OKVIS [18]和没有闭环的VINS-Mono [32]。 为了公平比较，使用了这些比较方法的作者提供的默认参数。 我们选择绝对姿态误差（APE）作为评估指标，直接比较估计与地面实况之间的轨迹误差[47]。 开源软件包evo（https://michaelgrupp.github.io/evo/）提供了一个易于使用的界面来评估测距和SLAM算法的轨迹。我们在本节中使用此工具来评估这些方法。 表1显示了沿着所有轨迹的平移和旋转的均方根误差（RMSE），并且还提供了它们的直方图，如图6所示。
表1显示，除了V1_01_easy之外，我们的PL-VIO共同优化了点和线，为旋转的八个序列提供了最佳性能。 除了V1_01_easy，V1_02_medium和V1_03_difficult之外，我们的方法在六个平移序列上也表现最佳。但是，相对于最佳结果的差异仅在亚毫米级别。 表1中的结果表明，将线特征集成到VIO中可以提高运动估计的准确性。为了直观地展示结果，PL-VIO和VINS-Mono估计的轨迹的几个热图如图7所示。颜色越红，平移误差越大。 比较三个轨迹，我们得出的结论是，当相机快速旋转移动时，具有线条特征的PL-VIO比VINS-Mono产生更小的误差。 此外，我们发现这些快速旋转的序列引起观察方向的大的变化，并且照明条件对于跟踪点特征尤其具有挑战性[25,26,28]。 如图8所示，27对点（包括10个异常值）成功匹配，而33线成功匹配且所有匹配都正确。
此外，表2列出了不同方法的计算时间。基于滤波器的ROVIO的计算效率最高，而其精度最低。 提出的PL-VIO系统是最耗时的方法，但其计算时间主要受线路检测和匹配步骤的限制。 在5.3节中，PL_VIO中不同模块的计算时间在V1_03_difficult序列中独立估算，并且发现PL-VIO系统的计算效率直接取决于线路检测和匹配。
5.2. PennCOSYVIO Dataset
PennCOSYVIO数据集是最近的VIO基准测试，它使用从室外到室内的手持设备收集大型玻璃建筑的同步数据（参见图9）[45]。 挑战因素包括光照变化，快速旋转和重复结构。 提供了所有内在和外在参数以及地面实况轨迹。 我们使用VI传感器收集的数据，也用于EuRoc MAV数据集。
我们将提出的PL-VIO与没有闭环的VINS-Mono进行了比较。 为了公平地评估，相同的参数用于PL-VIO和VINS-Mono。 这里采用5.1节中使用的相同度量和评估工具来评估轨迹。 表3列出了结果。
此外，评估工具（https://daniilidis-group.github.io/penncosyvio/）也在PennCOSYVIO数据集中提供，并采用两个指标，即APE和相对姿势误差（RPE）。 对于RPE，它通过将值除以路径长度来表示百分比误差[45] .PennCOSYVIO的创建者谨慎选择评估参数，因此他们的工具适用于评估此数据集中的VIO方法。 因此，我们在实验中采用了这种评估工具，结果列于表4中。
从表3和表4可以看出，PL-VIO获得了旋转部件的最佳性能。 由PennCOSYVIO工具评估的翻译APE提供了更多细节。 与VINS-Mono相比，PL-VIO在y轴和z轴上产生较小的误差，并且三个轴的误差总和较小。 VINS-Mono仅在x轴上获得了更好的性能。
5.3. Computing Time
最后，我们评估了在V1_02_medium序列上运行的PL-VIO的平均执行时间，因为该图像序列是从典型的室内场景中收集的。 表5显示了每个块的执行时间。 我们可以看到，前端以11 Hz运行的线路检测和匹配是效率方面的瓶颈。最先进的线路检测和匹配方法，例如LSD和LBD的组合，对于VIO / SLAM系统来说并不令人满意。 请注意，我们的方法与线特征检测和匹配无关，因此提高其效率超出了本文的范围。 后端的边缘化是另一个耗时的部分。 我们观察到边缘化的低效率是由边缘化特征时的填充引起的，这使得Hessian矩阵变得不那么稀疏矩阵。 通过在执行边缘化时丢弃一些特征来维持稀疏的Hessian矩阵，可以解决这个问题[18]。
6. Conclusions
本文介绍了新颖的紧耦合单目视觉 - 惯性测距算法PL-VIO，该算法优化了具有点和线特征的滑动窗口中的系统状态。提出的PL-VIO系统有两个主要模块：前端和前端后端。前端模块用于传播IMU体状态，并检测和匹配点/线特征。后端模块用于估计和优化身体状态。在后端模块中，线标志被认为是无限3D空间线，并且其正交表示用于在优化期间紧凑地参数化。此外，为了有效地求解滑动窗口优化，详细给出了误差项的所有雅可比矩阵。我们还提供了所提出的PL-VIO的评估结果，与三种最先进的单眼VIO方法相比，包括ROVIO [17]，OKVIS [18]和VINS-Mono [32]对EuRoc数据集和PennCOSYVIO数据集。根据分析和结果，得出两个进一步的结论如下：
具有线特征的重建的3D地图可以提供关于环境的几何信息，因此可以从地图提取语义信息。 这对机器人导航很有用。
线要素可以提高平移和旋转的系统精度，尤其是在照明变换场景中。 然而，线路检测和匹配是耗时的并且成为系统效率的瓶颈。
在未来，我们计划通过在三维空间线之间引入结构约束来改进我们的系统，例如曼哈顿世界场景中的平行线或共面线[48]。 这些线之间的几何约束有可能进一步提高定位精度并减少旋转累积误差。