vslam论文23：VIP-SLAM: 一种高效、紧耦合的RGB-D视觉惯性平面SLAM（ICRA 2022）

摘要

       本文提出了一种融合RGB、Depth、IMU和结构化平面信息的紧密耦合SLAM系统。传统的基于稀疏点的SLAM系统总是保持大量的地图点来建模环境。大量的地图点给我们带来了很高的计算复杂度，使其难以部署在移动设备上。另一方面，平面是人造环境尤其是室内环境中常见的结构形式。我们通常可以使用少量的平面来表示一个大的场景。因此，本文的主要目的是降低基于稀疏点的SLAM的高复杂性。我们构建了一个轻量级的后端地图，它由几个平面和地图点组成，以实现有效的BA，具有相同或更好的精度。利用单应性约束消除了优化过程中大量平面点的参数，降低了优化的复杂度。我们将单应性约束和点平面约束中的参数和测量分离，并对测量部分进行压缩，进一步有效地提高了BA的速度。我们还将平面信息整合到整个系统中，实现了鲁棒的平面特征提取、数据关联和全局一致的平面重建。最后，我们进行了消融研究，并在模拟和真实环境数据中将我们的方法与类似方法进行了比较。该系统在精度和效率方面具有明显的优势。即使涉及到平面参数的优化，我们也可以利用平面结构有效地简化后端图。全局BA比基于稀疏点的SLAM算法快近2倍。

一、引言

       综上所述，许多先前的工作表明，IMU有助于提高系统的鲁棒性，而结构平面有助于提高系统的精度和鲁棒性。此外，与线和点特征相比，平面可以使用更少的参数来模拟环境。

       在此基础上，我们充分利用多传感器的特点，提出了一个集IMU、RGB、Depth、Plane信息于一体的高鲁棒性、高精度的系统。

本文中有三个贡献:

（1）我们首次提出了一个完整的紧密耦合多传感器融合SLAM系统，融合RGB、Depth、IMU和结构化平面信息。将所有信息整合到一个统一的非线性优化框架中，共同优化关键帧姿态、IMU状态、点、面等参数。

（2）引入平面信息，减少地图点的数量，加快BA的优化。我们在优化过程中利用单应性去除点的状态，同时将多个约束压缩为一个约束。这些措施减少了优化时间。图4(a)和图4(b)显示了该过程。

（3）将平面信息整合到整个SLAM系统中，实现高精度跟踪。我们使用纯几何单帧点对平面约束来提高弱纹理场景中的平面估计的准确性和稳定性。此外，我们将平面点的重投影转换为单应性约束，以建立多个帧与平面之间的关系，进一步纠正漂移。图1显示了轨迹与重建平面的对比。

图1：轨迹和序列02中的重建平面。左上是深度图像，左下是RGB图像。右图:灰色区域为水平面。粉色、绿色和蓝色区域是不同的垂直平面。黑色，粉色和黄色的轨迹是地面真相，我们的VIP-SLAM和正常的VI-SLAM。

三、概述

A.系统概述

       提出的系统概述如图2所示。本系统以RGB、Depth和IMU为输入，由前端、平面模块和后端三个主要组件组成。前端模块是一个基于滑动窗口的VIO系统，可实时估计6-DoF姿态。我们的VIO类似于，只是我们没有考虑SLAM特征并添加了额外的深度测量。平面模块接收前端和后端数据作为输入。高频前端信息仅用于扩展平面。低频但是在新平面检测、平面扩展、点对面关联、面对面合并等过程中接受高精度的后端信息。由于室内环境的平面大多是水平或垂直的，所以我们只考虑水平和垂直平面的检测。但是，平面的融合优化适用于一般平面。后端模块采用局部BA(local bundle adjustment, LBA)或全局BA(global bundle adjustment, GBA)共同优化平面、点、IMU状态和关键帧姿态，修正前端姿态漂移。

 B. 符号

       我们首先定义在整个论文中使用的符号。我们认为是世界框架。为相机帧，为IMU帧。我们用表示一个姿态，它由旋转和平移组成。我们用表示一个平面，其中n是平面法线，d是原点到平面的距离。我们采用CP向量参数化平面Π,是需要估计的状态，包括姿态、速度、IMU偏差、点和面地标。

四、前端

       特征检测和匹配当接收到新的图像时，我们检测ORB特征点并计算相应的描述符。首先，我们使用KLT跟踪它们从最后一张图像到当前图像。然后，我们将具有3D信息的特征投影到当前图像上，并使用汉明距离找到最近的ORB特征点。将KLT跟踪结果作为初始值对剩余特征进行匹配，找到最优观测值。最后，我们使用基于ransac的基本矩阵来去除异常值

       运动估计运动估计是一种基于滑动窗口的VIO，它将RGB、depth和IMU紧密集成在一起。

我们的VIO类似于[7]，它使用平方根反滤波器融合所有测量值。主要区别在于我们没有考虑SLAM特征，而是将深度信息添加到视觉测量中。我们将在第VI-A.1节中详细描述深度视觉测量。

五、平面检测与关联

A.平面检测与合并

       平面检测平面模块只检测具有后端数据的平面。一旦检测到平面，平面模块将使用前端和后端数据扩展平面，并将平面与地图地标相关联。像[17]一样，我们使用Delaunay三角剖分创建3D网格和直方图检测平面。我们只检测垂直和水平面，通过检查网格法线是垂直还是平行于重力。我们的平面模块采用了一些额外的方法来提高平面精度。当从直方图中检测到一个平面时，我们将使用直方图中的数据和三维平面点来细化其参数，而不是直接使用直方图的比例值。对于水平面，我们设，平面距离为平面各点z轴的平均值。垂直面设，垂直面参数可细化为:

       我们使用QR分解求解式(1)，在检测平面参数的情况下，平面可以通过角度和距离关联三维网格。我们的角度和距离阈值是平面和3D网格之间的10度和5厘米。如图3所示。

图3:平面检测:(a)二维网格，(b)三维网格，其中红色和蓝色区域分别表示水平和垂直网格。(c)平面，其中垂直面为黄色，水平面为灰色。

       平面合并平面合并有两种策略。首先，我们检查一个平面与其他平面是否满足一定的角度和距离阈值。找到满足的平面后，检查满足平面的边界点是否相互包含。角度阈值和距离阈值分别是10度和10厘米。平面合并发生在检测到新平面或调整旧平面时。

B.点与平面关联

       我们使用3D网格将更多的地图点与平面联系起来。一旦深度图中的3D网格与平面相关联，我们将找到它的2D网格。如果一个地图点的二维坐标都在二维网格中，并且该地图点到平面的距离小于10厘米，则将该地图点添加到与该平面关联的候选集中。如果候选集上的一个点在超过3个关键帧中被观察到，我们将检查其几何一致性。我们计算点的重投影误差，强制点与平面相关联，然后计算另一个重投影误差。如果两次重投影误差相似，且最大重投影误差小于某一阈值，则认为点是一个平面点。如果一个点多次未能通过几何一致性，则将其从候选点集中删除。

六、后端

A.测量

       1) IMU和点特征测量:采用预积分方法对两个连续关键帧之间的IMU数据进行处理[8]，[9]。我们定义了基于IMU数据预积分的代价项，与[9]相同。

由于深度图像是有效的，我们将深度信息整合到视觉点特征测量中。定义第1个关键帧观测到的第1个特征的投影和深度残差为:

       2)压缩单应性测量:当三维地图点与平面相关联时，我们强制该点必须落在平面上。所以我们不用一般的点到平面的距离约束，而是用单应性矩阵来约束两个关键帧和一个平面。如果在第i帧和第j帧中观测到平面上的点，我们可以写出如下的点对平面和重投影方程:

       3)压缩点对面测量:单应性测量依赖于地图点的特征匹配，容易受到环境光照和纹理的影响。因此，我们增加了单帧点云和平面关联的几何约束，以提高平面估计的精度和无纹理场景下运动估计的稳定性。与激光雷达平面SLAM的相关工作类似[27]，[26]，[25]，我们使用压缩的点到平面的代价函数。是在第i个关键帧中观察到的第l个平面上N个点的集合。

平面点k的点对面残差定义为:

        其中twi为关键帧姿态。仅为一维，因此第1个平面上N个点到平面的代价函数为: 

B.局部平面和点的BA

       当一个新的关键帧被插入到映射中时，我们执行LBA优化。LBA优化了最新的K关键帧姿态，IMU状态，以及这些关键帧观察到的点和面。观察这些点和面的其他关键帧有助于成本函数，但在LBA中保持固定。这里，K被设置为20。我们采用Ceres1的LM算法来解决这个最小化问题。我们将最大迭代次数设置为10，最大求解时间设置为0.2s

C.全局平面和点的BA

       环路检测和位姿图平面优化与许多视觉SLAM[2]，[11]类似，我们使用DBoW2来检测环路闭合，并使用几何信息来验证其可靠性。在接受回路的情况下，首先采用图优化方法对较大的漂移进行校正。我们的优化问题是最小化以下能量函数:

       优化姿态平面图优化后，我们需要更新所有状态，包括关键帧姿态、IMU状态、点和面。对于GBA问题，我们融合了所有测量，包括预积分，重投影，压缩单应性，压缩点到平面和先验平面紧密耦合的形式。图4显示了这个问题。优化是使以下能量函数最小化:

图4:我们的优化问题(a)和(b)是压缩单应性约束前后的因子图。(c)是我们的全局BA问题

 七、实验

       我们将每个序列运行三次，并在表1和表2中显示平均精度结果。Kimera的结果来自[17]。在纹理弱、运动快的情况下，VI-SLAM算法比ORB-SLAM2和Manhattan-SLAM算法具有更高的精度和鲁棒性，这主要是由于IMU的集成不依赖于外部信息。VIP-SLAM和vi - slam精度最高，说明压缩多个观测值，用单应性约束代替平面上点的重投影约束是有效的。

       但由于闭环效应的存在，有效地消除了SLAM的累积误差，使得VIP-SLAM与VI-SLAM的精度差异较小。在关闭闭环模块后，VIP-SLAM和vi -SLAM在所有数据集上都取得了明显的优势，这有效地证明了平面约束对于缓解SLAM的累积误差非常有帮助。

       为了验证VIP-SLAM的优化花费时间，我们做了一个序列为09的GBA实验。所有算法的最大迭代次数为10次，并且不限制优化时间。图5显示了我们算法变体的GBA时间的比较。横轴为关键帧数，纵轴为GBA优化时间。我们发现VI-SLAMP的优化时间比VI-SLAM要长得多，这是由于有太多额外的点对平面和单应性测量。使用压缩测量值作为VI-SLAM- cp后，优化时间接近VI-SLAM，但仍高于VI-SLAM。最后，去除平面点的状态，优化时间比VI-SLAM快近2倍。我们还发现，随着关键帧数的增加，优化时间消耗会明显减少。表III显示了不同变体算法下GBA的耗时和准确性。GBA的状态包括215个关键帧、2236个平面点、2032个非平面点、10个垂直平面和6个水平平面。我们列出了优化耗时的几个重要部分，正如我们所看到的，我们的VIP-SLAM的所有重要部分都以接近的精度减少了耗时。

       表III显示了不同变体算法下GBA的耗时和准确性。GBA的状态包括215个关键帧、2236个平面点、2032个非平面点、10个垂直平面和6个水平平面。我们列出了优化耗时的几个重要部分，正如我们所看到的，我们的VIP-SLAM的所有重要部分都以接近的精度减少了耗时。

 八、结论

       我们的目标是充分利用多个传感器的信息，实现高精度、轻量化和鲁棒性的SLAM系统。实验证明了该系统的有效性。利用平面信息不仅可以减少系统的累积误差，还可以加快GBA的速度。我们的点平面GBA算法比基于稀疏点的SLAM算法快近2倍。到目前为止，我们系统的后端地图仍然包含了很多点。未来，我们将探索一个更轻量化的纯平面可视化SLAM后端是否可行。