RGB-D SLAM with Structural Regularities

本工作提出了一种针对结构化环境设计的RGB-D SLAM系统，旨在通过利用从周围环境中提取的几何特征来改善跟踪和建图的精度。结构化环境除了提供点之外，还提供了丰富的几何特征，如线和面，我们利用这些特征来设计SLAM系统的跟踪和映射组件。对于跟踪部分，我们基于曼哈顿世界(Manhattan World, MW)的假设探索这些特征之间的几何关系。我们提出了一种基于点、线和面的解耦优化方法，以及在附加的姿态优化模块中使用曼哈顿关系。在映射部分，以较低的计算代价重构了从稀疏到密集的不同层次的映射。提出了一种基于实例的网格划分策略，通过单独划分平面实例来构建密集地图。姿态估计和重建方面的整体性能在公共基准上进行了评估，与最先进的方法相比，表现出了更好的性能。

INTRODUCTION

视觉同步定位与映射(SLAM)算法用于在重建周围未知环境的同时估计6D摄像机的姿态。它们已被证明在广泛的应用中非常有用，例如自动驾驶机器人、自动驾驶汽车和增强/虚拟现实，在这些应用中，相机姿态估计使汽车、机器人和移动设备能够定位自己，而密集的地图提供了环境的表示，例如机器人-环境或人-环境交互。

许多SLAM应用程序必须处理结构化场景，即通常以低纹理表面为特征的人造环境——一个典型的例子是室内场景，或室外停车场。这导致了视觉特征的缺乏，视觉SLAM系统通常利用这些特征来改进相机姿态估计和/或3D重建，例如通过执行闭环和束调整来减少漂移。针对结构化场景，提出了具体的基于点和线段的SLAM方法，如S-SLAM[1]、Stereo-PLSLAM[2]、PLVO[3]、single - plslam[4]和probability - vo[5]，将工作环境扩展到可检测到线多于点的场景。SPSLAM[6]将平面特征合并到ORB-SLAM2[7]中，在低纹理场景中实现了稳健的结果。

重建方法有稀疏法、半稠密法和稠密法。与前两类只提供不完整地图相比，需要密集重建，为机器人-环境交互和3D场景理解等应用提供足够的信息。通过RGB-D传感器重建室内场景已经提出了许多算法。KinectFusion[8]是一个基于截断符号距离场(truncated signed distance field，TSDF)的地图表示的开创性工作。为了重建大规模场景，提出了基于surf的方法，如ElasticFusion[9]。Wang等人[10]从RGB图像和深度图中提取超像素，而不是重建每个像素，这是更高效的，但仍然有冗余信息，特别是在室内场景中，可以经常发现大平面。

truncated signed distance field（TSDF）是一种用于三维重建和几何建模的技术，通过对场景中所捕获的深度图像序列进行逐帧积累和综合，生成用于表示场景几何形状和表面法线的密集3d体素数据结构。每个3D体素中存储其到场景表面最近点的有符号距离（signed distance），并且在该体素的边缘上还可能存储一些其他信息，例如颜色或者灰度值等。通过对多个深度图像进行融合，可以不断更新这些距离值和其他信息，从而不断优化体素数据结构的表示精度，并最终得到比单张深度图更加精确和完整的三维场景模型。

在本文中，我们在单目Structure-SLAM[1]的基础上，提出了一个鲁棒的的RGB-D SLAM系统，专门用于处理结构化环境，同时改善了跟踪和映射。图1展示了这种结构化场景的组成部分，包括点、线和面段。根据Structure-SLAM的解耦策略，我们先估计一个无漂移旋转矩阵，然后再估计三自由度平移。最初的旋转和平移是通过映射到帧的策略来细化的。与[1]、[11]、[12]不同的是，平面特征被合并到我们的基于Manhattan的框架中，该框架用于估计初始平移向量，并在优化模块中保留Manhattan关系作为约束。在此基础上，提出了一种高效的网格划分模块，利用稀疏图中得到的平面区域重构场景结构。总之，我们的贡献是：

Structure-SLAM是一类用于同时地图和相机自我定位的算法，其中解耦策略是指将结构和运动两个问题分开解决的策略。具体来说，Structure-SLAM将场景中的三维结构建模为点云或三角网格等格式，并在此基础上估计相机在场景中的位置和姿态。这样可以将SLAM问题中的运动部分与结构部分解耦，并且可以让其他模块（例如回环检测、优化等）更加高效地工作。在估计相机位置时，Structure-SLAM通常使用任意视角重建或稠密重建等技术进行初始化，然后应用基于特征匹配的方法（例如orb-slam、svo等）进行增量式更新。而在建立三维结构方面，Structure-SLAM则通常应用基于激光雷达、rgb-d相机或者双目视觉的点云生成方法来构建场景模型。总之，Structure-SLAM通过解耦和分离的思想，将相对复杂的单个问题分解为几个简单的子任务，从而提高SLAM系统的鲁棒性、精度和效率。

1、基于基于MW的解耦姿态估计的概念，我们通过将点和线特征与平面结合，并使用Manhattan关系进行额外的姿态优化步骤来改进平移估计。

2、我们提出了一种基于平面实例网格的重建方法，从稀疏点云生成环境的紧凑表示。

3、实时RGB-D SLAM的通用框架，其中这些组件用于在结构化环境下进行高精度的定位和映射。

RELATED WORK

下面我们回顾了基于RGBD的SLAM系统的相关文献，以及利用结构规律性作为MW假设的方法。

a) RGB-D SLAM：在[13]，[14]中，建议尽可能使用平面而不是点特征，因为对多个深度测量值求平均可以显著降低噪声。在Dense Planar SLAM[15]中，将平面拟合到曲面上并将曲面反向投影到平面上来进行平滑。Le[16]依赖于由地面平面和几面墙壁组成的场景布局，并使用动态规划来推断像素与平面之间的连续一致分配。在Probabilistic-VO[5]中，点、线和面的不确定性被明确地建模并用于姿态估计，其中点、线和面在[17]中以统一的框架表示。在DVO-SLAM[18]中提出了一种结合光测量和几何的直接SLAM系统，并在CPA-SLAM [19]中以全局平面进行扩展，其中深度测量被分配到具有权重的全局平面。

b)密集重建：虽然上述方法的目标是估计精确的姿态，因此仅保留具有最可靠信息的地图，但已经提出了一些工作，其目标是创建一个完整的环境密集重建。KinetFusion[8]和ElasticFusion[9]探索了RGB-D传感器的密集重建。第一种方法将所有深度数据融合成体积密度表示，用于使用ICP跟踪相机姿态。由于内存限制，映射（map）的大小通常在体积方法中受到限制。与KinectFusion不同，ElasticFusion是一个以地图为中心的系统，可以重建基于surfel的环境地图。为了减少地图中的surf数量，[10]提出了基于超像素的surf，与ElasticFusion相比，它减少了surf的数量。最近，BAD-SLAM[20]针对RGB-D SLAM提出了一种直接的束调整方法。在[21]中，纹理网格是从密集的surfel云中提取出来的。在[22]中提出了一种直接基于网格的RGB-D传感器重构方法。

c)结构规律性：一系列作品利用了世界上附加的约束和规律，提高了重构性能。在[23]和[24]中，旋转估计误差是造成长期漂移的主要原因。

在[25]中提出了用于曼哈顿框架估计的分支定界框架。在MVO[24]中，利用单位球上的平均位移来寻找MW与当前帧之间的变换。当像OPVO[26]那样只使用平面进行旋转估计时，每帧必须检测到至少2个正交平面，可以选择从直线中提取的消失点相加，就像在LPVO[11]中所做的那样。上述方法使用点特征来估计翻译。StructureSLAM[1]是一个单目系统，通过卷积神经网络预测法线，在解耦策略中利用具有点和线的法线。由于预测的法线不如从深度图中计算的法线准确，系统提供了基于点和线的细化/回退模块。与Structure-SLAM相比，本文采用优化后的直线和平面特征消失点进行旋转和平移。然后，去除回退部分，将平面几何关系纳入细化部分。相对于稀疏的点线图，密集的网格作为输出在机器人应用中更有用。L-SLAM[12]也是基于MW假设，从LPVO中获得潜在平面区域的平移、旋转和像素。然后利用线性卡尔曼滤波器对三维平动偏移量和一维平面偏移量进行细化。但是，我们使用一个更鲁棒的前端来进行初始转换估计。此外，6D姿态优化步骤用于同时优化旋转和平移，并允许从MW的初始旋转偏移，与L-SLAM和LPVO相比，这对非MW(曲面和少数平面区域)更具鲁棒性(见图6)。

PROPOSED FRAMEWORK

给定一组来自结构化环境的RGB-D帧，我们方法的目标是重建3D场景，同时估计每一帧的6D相机姿势。第四节概述了所提出的跟踪管道，它解耦了旋转和平移，而第五节描述了由系统生成的不同类型的映射表示。我们现在描述系统的基本特性和结构组件。

A、 Extended feature set

在我们的方法中，我们使用了快速提取和匹配的ORB特征[27]。在低纹理环境中，很难提取足够的点来进行鲁棒姿态估计，因此我们用线扩展特征集，这些线特征使用LSD[28]方法提取，如LBD[29]所述。此外，在室内环境中经常发现无纹理的平面区域，其中从深度图中提取的平面实例是扩展点和线的有价值的线索。平面检测使用连通分量分析方法（connected component analysis method）[30]。它们由Hessian范式π=(n, d)表示，其中n=(nx, ny, nz)是平面的法线，表示其方向，d是相机原点到平面的距离。

a)Points and line
在提取fi帧中的二维点特征xj =（uj，vj）和线段lj =（xj，start，xj，end）之后，我们可以使用相机内部参数和深度图回投影点和线，以获得三维点xj和三维线lj。深度图并不总是准确的，尤其是在深度不连续处，例如对象边界。因此需要一种用于3d线的鲁棒拟合方法。首先，我们计算检测到的线段与非零深度值相交的像素数。如果该数量超过某个阈值，则通过ransac估计3d线lj以消除潜在的异常值。

b) Normals and planes

平滑法线是利用积分图像对10*10像素的patch内深度图像的切向向量求平均值。在平面检测后，我们使用[6]策略将观测到的平面与地图中存在的平面相关联。为了将观测到的平面与地图上的平面匹配，我们首先检查它们法线之间的夹角。如果它低于阈值θn，我们检查它们之间的点到面距离。与被观测平面的距离最小，同时也位于距离阈值θP以下的平面与被观测平面相匹配。在实验中，θn和θP分别设置为10度和0.1 m。此外，我们还在地图平面之间保持平行和垂直关系[6]，以在跟踪过程中利用额外的约束。这是由平面法线之间的夹角决定的。由于它们只提供方向的约束，我们不考虑它们的距离。

B、Decoupling pose estimation and refinement.

为了减少帧间误差传播，我们在基于mw假设计算旋转运动的单目架构[1]的基础上构建。然后，与上一步计算得到的固定旋转相对应的平移运动由特征估计得出。在本工作的前端，我们使用优化的线来进行旋转估计，并使用平面进行平移估计。与structure-slam [1]使用点-线局部映射一起优化平移和旋转不同的是，我们利用局部映射中的平面，并利用这些平面之间的几何关系（平行和垂直）作为约束，这提高了系统的精度，如图4和表I所示。

TRACKING

与传统的姿态估计方法不同，我们将6D相机姿态解耦为旋转和平移。基于MW假设，我们获得了MW和相机ci之间的旋转运动Rcim。这样，旋转估计不受最后一帧或最后一关键帧姿态的影响，有效地减小了漂移。然后，点、线和面特征以及初始旋转矩阵用于平移估计，其中该平移估计仅包含3个自由度(dof)。

A、Rotation estimation

无漂移旋转估计方法不是直接从帧到帧跟踪摄像机，而是通过将室内环境建模为MW来估计每帧与Manhattan坐标帧之间的旋转Rcm，从而减少帧到帧跟踪产生的漂移。如图1所示，可以通过Rk+1,m将曼哈顿坐标帧对齐到相机的起始帧。一般将第一帧的坐标被视为世界坐标系，即R1,m = RT m,w。因此，我们可以通过使用，

其中Rm,w表示世界到MW的关系，由MW初始化步骤得到，Rk+1,m是MW到第(k +1)帧的关系。这两个矩阵是通过球面均值位移方法[24]计算的，其中法线和归一化消失方向被投影到当前旋转估计的切平面上。然后在切平面上执行均值位移步骤，生成新的中心并将其反向投影到球面作为新的估计。我们建议读者参考[24]和[26]，以获得球体均值位移方法的更多细节。为了处理只检测到一个或根本没有平面的困难场景，我们用细化的3D线的消失方向和平面的表面法线来来提供单位球体数据。这是在这些具有挑战性的条件下比[26]，[1]更鲁棒的方法。

B、Translation estimation

在估计旋转后，用点、线和面来估计平移。我们将上一帧的3D点重投影到当前帧中，并根据重投影误差定义误差函数，如下所示：

其中Π(·)为投影函数。由于上一步已经得到了旋转矩阵Rk,j，我们对旋转进行了固定，只使用(2)对应的雅可比矩阵估计平移。

对于直线，我们从二维端点pstart和pend得到归一化的直线函数，如下所示：

然后，我们根据关键帧中匹配的3D线l与投影的3D端点Pstart和Pend之间的点到线的距离[4]来制定误差函数。对于每个端点Px，误差函数记为：

为了得到一个平面π的最小参数化以进行优化，我们将其表示为q(π)=(φ，ψ，d)，其中φ和ψ是法线的方位角和仰角，d是到Hessian形式的距离

因此，坐标系中观测平面πk与对应映射平面πx之间的误差函数为：

其中T−T cw是从世界坐标到相机坐标的变换。假设观测值服从高斯分布，最终的非线性最小二乘代价函数t *可以写成(7)式，其中Λpk,j， Λpk,Px和Λk，πx是点、线和面的逆协方差矩阵，ρp， ρl和ρπ分别是鲁棒Huber代价函数。

这里，使用LevenbergMarquardt算法确定解决方案。

C、Pose refinement

最后两个步骤假设场景是一个很好的Manhattan模型，然而一些一般的室内环境并没有严格遵循MW假设，导致精度下降。因此，在通过解耦旋转和平移策略获得初始姿态后，优化模块[1]对姿态进行微调，以补偿与MW偏差或不稳定的初始估计。在细化步骤中，为了减少帧间姿态估计的漂移，利用之前关键帧构造的局部映射，基于映射帧策略[7]对姿态进行优化。

与[6]，[7]，[31]类似，我们也使用关键帧来构建局部地图，尽管我们的地图有点、线和面地标，它们被投影到当前帧中以搜索匹配。此外，我们还探讨了局部地图中的平面与当前帧中检测到的平面之间的关系。这些平面之间的平行约束和垂直约束描述为(8)，

其中qn(π)=(φ，ψ)， Rcw是从世界坐标到相机坐标的转换。对于垂直平面，将其平面法线旋转90度(R⊥)构成误差函数。将这两个误差函数合并到(7)中，在优化模块中构建一个联合优化函数。

MAPPING

本节描述在我们的SLAM框架中使用的基于关键帧的3D映射策略。关键帧和3D特征构建了一个共视图，每当有新的关键帧和新特征可用时，节点和边都会更新。

A、Sparse Mapping

如图3所示，从关键帧中提取点-线-面特征，重构稀疏映射模块。第一帧被设置为第一关键帧，全局映射由由此检测到的地标初始化。当在新的关键帧中检测到不在全局地图中的新点、线和面时，它们将首先保存到局部地图中。然后我们检查局部地图中地标的质量，在剔除不好的地标后，将可靠的地标推送到全局地图中。与点和线的匹配方法不同，对于新关键帧中每个检测到的平面，我们首先使用第三节中描述的策略检查它是否与地图中的平面相关联。如果我们找到关联，我们将新平面的3D点添加到全局地图中的关联平面中，并使用体素网格过滤掉冗余，再次获得紧凑的点云。如果进入的平面没有关联到全局映射中的任何平面，我们将其作为一个新平面添加到映射中。

B、Planar instance-wise meshing

在前一节中获得的稀疏映射仍然不足以用于涉及机器人-环境交互的应用程序，但它提供了关于平面和非平面实例的信息。因此，我们使用基于实例的网格策略构建一个密度更大的地图。室内场景可分为平面区域和非平面区域。平面区域，如地板，墙壁和天花板通常有很大的范围，但密集的像素级信息不会增加质量，而且是高度冗余的。因此，我们不使用surfel或TSDF，而是将平面区域视为包括少量固定数量元素的实例，而与它们的大小无关。

特别地，我们将平面实例输入到网格划分模块，该模块将它们独立地进行网格划分。首先，将属于一个平面的点组织为kd树数据结构。与非结构化输入不同，我们的方法需要更少的时间来搜索几个最近的邻居。然后，我们使用贪心曲面三角化(GST)[33]建立一个实例级网格，该网格旨在处理平面曲面。请注意，在我们的实验中，三角化选择邻居的初始搜索半径设置为5m，乘数设置为5，修改最终搜索半径以适应平面区域上不同的点密度。

EXPERIMENTS

我们在两个众所周知的公共数据集上评估了所提出的SLAM系统，icl-num[34]和TUM RGBD[35]基准，将其性能与其他最先进的方法进行比较，如ORB-SLAM2 [7]， PSSLAM[6]，这些方法是基于特征的方法，但在接下来的实验中删除了全局BA模块。基于MW假设的方法如LPVO[11]和L-SLAM[12]。DVO-SLAM[18]是一种直接的方法，InfiniTAM[32]使用GPU进行基于RGB和深度图像的实时跟踪和映射。此外，我们提供了重建模型在ICL-NUIM数据集上的重建精度，并将其与其他流行的密集重建方法进行了比较。最后，为了证明我们的系统随着时间的推移是健壮的，我们还测试了来自TAMU[3]数据集的序列，其中包含覆盖大面积室内区域的长序列。所有实验均使用英特尔酷睿i7-8700 CPU (@3.20GHz)进行，不使用任何GPU。ICLNUIM数据集[34]提供了两个室内环境的合成场景，一个是客厅，一个是办公室场景。这些场景包含大面积的低纹理表面，如墙壁、天花板、地板等。每个场景有四个序列。我们在所有序列上评估我们的方法。

A、ICL-NUIM RGB-D Dataset

表I显示，基于平移RMSE (ATE)，我们的方法在八个序列中的三个序列上获得了最佳性能。InfiniTAM在lr-kt1, lr-kt2和of-kt3序列上也表现良好，但在其他序列上也会丢失跟踪。由于数据集包含较大的结构化区域，基于曼哈顿的LPVO和L-SLAM方法能够很好地估计方向，并在整个过程中提供良好的结果。然而，他们通常需要两个平面，或者一个平面和一个消失的方向，以便在任何时候都可以看到，以估计一个好的曼哈顿帧。如图6所示，lr-kt3中有几个具有挑战性的场景，当镜头靠近墙壁时，只捕捉到一堵白色的墙和植物的两片叶子。在这种情况下，OPVO和L-SLAM都不能获得很好的性能。当我们的系统由于场景不是刚性MW而获得了一个糟糕的初始姿态时，基于点-线-面特征的细化步骤允许我们恢复姿态，而L-SLAM忽略了LKF模块中的优化旋转。此外，DVO作为一种密集方法，可能会因为大面积的墙壁、地板等不包含足够的光度误差梯度而挣扎，ORB- slam2和PSSLAM表现良好，因为这两个环境都包含从家具、物体等中提取的足够的ORB特征。由于我们的方法利用了所有的几何元素，它能够在大多数序列中稳健地执行。另外，图4给出了ORB-SLAM、PS-SLAM和我们的方法的相对位姿误差。我们的方法在相对平动和旋转方面的效果明显优于其他两种方法。特别是由于采用了解耦的MW旋转估计方法，使得旋转误差大大降低。

B、TUM RGB-D Dataset

TUM RGB-D基准[35]是RGB-D SLAM系统最受欢迎的数据集之一，它提供了不同纹理和结构条件下的室内序列。这允许我们分别测试具有结构、纹理或两者兼有的序列。为了在具有挑战性的环境中评估我们的方法，我们选择了四个结构化图像序列，前三个具有低纹理，最后一个具有大规模环境。表I中列出的所有序列都有结构，但大柜子序列并不是一个严格的曼哈顿场景。基于manhattan的方法能够在snot-far序列上提供良好的位姿估计，但在大cabinet和cabinet序列上，结果退化。前两个序列包含由多个非纹理平面组成的相同环境。在这里，ORB-SLAM2不能沿着序列找到足够的点对应，从而失去跟踪。我们的方法增加了直线和平面的平移估计，获得了更好的结果。如图4所示，由于几个低纹理帧，cabinet和大cabinet是具有挑战性的序列。该方法的跟踪策略将相对旋转误差限制在2度以内，优于ORB-SLAM2和PS-SLAM。每个操作所花费的时间统计如表II所示，在表II中，我们使用不同的CPU线程来处理特征提取和细化模块中的点、线和面。

C、Large scale sequence

TAMU数据集[36]提供大规模室内序列(恒定照明)。虽然它不提供真实的相机姿态，但起点和终点是相同的，可以通过计算最终位置误差来评估总体漂移。如图5所示，楼梯c序列的轨迹是一个两层之间的环路，其中我们的方法在整个轨迹长度上的漂移比ORBSLAM2提高了34.7%。a走廊和入口大厅也有类似情况。在补充材料中提供了更多定性的结果。

D、Reconstruction Accuracy

我们从ICL-NUIM重建模型，并将结果与最先进的映射方法进行比较，如表III所示。重建结果的精度定义为预测模型与地面真实模型[34]之间的平均差值。我们将所提出的映射模块与RGB-D SLAM[37]、ElasticFusion[9]、InfiniTAM[38]和SuperpixelFusion[10]进行比较。

SuperpixelFusion方法受到使用ORB-SLAM进行姿态估计的限制，而我们的方法在低纹理环境中也能很好地工作。InfiniTAM在kt2中得到了最好的结果，但在kt0和kt3序列上表现较差，这是由于存在较大的低纹理区域。ElasticFusion显示了类似的行为。我们的方法重建的地图比其他方法更精确，但InfiniTAM和ElasticFusion提供的模型比我们的地图更完整，因为我们忽略了小物体，即使基于点、线和面的特征覆盖了大部分像素。值得注意的是，所有的融合方法，除了SuperpixelFusion和我们的，都依赖于基于GPU的加速。

CONCLUSIONS

提出了一种基于点、线、面的RGB-D SLAM系统。使用MW假设进行旋转估计，并使用点、线和面特征进行平移估计，我们实现了最先进的性能。此外，一种新的基于实例的网格划分方法可以有效地重建环境中的平面区域。由此产生的密集地图允许在机器人和AR/VR应用程序中与环境进行交互。在未来，我们希望通过对环境中的非平面部分进行网格划分来扩展平面重建，以实现更复杂场景的完整重建。