vslam论文21：基于点、面图的高效视觉惯性导航（ICRA 2023）

摘要

       相对于全局先验地图，精确和实时的全局姿态估计在许多应用中是必不可少的，例如微型飞行器和增强现实的物流。假设纯稀疏的三维点图可以提供环境的无结构表示，那么生成点平面先验图可以进一步建模环境拓扑并为精确定位提供全局约束。为了实现这一点，我们提出了一个基于滤波器的大规模视觉惯性里程计系统，称为PPM-VIO，它利用点平面图来纠正累积漂移。该系统利用语义信息检测稀疏点云的共面信息，通过几何约束、语义约束和描述符约束实现精确的在线平面匹配。为了提高定位性能，我们将全局平面测量和点测量有效地集成在一个基于滤波器的估计器中。在不同场景下收集的真实数据集上广泛验证了所提出方法的有效性。实验结果表明，与单独使用点地图相比，利用先验地图中的平面信息可以获得更好的轨迹估计，并扩大了先验地图在不同场景下的有效范围。

论文：      https://ieeexplore.ieee.org/document/10160393

一.引言

总体而言，我们工作的关键贡献如下:

（1）我们开发了一个基于点平面地图的大规模VIO系统，该系统完全运行在计算和内存资源有限的设备上，同时提供准确和实时的定位。

（2）为了利用环境的结构信息，我们提出了一种离线点平面地图构建方法和一种高效的在线匹配策略。该方法离线执行语义分割和平面检测操作，并利用语义、几何和描述符约束提高在线平面匹配的正确性和鲁棒性。

（3）我们提供了一种全局定位方法，有效地将点和平面全局信息整合到估计器中，以避免漂移积累的影响。

（4）我们在实际数据集上评估了所提出的PPM-VIO，这表明我们的系统在低计算消耗的情况下提高了估计器的精度。此外，与单独使用点图相比，利用先验图中的平面信息可以显著提高定位精度，特别是在低纹理和以平面为主的场景下。

二.系统概述

       基于MSCKF估计器，本文提出的PPM-VIO采用点平面先验映射来提高系统的精度和鲁棒性。如图1所示，本文提出的系统包含三个主要模块:离线映射、在线查询和定位。在离线映射阶段，我们遵循经典的SFM生成稀疏点云，然后利用语义分割和投票方法获得点云的语义标签。在语义点云的基础上，采用区域增长聚合算法和基于RANSAC的平面拟合生成精确的平面地图。在在线阶段，利用快速检测和ASLFeat描述符建立包含地图平面信息的二维查询点与三维地图点的对应关系。此外，还可以通过描述符匹配和最近邻搜索来获得语义标签。在定位过程中，将匹配点作为点映射全局约束加入到局部估计量中，对系统的姿态进行约束。此外，通过2D-3D对应关系、语义标签和平面几何参数，我们可以找到地图平面上的所有特征，从而提供共面全局约束。最后，通过将全局约束、局部窗口的MSCKF和SLAM约束集成到估计器中，可以获得精确的全局定位，而不会产生累积漂移。

图1:PPM-VIO系统概述。

三.离线地图

如图1所示，我们在离线阶段预构建了一个点平面地图，并按照以下步骤生成地图。

（1)点云图生成:我们按照经典的SFM流程构建点云图，生成准确的相机姿态和3D点，从而帮助我们在后续流程中生成准确的平面结构信息和全球定位。

（2)语义标签分配:由于点云的语义信息是有帮助的，我们利用语义分割模型来生成映射图像的语义标签。我们都知道，在上一步生成的点云中，一个3D点将被多个摄像机观察到，因此与几个语义类相关联。由于可能的视图遮挡或分割错误，一个3D点可以与几个语义类相关联，因此它的语义类被投票为最常见的一个。

（3)平面拟合:我们的方法产生的点云是稀疏的，因此我们选择区域生长的方法来产生平面。为了提高效率，我们删除了那些不能明确形成平面的地标，如树木和车辆。然后，我们利用邻近点计算每个三维点的曲率。所有满足曲率一致性、语义一致性和距离一致性的三维点将被划分到同一区域。

       使用RANSAC方法，将每个点云区域拟合为一个平面，并利用其语义类别、点到平面距离的标准差和平面协方差进行定位。

四.在线视觉惯性定位

所提系统的状态向量定义如下:

 A. IMU传播

惯性状态 xI 使用输入的线性加速度 am 和角速度wmIMU测量去前向传播，基于以下通用非线性 IMU运动学 ：

B. 局部测量更新

基于轨迹长度，我们将所有跟踪的点特征分为“MSCKF”和“SLAM”，分别对应不同的更新方式。

（1) MSCKF特征更新:考虑从k时刻的相机图像中检测到一个3D特征，其在图像平面上的测量值为:

 线性化残差方程为:

（2) SLAM特征更新:对于那些可以比当前滑动窗口更长时间可靠跟踪的特征，我们将其初始化为状态，并使用Eq.(13)直接更新状态。 

C.点图测量更新

在MSCKF的基础上，我们详细描述了如何充分利用先验点图来纠正局部姿态估计中的漂移。

（1)地图匹配:

       在在线阶段，除了实时VIO系统外，当当前图像帧为关键帧且与上一次触发的时间间隔大于1秒时，我们将触发单独的线程进行地图匹配。对于帧触发的地图匹配，除了在VIO过程中提取的点(称为VIO点)外，我们还将提取一些额外的点来保证足够的地图匹配。利用ASLFeat计算描述符，该描述符对光照具有鲁棒性，但对尺度敏感。为了克服比例尺问题，我们根据当前飞行高度将图像大小调整为与构建地图时相同的比例尺，并在调整后的图像上提取描述符，避免了建立多比例尺地图，从而减小了先验地图的大小。

       在地图匹配过程中，局部姿态估计器可以提供一个足够的先验姿态，以便我们找到附近的图像和3D点。基于先验位姿，将相邻点云投影到查询图像上，并在投影点之间寻找具有相似视觉描述符的匹配点。然后，利用比值测试和PnP RANSAC技术可以过滤掉大部分的假匹配。上述过程流程图如图2所示。

图2:地图匹配流程图。

2)点图的全局融合:

       为了引导定位系统，在起飞位置放置一个离线校准的标记板，这有助于我们对齐VIO本地坐标和地图坐标系统。随后，结合全局信息的2D-3D点匹配可以直接用于更新局部估计量。根据匹配点的类型(VIO点或其他点)，我们采用不同的方法进行全局融合，以获得更好的定位效果。

与VIO点的匹配:对于全局2D-3D匹配，其中2D点可以在滑动窗口中跟踪，而不添加到状态中，我们将点的3D位置作为地图点xM添加到状态中。对于点图测量，非线性测量模型为

我们对该测量模型进行线性化处理，得到如下残差:

与其他点匹配:由于额外提取的匹配点在未来没有其他观测值，因此我们不估计该状态下对应的地图点。这种情况下的线性化测量模型为

D.平面图测量更新

1)平面数据关联:

点-平面地图提供的先验信息包括以下几类:

平面信息:对于用地图检索到的每一个点，该点是否在一个平面上，由地图提供。对于地图平面上的点，我们可以得到相应的平面信息，包括平面ID、语义ID、平面参数，以及对应的协方差RΠ，由求解。

语义信息:与地图一起检索到的所有SLAM特征的语义标签，由地图点对应的语义标签分配。对于与映射不匹配的特征，应用最近邻方法分配语义标签。由于映射匹配模块还需要查找每个点的最近邻居，因此这里没有添加额外的计算。

       除了已经与地图匹配的点之外，我们的目标是在匹配点所在的平面上找到更多的SLAM特征，从而提供更多的全局约束。为此，我们将具有语义信息的3D SLAM特征投影到触发全局定位的查询图像上，并对投影点应用2D Delaunay三角剖分。但是，由于在三角建立过程中没有考虑共平面性，可能会出现一些无效的三角块。为了去除不共面三角形，我们过滤掉三个顶点的语义标签不同的三角形，并将剩余的三角形作为候选三角形，这些三角形具有语义信息并且三个顶点共面。

       对于每一个具有平面信息的匹配点，我们找到一个以匹配点为顶点的候选三角形或一个包含该匹配点的候选三角形，如图3所示，然后将满足以下几何约束的候选三角形作为种子三角形。

图3:基于地图匹配、几何信息和语义信息，检测slam特征与地图特征的共面规律

2)平面地图的全局更新:

       一旦在地图平面上检测到特征，这种共面约束总是存在。为保证计算效率，仅在触发映射匹配时更新约束。考虑在地图平面上确定的一个平面点

我们有如下平面上点的测量约束:

       在这里，我们将平面上的点约束作为平面模型特征不确定性的概率补偿，而不是硬约束。 表示点平面约束的噪声，其中σd表示平面离线拟合时点到平面距离的标准差。我们对该测量模型进行线性化处理，得到如下残差:

五、现实实验

       在本节中，为了证明我们方法的有效性和鲁棒性，我们在两个不同的环境(城镇和草原环境)中进行了实验，并从RTK-GPS中获得了地面真值。数据采集平台的传感器包括一个IMU (250Hz)、一个向下立体摄像头(10Hz)和一个向下LRF (100Hz)，有效距离为120m。我们评估了估计器的计算成本和每个模块的精度。

A.定位精度

1)城镇序列:

       基于杭州市收集的数据对系统进行评估，杭州市平面图如图4(a)所示。在这些序列中，点映射匹配效果更好，其中召回率(有效匹配的数量除以触发的映射匹配的数量)超过0.9，平均内线数超过80。在不同高度和轨迹长度的序列中，我们比较了基于不同先验图谱类型的VIO和纯VIO。

       表1给出了所有方法的绝对轨迹误差(ATE)。VIO的累积漂移可以通过使用包含全局信息的点图或平面图来纠正。请注意，城镇场景中大多数平面法线方向与重力方向平行，这导致对水平位置和偏航的约束作用较弱，从而解释了点图的精度优于平面图的原因。在包含其他方向平面的序列中，平面图的改进效果更为明显。如表1和图5所示，在城镇场景中，本文提出的PPMVIO算法对纯VIO系统的累积漂移进行了校正，在3km的轨迹范围内，获得了较高的定位精度，定位误差小于1 m。

 2)草地序列:

       为了验证我们提出的平面地图先验算法的有效性，我们在具有重复纹理环境(如草坪)和非共面环境(如树木)等挑战的草地序列上进一步评估了我们提出的系统。点平面图如图4(b)所示。在这些序列中，由于匹配点个数小于15，召回率小于0.3，难以构建点图匹配。当匹配点数量较少时，点图的约束效果有限，但这种情况下可能存在较强的共面约束。如表2所示，平面地图约束能有效提高定位系统的精度，而点地图约束效果较差。

 所提出的点平面地图拓宽了原有地图在不同场景下的有效范围。

B.运行时间评估

       在本节中，我们评估了该方法在Intel Core i7-10750H CPU和NVIDIA GeForce GTX 1650 Ti平台上的执行效率。考虑到系统的实时性，我们将滑动窗口的大小设置为10，在正常帧中提取点的数量设置为100，在地图匹配帧中提取点的数量设置为500，在更新中应用的最大地图点数量设置为50。从表中。第三，我们可以观察到，在我们的系统中，平面匹配、平面更新和地图点更新是非常有效的。最耗时的部分是地图匹配模块，但这部分只在1Hz以下触发。因此，与纯VIO系统相比，我们的系统的总时间只增加了2 ms, CPU负载增加了3%。

六.结论

       在本文中，我们提出了一个高效的、大规模的VIO系统，该系统利用了先前点平面地图的全局信息，可以更有效地利用结构化环境中可用的几何信息。使用无论是基于语义稀疏云的精确平面检测，还是基于点描述子、语义信息和几何约束的精确平面匹配，都可以检测出地图平面上的所有点。此外，采用了一种高效的点-平面地图融合策略，提高了定位精度。在不同场景的MAV数据集上广泛验证了该方法的有效性。实验结果表明，利用先验地图中的平面信息比单独的点地图和纯VIO系统能显著提高定位精度，特别是在低纹理和以平面为主的环境下，具有广阔的应用前景。