商汤×浙大联合提出VIP-SLAM：高效的紧耦合RGB-D视觉惯性平面SLAM

论文链接：https://arxiv.org/pdf/2207.01158.pdf

导读

传统基于稀疏点的SLAM系统需要维护大量3D地图点来对环境进行建模以维持高精度跟踪。大量的3D地图点给系统带来高计算复杂度，使其难以部署在移动设备上。平面是结构化环境中非常常见的特征，尤其是在室内环境中，通常可以用少量的平面对一个大的场景进行建模。

本文的主要目的是降低基于稀疏点SLAM 的高复杂度同时维持高精度跟踪。我们构建一个由少量平面和非平面3D地图点组成的轻量级后端地图，以实现具有相同或更好精度的集束调整（BA）。使用单应性约束消除优化问题中大量平面点的状态量，降低BA的复杂度。而且将单应性和点到面约束中的状态量和测量分开，并压缩测量部分以进一步有效地提高BA的速度。

我们还将平面信息集成到整个系统中，以实现鲁棒的平面特征提取、数据关联和全局一致的平面重建。最后，在模拟和真实环境数据的实验以及消融实验表明我们的方法在精度和效率上取得了明显的优势。即使平面参数参与优化，我们通过有效利用平面结构简化了后端地图。全局束调整比基于稀疏点的 SLAM 算法快近 2 倍，比普通平面SLAM快近6倍。

创新点

1) 首次提出完整融合RGB、Depth、IMU和结构化平面信息的紧耦合多传感器融合SLAM系统。

2) 引入平面信息以减少地图点的数量并加快BA的优化。我们使用单应性来去除优化中点的状态量，并将多个约束同时压缩为一个。这些措施减少了优化时间。

3) 将平面信息整合到整个SLAM系统中，实现高精度实时跟踪。使用纯几何单帧点到平面约束来提高无纹理场景中平面估计的准确性和稳定性。此外，将平面点的重投影转换为单应性约束，以建立多帧和平面之间的关系，进一步纠正漂移。

背景

目前与本文工作相关的方法主要分为两种：RGBD惯性SLAM和基于平面的SLAM。

RGBD惯性SLAM

在过去的几年里，RGB-D传感器变得越来越流行。许多方法[1]、[2]、[3]、[4]、[5]提出使用RGB-D相机进行实时3D稠密重建或SLAM。然而，传统的RGB-D SLAM在特殊场景下容易出现鲁棒性问题，例如快速运动、弱纹理、动态环境、平坦的白墙等。为了减少对视觉和几何的依赖，研究人员提出了一些RGB-D惯性SLAM系统。[6]提出整合 IMU和RGB-D信息，实现第一个紧耦合RGB-D惯性SLAM系统。实验表明他们的系统在快速运动、弱纹理和弱几何场景中的优势。VINS-RGBD[7] 将深度信息集成到单目视觉惯性系统VINS-Mono[8]。使用深度信息来加速系统初始化、稳定系统尺度并提高系统准确性和鲁棒性。在 ORB-SLAM3[9] 中，惯性约束和多地图的方法被融合到 ORB-SLAM2[5] 中。然而，ORB-SLAM3仅支持单目/立体视觉惯性SLAM或 RGB-D SLAM。

基于平面的SLAM

很多研究人员已经探索了使用平面结构提高SLAM系统精度的方法。[10] 提出了一种基于RANSAC的基于点和平面的定位配准方法。[11] 使用全局平面模型来减少RGB-D SLAM漂移。Kimera[12]提出使用2D图像Delaunay三角剖分和相应的稀疏点云来重建3D Mesh。SP-SLAM[13]为ORB-SLAM2添加了平面特征。[14]强制平面上的点一定落在平面上，并基于此假设添加新的特征点和平面约束。KDP-SLAM[15]开发了一个快速密集的平面SLAM系统，它优化了全局因子图中的关键帧姿势和平面。但是，平面和VO的松散耦合可能会导致在具有一个或两个平面的场景中运动估计的准确性下降。

有一些工作将环境建模为曼哈顿或亚特兰大世界，并使用此假设来减少旋转中的累积定位误差。[16]基于曼哈顿假设解耦旋转和平移估计，并结合点、线和表面特征来提高平移的准确性。[17]的系统也是基于曼哈顿世界假设，但它可以支持无缝切换到非曼哈顿世界。这些世界假设算法可以在特殊场景下很好地工作。但是，在复杂的环境下可能容易关联出错。

还有一些工作专注于提高平面束调整（PBA）的速度。[18]、[19]、[20] 提出将与同一状态相关的多个测量值合并到一个约束中，提高了 PBA 的效率。受这些工作的启发，我们不仅合并了点到平面约束并将它们扩展到单应性约束。[21]强制与平面相关的点落在平面上，并使用平面和相应的锚点位姿来表示该点。这种表示形式消除了点状态量，降低了Hessian矩阵的维数，也会提高BA求解速度。目前，并没有一个高效高精度的完整紧耦合RGBD惯性平面SLAM系统。

方法

图1 系统框架

1、系统概述

系统的框架如图1所示。系统以RGB、Depth和IMU作为输入，主要包括前端、平面模块和后端三个主要组件。

前端模块是一个基于滑动窗口的VIO系统，实时估计6-DoF位姿。我们的VIO类似于[22]，只是我们不考虑SLAM特征并添加额外的深度观测。

平面模块接收前后端数据作为输入。高频前端信息仅用于平面扩展。低频但高精度的后端信息被用于新平面检测、平面扩展、点到平面关联和平面到平面合并。由于室内环境大多数平面是水平或垂直的，我们只考虑水平和垂直平面的检测。关于平面的融合优化适用于一般平面。

后端模块使用局部集束调整（LBA）或全局集束调整（GBA）来联合优化平面、点、IMU状态和关键帧位姿，并修正前端位姿的漂移。

2、快速的平面检测

只使用后端数据进行低频的平面检测。一旦检测到平面，平面模块将使用前端和后端数据快速扩展平面，并将平面与已有平面相关联。我们使用均匀网络创建的3D Mesh和直方图来快速检测平面。并通过检查法线是垂直还是平行于重力筛选出可靠的水平和垂直面。当从直方图中检测到平面时，使用直方图中的数据和3D平面点来重新优化平面参数。对于水平面，n = [0, 0, 1]，平面距离是平面点z轴的平均值。对于垂直面，我们设置n=[nx, ny, 0]，垂直面参数可以用下式细化：

我们使用QR分解来求解上面方程。

图2 平面检测：(a) 2D Mesh (b) 3D Mesh (c) 水平和垂直面

3、压缩的单应约束

当3D地图点与平面关联时，我们强制该点必须落在平面上。因此，我们不使用常见的点到平面距离约束，而是使用单应矩阵来约束两个关键帧和一个平面。如果平面上的点被第i帧和第j帧观察到，我们可以写出以下点到平面和重投影方程：

整合上面方程可以得到：

所以当点在平面上时，单应性约束和重投影约束是等价的。值得注意的是，单应性不需要点特征的3D位置。在BA问题中，我们将重投影约束转化为单应性约束，相当于去除平面上3D点的状态变量。最后，形成更小更稀疏的Hessian矩阵，集束调整的效率将大大提高。Homography将两个关键帧和一个平面的状态关联起来。这三种状态可能有非常多共同的观测。我们将这些观测合并为一个观测，等价的降低约束数量，以进一步提高优化速度。

Homography约束为：

消去尺度s，可以得到：

N个在平面上的点建立的Homography代价函数如下：

其中：

使用特征值分解。由于L只与观测相关，H与优化状态量相关，所以这里我们已经将多个代价函数合并成单个代价函数。点到平面的约束也使用类似合并方法，更多细节可以在论文中找到。

4、后端优化

后端优化主要由局部的平面和点集束调整，全局平面和点集束调整，Pose-Plane Graph和回环检测组成。全局约束包括IMU预积分约束，普通点重投影和深度约束，压缩的Homography约束以及压缩的点-面约束，公式如下：

实验

我们将提出的方法和下面方法进行对比：(a) ORB-SLAM2 [5]（由于IMU只支持单目，固选用RGBD版本）, b) Manhattan-SLAM [17] , c) Kimera[12] 。消融实验包括VI- SLAM （不带平面约束的视觉惯性SLAM），VI-SLAM-P （在VI-SLAM基础上增加平面约束，但是并没有使用压缩约束措施），VI-SLAM-CP（在VI-SLAM-P基础上使用压缩约束措施）， VIP-SLAM（我们的完整算法，在VI-SLAM-CP基础上，去除平面上的地图点）。

精度实验

VIP-SLAM和VI-SLAM-P都达到了最好的精度，这表明压缩多个观测并用单应性约束代替平面上点的重投影约束是有效的。由于闭环检测可以有效消除了SLAM的累积误差，导致VIP-SLAM和VI-SLAM的精度差异很小。在关闭回环模块的情况下，VIP-SLAM 和 VI-SLAM-P 在所有数据集上都取得了明显的优势，这有效地证明了平面约束对于缓解 SLAM 的累积误差非常有帮助，在没有回环辅助下，能提供实时的高精度跟踪。