SLAM++ 物体级的同时定位与建图

翻译 语义SLAM

by jiayao ma 2017.3.21

作者：Renato F. Salas-Moreno1 Richard A. Newcombe2 Hauke Strasdat1 Paul H. J. Kelly1
Andrew J. Davison1
1Imperial College London 2University of Washington
作者单位：帝国理工大学，华盛顿大学

摘要：

我们提出一种新的面向物体的3D SLAM 范式，它的好处在于，当许多场景中有重复的、领域特定的物体和结构时，我们充分证明了能充分利用这些先验知识。
当手持深度相机扫过杂乱的场景时，实时的3D物体识别和跟踪提供了6DoF的相机-物体约束，这些约束作为显式的物体图（object graph），这个图接着通过高效地姿态图优化的方法进行调整。
这使得SLAM系统有可以进行描述和预测的力量，用稠密的表面重建来表示，但是表示起来极为浓缩。
物体图使得能够在每个实时帧中用精确的基于ICP的相机的方法进行跟踪，并且高效的主动地在当前没有描述的图片区域中找到新的物体。
我们在大且杂乱的场景中证明了这个实时的增长式的SLAM。其中包括，包括回环，重定位和移开物体，当然，物体级场景的描述的得到是有潜力去支持交互的。

1.简介：

目前很多实时的SLAM系统都在底层元素上操作（例如点、线、补丁、非参数的曲面表示例如深度图），为了地图能够表示纷杂的现实世界，它们必须被鲁棒地匹配，几何上转换，优化。现代硬件处理器，提供前所未有的水平的细节和,更多的研究兴趣转向几何的语义标签，尤其这些对象和区域在场景中已知存在。然而，关于这个过程的一些想法暴露出其需要大量耗费计算的问题，这些想法有潜力去找到一种更好的方法能够在在SLAM回环(in the loop of SLAM)的操作中利用已知（domain knowledge）。
我们提供了一种范式来进行实时定位和建图，跳出底层几何过程，利用3D物体识别在“面向物体”的层面直接进行递增式的建图。用手持深度相机扫过一个混乱的场景，物体的先验知识可以被重复的表示使得我们能够实时的进行3D识别，用物体的位置联系建立简单的姿态图。新的测量不断到达，此姿态图不断优化，能够始终更新。这些预测用来进行鲁棒地相机跟踪和为了更长远的物体检测进行的主动区域搜索（active search regions）。
我们的方法使用了实时3D物体检测的最新进展, 一种高效的GPGPU并行,还有基于ICP的6DOF（自由度）的姿态优化。我们表明，除了在物体级别场景描述中有明显的好处，这个范式也压缩了地图的存储空间，相比于稠密建图的系统却有着相近的预测能力。这个方法能够支撑很大的尺度的回环，重定位和在特定的先验上有巨大的利用潜力。

2.手持式传感器的实时SLAM

SLAM（同步定位与地图）中，移动的传感器在任意的长的运动中，实时地建立一个内部一致的地图，获得没有漂移的定位。目前，我们还没有真正看到一个'即拿即用（Pick up and play）的SLAM系统，可以嵌入在低成本的设备，同时不需要非专家用户的照料和理解，但最近已经有很多的进展。直到最近，最好的系统使用单眼或立体被动相机。稀疏特征滤波方法像[ 5 ]中的方法，用类似PTAM[ 8 ]的’关键帧SLAM’方法提升，他们使用集束调整，并行跟踪，使得能够统计高维度特征，得到更准确的跟踪。
最近，‘稠密SLAM’系统有一个重大突破，它利用GPGPU硬件进行重建和跟踪全表面非参数模型，这些模型表示为网格或隐式曲面。这种方法可以是用一个RGB相机[ 12 ]，商业深度相机现在有高性能、鲁棒的室内3D建图，特别是通过kinectfusion算法[ 11 ]。如[ 18 ]的新发展已解决尺度问题，通过滑动量，分块或八叉树的方式；但一个真正的尺度的，多分辨率，能够闭环的，稠密的，非参数曲面表示的方法仍然是难以捉摸的，将总是在浪费环境的对称信息。
基于稀疏特征的SLAM，世界被建模为一个互不连接的点云，不同于密集的SLAM假定场景包含连续的曲面，我们已经看到了先验场景知识已经出现并且增加。在SLAM+ +中，我们跨到了更强的假设，我们假设世界有内在的对称性的形式：重复的对象。当我们预先定义了场景内的物体，我们假定，范式允许在场景中的对象，作为显著，突出的元素被识别和自动分割。
物体SLAM具有很多特性，像是基于特征方法的回归。不同于稠密非参数的方法。我们的图中的离散的相关实体很少，是的他高度灵活能够通过所有的物体姿态进行优化，得到全局的一致性地图。在这个角度来说，地图实体是物体，而非点特征。但是，这使我们处于一个以往没有过的，更强大的位置上；跟踪6DOF的对象足以定位相机，因为物体测量时高度显著的，我们通过少量的物体测量来定位一个丢失的相机检测回环。此外，最重要的是，即时识别对象提供了极大的效率和鲁棒性，通过主动方法，通过我们一直物体的位置进行稠密预测，而保证了跟踪和对象检测。
SLAM + +对于场景重建和地图中的语义标记的兴趣越来越大，同样在计算机视觉和机器人社区也是。尽管我们强调的实时的算法和后处理几何标签以及闭环有很大的不同。一些最近的复杂工作是由Kim et al。[ 7 ]提出的。深度相机第一次被用来扫描场景，在规模上和物体内容上和我们稍后演示的结果相似，所有数据被融合成一个大点云。
离线，学习的物体模型，这些模型具有一定程度的变化以应付一系列的真实物体类型，被匹配到姿态中（joint scan），优化相似性和物体姿态之间的约束。结果令人印象深刻。强调标识而非辅助建图，我们可以看到一些问题，问题是缺失数据不能用来修复没有互动的图片。其他用RGBD数据进行标记的出色工作是由Silberman [16]和Ren et al. [14]做的，他们提出了一种对于外观和形状的核描述子，用物体和区域的身份来便是单个深度相机的图像。
一些已经发表的方法，用到了物体检测，和我们一样，目的不仅是表示而且是是基地用来提升重建和跟踪。但是这些方法没有人向我们这样做到实时的处理，3D的操作，稠密的预测和现代后端图优化。为了尝个鲜，Castle et al. [4]在基于特征的稀疏单目SLAM [5]中合并了平面的物体检测。这些物体，通过SIFT描述子进行识别，提高了SLAM的质量，因为这些大小和形状已知，因为这些物体都是简单的高纹理的海报，场景规模也非常小。在EKF SLAM范式中，Ramos et al. [13]提出了3D的激光/相机系统使用物体检测来讲离散的实体添入到地图中，不用原始的测量。最终同样的想法，物体检测辅助重建，同样被用在离线的三维重建中。Bao et al. [3] 用一组点，物体，区域来表示场景，是一个两个视角的SFM，解决了在图优化中做标识和重建，利用了场景中实体的联系。

3 方法

图2所示，接下来详细叙述。

3.1先建立物体数据库

在特定场景实时操作之前，我们先迅速地对于重复出现的物体建立高质量的3D模型数据库，用KinectFusion [11]方法交互地扫描，物体可以很容易地被环绕（circled）没有闭塞（occlusion.）
物体网格（mesh）从kinectfusion中利用移动立方体（marching cubes）[ 10 ]得到的截断（the truncated signed distance volume）标记距离的容积（volumn）。
为了从地平面分离物体，运动了少量的手工编辑，并用坐标系标记，从而可以应用特定域（domain-specific）的物体约束。这些重建的对象，存储在一个简单的数据库中。

3.2地图表示

我们对世界的描述是一个图，每个节点存储的j物体J的估计SE（3）姿势Two（旋转和位移相对于一个固定的世界坐标系）
Twi是相机在时间戳I的历史姿态。
（见图5）。

每个对象节点都是一个物体数据库里面的类型。每个通过相机的到的物体姿态的SE(3)测量 Zi,oj 一个相机的姿态对应一个物体的姿态。
附加的因子可以被选择性地加入到图中。
在相机姿态表示相机相机之间的运动估计（通过ICP方法）；
领域特定的结构先验信息，例如特定形状的物体必须被放在一个平面上。图优化的谢姐在3.5节讲述。

3.3 实时物体识别

我们遵循Drost等人的方法。[ 6 ]识别三维物体的六自由度姿态，在深度图像中用网格表示（mesh）。
我们详细介绍了我们的并行实现，完成了实时检测多个对象，同时在增长的参数空间中同时定位。
这个基于点对特征（PPFS）的投票机制；
对于相关位置和物体表面向量对的四个维度的描述子。
位置，和向量的估计，从深度相机的双边滤波图像随即采样。
这些采样在所有可能的组合中被联系起来生成点对特征PPF，代表了6DOF模型有相同的PPF.
每个物体网格有一个全局的描述，通过GPU来生成，通过以相同的值来分离PPF们，用并行的私有的操作把他们存储在搜索数据结构中，像reduction，scan和排序一样。
见ALG1

通过现代GPU模板库来实现，想BOLT，
相似的结构也用来建立实时的帧。
这个过程160K PPF只用不到5MS，可以在未来用在描述新的物体分类。On the fly只要他们可以被自动分割。
匹配相似的场景特征和模型，可以并行操作，通过一个投票的二进制搜索，每一个匹配就是一票。
总账的票在一个共享的buffer里面，可以prohibitively expensive在GPU上，可以有很多线程，可以进行原子化的操作，当一个共同的存储地址在增长 的时候，这样可以避免竞争条件。
为了克服这一点，每个票都用一个64位的整数码来表示，见图4.

排序相关的模型店，场景的参考点，ALIGNMENT角度，CONTIGUOUSLY在内存中。
这可以用一个并行的REDUCTION，
一个嘉禾操作来统计相同的票的码。
见算法2

在峰值查找后，每个场景参考点的姿态估计都在CPU上进行CLUSTERED聚类，通过翻转和旋转阈值。

3.3.1主动对象搜索

Drost等人识别算法[ 6 ] 的标准应用在房间的场景是非常成功的，尤其在对象时占据了大部分相机的视野，但对于远处或部分被其他物体遮挡的物体，由于样本点覆盖率，在这种情况下表现不好。
正是在这里，我们认识到我们在SLAM++方法的一个主要好处。
系统的视角预测能力，
意味着我们可以在深度空间中对于未被投影已知的像素物体生成掩码mask。
从相机测得的深度图像用mask来建材，采样只在未描述的区域。
（见图3）。

目标检测的结果往往是多簇峰，和量化的定位结果。
这些必须传递到下一节ICP refinement中来解释。
3.4. 相机跟踪,基于ICP的准确地物体姿态估计
相机模型跟踪：在kinectfusion 中[ 11 ]，最新的容积图被用于计算视角预测，
到先前估计的相机姿态，
使实时摄像机能被估计，利用快速密集迭代最近点（ICP）算法[ 15 ]
在SLAM + +中，相对kinectfusion早期测绘较不完整的模型，我们跟踪了完成高质量的多目标模型预测。
以下[ 11 ]，
我们计算当前场景集合的参考视角预测，包括，一个深度图，一个向量图，被渲染成之前帧的位子，用SE群以6DOF的刚体变化来表示。

我们更新了实时摄像机的世界变换TWL, 用估计的m序列的增量更新{ T˜RL n} m n = 1 parametrised与向量x∈R6定义Twist在SE（3），

TN=0是身份。
我们迭代最小化整个深度图像，点面度量，在实时深度图像的所有可能有用的像素上。

这里UN，都是和预测的数据关联，预测了顶点和向量，在相关的像素u上估计，计算顶点投影u,在像素u中，从实时深度地图到参考帧，利用以一个相机内参矩阵K，和标准的针孔模型投影公式pi，试试的顶点被转换到参考帧中利用现在的递增的变换T。

这里，用fi表示鲁邦的huber 惩罚函数，代替精确的点compatibility检查，在11中，这能够使得软的利群点soft outlier down weighting.
一个基于高斯牛顿梯度下降的方法在式子1中，来解决普通的问题。

这里j是雅克比矩阵，fi计算了鲁邦的惩罚函数是的估计误差。
我们用6*6的线性系统，使用了cholesky分解。
解出向量X在SE3中，通过exponential地图，
我们把递增地解迭代N+1，结合到之前的估计t中。

当前实时的顶点被转化成参考的帧，利用当前的增量的变化tn.

跟踪收敛：我们使用最大的M = 10迭代
，使用两个简单的标准检查收敛速度差的优化过程。
首先，如果它的像素覆盖率是小于18的一个完整的图像，我们不尝试跟踪预测模型。
第二，优化后迭代完成后，计算在实时图片中已经在预测模型中匹配的像素的比例，ASCERTAINED，DISCOUNTING像素，和一个特殊的magnitude EPP来比较，减少点面的误差。
模型初始化跟踪：利用稠密ICP姿态估计
与收敛检验，是SLAM+ +的两个关键部分。
首先，我们注意到，我们使用的现实世界物体往往其特征是模糊的，

在单个视角中没办法很好的区分？discriminated.
因此，
给出一个候选物体和检测的姿态，我们运行相机模型ICP估计检测到的物体姿态，并使用先前描述的标准收敛进行检查。
我们发现对于正确检测到的物体，从detector检测器姿态估计到的旋转误差在±30◦之间，平移误差在±50cm之间。这允许一个更保守的设置阈值OI和在早起出去不正确的物体。
相机对象姿态约束：
在SLAM++中，给定的一组可被检测的物体，我们进一步估计相对相机的物体姿态参数，用于在场景姿态图中induce约束。
在这一段，我们运行稠密的ICP估计之间的实时帧以及当前在帧中每个可见的模型对象。
这个能够计算独立的相关的姿态估计，使得引入了初始化prune poorly的概率，在之后从姿态图中跟踪物体产生了错误，
通过分析相机物体姿态的估计一致性的统计，我们能够保证一个在群点-离群点风格的计数。
去除哪些表现不佳的物体。

3.5图优化

我们假定从深度相机估计的I时刻的历史姿态，
静态物体T作为图优化，
Z代表在i帧中物体I6DOF的测量，
EIOJ式你测量covariance，可以被估计，利用hessian矩阵。
从最终的物体ICP重得出
Z是相关ICP的约束，在相机I ,I+1之间，
E是相关逆方差。
绝对的姿态T，室在优化中的决定变量。
Z式约束的测量。
所有的变量都有6DOF，可以用SE3来表示。
如图5中所示。

X是马氏距离，log是SE3 logarithmic图的对数。
这个均已最小问题，用LM解法。
底层的向量稀疏，精确用稀疏CHOLESKY算子。
姿态雅克比矩阵形势是。。
近似于用BCH来解。
包括已知结构（凳子都在地板上）

3.5.1 包含结构先验

附加信息可以加在途中，提高优化的鲁棒性和准确度。
在我们现在的补充版门中，我们利用了一个结构平面先验知识，就是物体都放置于一个地平面上。
世界坐标w，中w,x来表示地平面，y就是perpendicular.
地平面精确被探测，从初始的观测Z中，第一个物体重。
他的姿态在后来的优化中被调整。
为了惩罚地面物体的不同。Divergence.
我们讨论这个能量。

利用一组但限制P，
像图5中一样，
特别的，我们沿着Y皱来评议，V2式0，同时，X,Z旋转成0,
我们用一下的逆先验协方差。

正的权证是w,w。
因为物体可以被放置在任何地方，xz平面不能被角度限制，
y相关权重是支撑0，
先验V132不影响能量，可以被设置成绝对的一个值。

3.6其他已知

地平面约束在姿态图中非常有用。我们可以知道点对特征的采样，对姿态投票，在物体的非限制的自由度上。
在ICP方法中，一个先验可以用于能量最小过程，在场景限制的姿态。
当这些不添加进去，我们至少可以去去除哪些物体远离地面的位置。

3.7重定位

当相机跟踪丢失系统进入到重定位模式。
这里一个新的局部因子图被创建和跟踪，
当它至少有三个物体被之前的长期图匹配。
见图6

图匹配是通过同时考虑局部和长期图，
在3.3节我们描述过这两个图是在网格中是一组有方向的点。
我们用物体的位置，就像是顶点和x轴作为向量。
匹配的顶点在长期图中有最高的票数，
那么在局部图中酒杯替代了。
相机跟踪的恢复就从这点开始，丢弃局部地图。

4. 结果

在我们系统的回环操作中，提交的视频比文字更加的有效，我们方法的有点在历险场景标注也许不那么明显。
我们提供了一种新颖的实时定位于见图，
超过了现有的SLAM系统，在质量和几何描述的密度方面，
并且只用了非常少的footprint.
Rivalling 离线的多视角场景标注系统，解决了物体标注和姿态描述。

4.1 回环

Slam中回环发生在一个位置被重新走到，在一段护士过后，一段漂移被修正了。
在SLAM++中，小回环用在标准的ICP跟踪机制中。
大回环见图7,在漂移非常严重的时候能够通过ICP进行预测，
和在重定位中的，在长期姿态图中匹配的方式一样。

4.2 大场景地图

我们在大房间里运行SLAM++
如图1

大小是15 10 3
有两种椅子两种圆桌子，所有的都被一个普通的地面限制住。
在实时处理的过程中，大概10分钟，在不同的回环，重定位，为了解决丢失中。
34个物体被检测。
图一阐述了建图的结果。
注意在系统中没有关于桌子和椅子的位置的先验，只有他们都放置在地平面上。

4.3 移动物体检测。

我们阐述了算法检测移动物体的能力。在ICP中因为非一致性是不行的。
见图8

4.4 物体级虚拟现实

最后，算法能够预测完成的表面，在语义程度上，在部分视角中。
提供了一个新颖的虚拟现实能力，比如路径规划，
能够在到达目标物体的时候避开障碍物。
我们把这个细腻的能力用在在场景中导航，只要系统开始就在找一个作为坐下。
如图9

4.5 系统分析

我们系统在图7中就用了一个游戏笔记本。
我们比较了SLAM++和KINECTFUSION，在同样的存储。

5.总结和未来工作

在这篇文章中，我们提出了一种3D物体识别在循环中可以用来实时SLAM，在语义场景中非常有效。
特别的是，我们解释如何将地图和元素,紧密和识别互动。
现在我们的方法被用在像是有很多公共建筑物的场景，但是我们相信这是通往更通用SLAM的第一步，利用低纬度的形状物体，活着长远将，可以分割他们自己的物体分类。