摘要

本文中，我们提出了一个新的基于平面和IMU的稠密SLAM方法(DPI-SLAM)，IMU的预积分与稠密VO进行松耦合，与平面的观测进行紧耦合。位姿，速度以及IMU偏置与平面特征一起进行优化。使用的是iSAM2的优化方法。结合基于RGBD和IMU的里程计估计能够在没有充足平面的场景下进行位姿跟踪。将IMU的状态和平面的状态在全局优化中的建模能够降低漂移误差，提升建图性能。而且，相邻平面的结构约束也被加入到DPI-SLAM中，进一步降低了漂移误差。

引言

目前的基于平面的方法仍然存在的问题是：1. 仍然存在较大的累积误差，2. 在没有足够的几何特征或者纹理特征的场景下仍然失效。作者认为，将里程计与IMU相结合，能够解决这些问题。本文的贡献主要有：

1. 提出了一种新的机遇IMU预积分的稠密SLAM方法
2. 基于iSAM2进行优化，达到了CPU上的实时性能
3. 将平面特征之间的结构化约束加入到SLAM优化框架中

系统框架

我们的DPI-SLAM系统包括是三个主要部分：(a) 里程计估计和一个frame label过程；(b) 执行局部深度融合；© 将全局平面-惯性建图和结构化约束融合，并闭环。

在第一部分中，每个RGBD帧的位姿被根据IMU预积分信息进行预测，然后再用快速的RGBD里程计进行优化估计。位姿预测和估计都是相对于最近的参考帧的$R_j$Rj​。只有所选择的关键帧，参考帧以及融合帧被使用全状态的快速稠密RGBD里程计方法进行估计。对于其他帧，我们只大概估计其位姿，这样做可以提高效率。在每个参考帧，当前的预积分IMU信息和VO信息加入到全局的因子图中。如果一个新的帧的位姿距离参考帧较远，其会被设置为下一个参考帧。对关键帧执行同样的操作。
在第二部分，我们将所选择出来的两个关键帧之间的融合帧的深度信息进行融合，产生一个局部的深度地图。由于参考帧的位姿已经被IMU预积分信息校正，产生的深度地图具有较高的精度。
在第三部分，我们从局部地图中提取平面以及对应的点云类，使用投影方法将它们与已经存在的环境平面特征进行关联，将平面因子加入到全局因子图当中，并且与现有的VO因子以及惯性因子共同优化。然后，全局因子图被根据新的找到的结构化约束再次更新。最红，进行闭环检测与校正。
【关于快速的RGBD里程计，局部深度融合，平面关联，参考作者之前的文章KDP-SLAM[12]】

Planar-Inertial SLAM

A. IMU 预积分
详述见文献[9]。与其他方法不同的是，本文方法是使用的两个连续的参考帧之间的IMU预积分，而不是关键帧，因为关键帧相距较远。

B. Fast Dense RGB-D Odometry with IMU Preintergration
在传统的rgbd里程计中，当前帧的位姿初始估计设置为上一帧的位姿，其基于的是小范围运动的假设。但是，由于我们融合了IMU传感器，我们可以使用预积分预测每帧相对于最近的参考帧的位姿，这会产生较好的里程计估计，特别是在快速运动以及缺少纹理的情况下。

对每个参考帧的RGBD里程计和IMU观测的同步优化可以产生更加鲁棒精确的IMU状态估计，也能对IMU的偏置进行校正。需要注意的是，这里的IMU的观测与相机VO的观测存在一个变换，即相机到IMU的相对变换。
这里其实是和VINS-Mono类似，都是将重投影误差和IMU预积分进行联合优化。

C. Global Planar Mapping with IMU Preintegration
在每个关键帧，平面被从局部融合的深度地图中分割与提取，使用的是[11,7]的方法，使用[12]的方法进行平面特征的关联。每个平面被建模为一个单位长度的齐次向量$\pi=[\mathbf{n}^T,d]^T\in \mathbb{P}^3,\Vert\pi\Vert=1$π=[nT,d]T∈P3,∥π∥=1。这个平面的表示对应一个最小表示，四元数对应的李代数。详见上一篇博客的平面参数化表示。

平面的观测会被作为一个对应的平面特征和IMU状态之间的一个引子加入到全局图模型中，能够产生更强的约束。
这里也使用了相关观测，即将每个平面的原始坐标系定义为第一次观测到该平面的帧中。可以产生更快的收敛速度。

结构化约束

到目前为止，前面的位姿估计以及全局因子图的构造都是按部就班的，唯一不同的就是使用了面特征。这一部分才是真正有效地利用面特征的信息。
正交和平行是室内场景中两个最常见的平面间的结构化约束。可以用来加入到SLAM系统中进一步校正在旋转上的漂移。（这里确实是只能校正在旋转上的漂移）
对于两个相邻的平面$\pi_a,\pi_b$πa​,πb​，我们首先计算其$h_{ab}=|(R_a\mathbf{n}_a)R_b\mathbf{n}_b|$hab​=∣(Ra​na​)Rb​nb​∣，即他们在全局坐标系下的法线的点积，如果$h_ab$ha​b大于平行关系的阈值，则二者平行，如果小于垂直关系的阈值，则二者垂直。或者二者没有特殊的关系。
每个结构化约束因子不仅与相关的平面特征相连接，同时与他们的基坐标也相连。这会导致优化过程中更快的收敛速度。对于任意一对平面$\pi_a,\pi_b$πa​,πb​，对于正交约束的因子，其误差函数为
$e_{\perp}=\frac{1}{\sigma_{\perp}^2}[(R_a\mathbf{n}_a)^TR_b\mathbf{n}_b]^2$e⊥​=σ⊥2​1​[(Ra​na​)TRb​nb​]2
对于平行平面的约束，其对应的误差项为
$e_{\parallel}=\frac{1}{\sigma_{\parallel}^2}\Vert [R_a\mathbf{n}_a]_{\times}R_b\mathbf{n}_b \Vert^2$e∥​=σ∥2​1​∥[Ra​na​]×​Rb​nb​∥2
其中的方差$\sigma_{perp},\sigma_{\parallel}$σperp​,σ∥​被设置为一个很小的数$(3\times 10^{-5})$(3×10−5)用于产生很强的约束。
其实，最关键的启发信息，只是用在了这一节中。注意到这里只是用的两个平面间的。

实验设计和实验

接下来就是实验部分，其中提到的一个有意义的部分就是，如果不采用IMU信息，位姿估计就会产生较大的偏差，就无法判断平面间的约束关系，无法设置合理的阈值。

总的来说，本文是一个基于平面VI-SLAM系统，具有较为完善的前段和后端框架，适合室内环境。并没有在公用数据集上的进行验证对比。