具有结构规律的RGB-D SLAM（ICRA 2021）

点击上方“3D视觉工坊”，选择“星标”

干货第一时间送达

作者丨泡泡图灵智库

来源丨泡泡机器人SLAM

RGB-D SLAM with Structural Regularities

下载链接：https://arxiv.org/abs/2010.07997

代码：https://github.com/yanyan-li/PlanarSLAM

作者：Yanyan Li, Raza Y unus, Nikolas Brasch, Nassir Navab, and Federico Tombari

来源：ICRA，2021

机构：Technical University of Munich, Germany

编辑：陈嘉皓

审核：欧阳展鹏

摘要

这项工作提出了一个专门为结构化环境设计的RGB-D SLAM系统，旨在通过依赖于从周围提取的几何特征来提高跟踪和测绘的精度。除了特征点，结构化环境还提供了大量的几何特征，如直线和平面，我们利用这些特征来设计我们的SLAM系统的跟踪和建图组件。对于跟踪部分，我们基于曼哈顿世界(MW)的假设探索了这些特征之间的几何关系，并提出了一种基于点、线、面的解耦-调整方法，并在额外的姿态调整模块中使用曼哈顿关系。在地图部分，我们以较低的计算代价重建由稀疏到稠密的不同层次的地图。提出了一种基于实例的网格划分策略，通过独立划分平面实例来构建稠密的地图。最后，我们对姿态估计和重建方面的整体性能在公共基准上进行了评估，并表明我们的方法与最先进的方法相比，性能有所提升。

主要工作与贡献

本文将平面特征合并到基于Manhattan的框架中，用于估计初始平移向量，并在调整模块中保留Manhattan关系作为约束。在此基础上，还提出了一种基于稀疏地图中获得的平面区域重构场景结构的高效网格划分模块。其主要的贡献如下：

• 在基于MW的解耦姿态估计基础上，通过将点、线特征与平面相结合和附加的曼哈顿关系的姿态细化步骤来改进平移估计。

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

• 实时RGB-D SLAM的通用框架，这些组件用于结构化环境下的高精度定位和地图构建。

算法流程

1. 总览

给定一个结构化环境中的RGB-D帧序列，我们的方法的目标是重建3D场景，同时估计6D摄像机在每一帧的姿态。

A 特征集扩展

a) 点和线：在帧中提取ORB特征点，和线段特征LSD （采用描述子为LBD）后，利用相机固有参数和深度图对点和线进行反投影，得到三维点和三维直线。深度映射并不总是正确的，特别是在深度不连续处，例如对象边界。因此，需要一种鲁棒的三维直线拟合方法。首先，我们计算被检测线段与非零深度值相交的像素的数量。如果数量超过一定阈值，则通过RANSAC估计三维直线，去除潜在的离群值。

b) 法线和平面：平滑法线是通过使用积分图像对10×10像素内的深度图像的切向向量进行平均计算的。在平面检测后，我们使用 [6: Point-plane SLAM using supposed planes for indoor environments] 策略将观测到的平面与地图中存在的平面进行关联。为了将观察到的平面与地图上的平面相匹配，我们首先检查它们法线之间的角度。如果低于阈值θ，则检查它们之间的点到平面的距离。与观测平面的距离最小，且位于距离阈值θ以下的平面与观测平面匹配。在实验中，θθ分别设为10°和0.1m。此外，我们还保持地图平面之间的平行和垂直关系，以利用跟踪过程中的额外约束。它们是由平面法线之间的夹角决定的。因为它们只提供方向约束，所以我们不考虑它们的距离。

B 解耦姿态估计和改进

为了减少帧间的误差传播，我们首先建立了基于MW假设的单目体系结构用来计算旋转运动，然后固定从上一步算出的旋转，并根据特征估计相应的平移运动。在这项工作之后，我们采用线来优化旋转估计，采用平面来优化平移估计。

不同于Structure-SLAM使用局部地图中的点线特征来联合优化平移和旋转，本文还利用了平面在局部地图中的几何关系（平行或者垂直）作为约束，较大程度上提高了系统的准确性。

2. 追踪

与传统姿态估计方法不同，我们将6D摄像机姿态解耦为旋转和平移。在MW假设的基础上，我们得到了MW和摄像机之间的旋转运动。这样旋转估计就不受最后一帧或最后一关键帧姿态的影响，有效地减少了漂移。然后，使用点、线、面特征以及初始旋转矩阵进行平移估计，只有3个自由度(dofs)。

A 旋转估计

无漂移旋转估计方法不是直接从帧到帧跟踪摄像机，而是通过将室内环境建模为MW来估计每帧和曼哈顿坐标帧之间的旋转，从而减少帧到帧跟踪产生的漂移。曼哈顿坐标帧可以通过对齐到相机的起始帧。一般将第一坐标系的坐标视为世界坐标系，即。所以我们可以通过使用：

这里表示世界到MW的关系，由MW初始化步骤得到，是MW到第(k +1)个坐标系的关系。这两个矩阵通过 sphere mean-shift算法计算，其中法线和归一化消失方向被投影到当前旋转估计的切平面上。然后在切平面上执行mean-shift 步骤，生成新的中心，并将它们反向投影到球面上作为新的估计。关于球面均值移位方法的更多细节，请参阅[24 :Divide and Conquer: Efficient Density-based Tracking of 3D Sensors in Manhattan Worlds]和[26: Visual Odometry with Drift-free Rotation Estimation Using Indoor Scene Regularities]。为了处理只有一个平面或没有平面被检测到的困难场景，我们为单位球体提供了细化的3D线的消失方向和平面的表面法线，这是一种更稳健的方法。

B 平移估计

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

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

然后，我们根据与关键帧中匹配的3D线投影的3D端点之间的点线距离推导出误差函数。对于每个端点，误差函数记为：

为了得到最优化的平面的最小参数化，我们将其表示为，其中和是法线的方位角和仰角：

因此，帧间的观测平面与对应的地图上平面误差函数为：

其中为世界坐标到摄像机坐标的转换。假设观测服从高斯分布，最终的非线性最小二乘代价函数t∗可以写成：

其中π分别是点、线、面的逆协方差矩阵，ρρ和ρπ分别是鲁棒的Huber代价函数。这里，我们采用用LevenbergMarquardt算法确定一个解。

C 位姿调整

最后两个步骤假设场景是一个很好的曼哈顿模型，然而一些一般的室内环境并不严格遵守MW假设，导致精度下降。因此，在通过解耦旋转和平移策略获得初始姿态后，调整模块将对姿态进行微调，以补偿偏离MW或不稳定初始估计的偏差。在细化步骤中，为了减少帧间姿态估计的漂移，使用之前关键帧构造的局部地图基于从地图到帧的策略来优化姿态。我们使用关键帧来构建局部地图，地图中的点、线和面路标将被投影到当前帧中以搜索匹配。此外，我们还探讨了局部地图中的平面与当前帧中检测到的平面之间的关系。这些平面之间的平行约束和垂直约束描述为：

其中π，是世界坐标到摄像机坐标的变换。对于垂直平面，其平面法线被旋转90度(R⊥)来构造误差函数。将这两个误差函数合并，在调整模块中建立联合优化函数。

3. 建图

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

A 稀疏地图

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

B 平面实例级网格划分

上一节中获得的稀疏图对于涉及机器人-环境交互的应用程序仍然不够，但它提供了关于平面和非平面实例的信息。因此，我们使用实例划分策略构造一个更密集的映射。室内场景可分为平面和非平面区域。平面区域，如地板、墙壁和天花板通常有很大的范围，但是高密度的像素信息并不能增加质量，而且是高度冗余的。因此，我们不使用surfel 或TSDF，而是将平面区域视为包含少量且固定数量的元素的实例，这些元素独立于它们的大小。特别地，我们将平面实例输入到网格划分模块，该模块独立地对它们进行网格划分。首先，属于一个平面的点被组织成一个kd-tree数据结构。不同于非结构化输入，我们的方法需要更少的时间搜索几个最近邻。然后，我们使用贪婪曲面三角剖分(GST)建立一个实例级网格，用于处理平面曲面。需要注意的是，在我们的实验中，选择三角剖分邻居的初始搜索半径设置为5m，乘子设置为5，以修改最终搜索半径，以适应平面区域上不同的点密度。

4. 实验

我们提出的SLAM系统以公共数据集的ICL-NUIM和TUM RGBD基准，并比较与其他先进的方法的性能，比较的系统包括：基于特征的ORB-SLAM2，PSSLAM等（此时全局BA被移除），基于MW假设的LPVO和L-SLAM，直接法DVO-SLAM，使用GPU实现基于RGB和深度图像的实时SLAM的InfiniTAM。此外，我们还提供了我们在ICL-NUIM数据集上重建模型的重建精度，并与其他常用的密集重建方法进行了比较。最后，为了证明我们的系统随着时间的推移具有鲁棒性，我们还对一个来自TAMU数据集的序列进行了测试。所有实验都是在Intel酷睿i7-8700 CPU (@3.20GHz)下进行的，不使用GPU。

A ICL-NUIM RGB-D数据集

B TUM RGB-D 数据集

C TAMU数据集（大规模室内序列）

D 重建精度

点击阅读原文， 即可获取本文下载链接。

本文仅做学术分享，如有侵权，请联系删文。

3D视觉精品课程推荐：

1.面向自动驾驶领域的多传感器数据融合技术

2.面向自动驾驶领域的3D点云目标检测全栈学习路线！(单模态+多模态/数据+代码)
3.彻底搞透视觉三维重建：原理剖析、代码讲解、及优化改进
4.国内首个面向工业级实战的点云处理课程
5.激光-视觉-IMU-GPS融合SLAM算法梳理和代码讲解
6.彻底搞懂视觉-惯性SLAM：基于VINS-Fusion正式开课啦
7.彻底搞懂基于LOAM框架的3D激光SLAM: 源码剖析到算法优化
8.彻底剖析室内、室外激光SLAM关键算法原理、代码和实战(cartographer+LOAM +LIO-SAM)

重磅！3DCVer-学术论文写作投稿 交流群已成立

扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群，旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。

同时也可申请加入我们的细分方向交流群，目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计等微信群。

一定要备注：研究方向+学校/公司+昵称，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。原创投稿也请联系。

▲长按加微信群或投稿

▲长按关注公众号

3D视觉从入门到精通知识星球：针对3D视觉领域的视频课程（三维重建系列三维点云系列结构光系列、手眼标定、相机标定、激光/视觉SLAM、自动驾驶等）、知识点汇总、入门进阶学习路线、最新paper分享、疑问解答五个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近4000星球成员为创造更好的AI世界共同进步，知识星球入口：

学习3D视觉核心技术，扫描查看介绍，3天内无条件退款

 圈里有高质量教程资料、答疑解惑、助你高效解决问题

觉得有用，麻烦给个赞和在看~