Fusion++: Volumetric Object-Level SLAM

主要内容

本文提出了一种物体实例级别的基于体素的语义SLAM系统, 主要内容包括:

• 对每个物体实例维护一个TSDF体素地图.
• 对非物体(背景)维护一个TSDF体素地图,使用KinectFusion进行局部跟踪.
• 基于物体的重定位方法.
• 包含相机位姿和物体位姿的因子图优化. 包含相机位姿-物体位姿和相机位姿-相机位姿约束

不足

• 假设静态场景

摘要

McCormac等人提出了一种在线的物体级别的SLAM系统，能够建立持续准确的任意物体的3D地图。一个RGB-D相机在一个杂乱室内环境中运动，Mask-RCNN实例分割被用于初始化每个物体的紧凑TSDF重建和一个3D前景掩膜。每个物体的TSDF重建的分辨率与物体大小相关。重建的物体存储在一个优化的6DoF位姿图中，作为我们的持久地图表示。物体通过深度融合增量优化，并用于跟踪，重定位或回环检测。回环导致物体级别的相对位姿估计的调整，但是没有内部的物体扭曲。每个物体也保留随着时间被优化的语义信息和虚假物体预测的概率。

1. 介绍

室内场景理解和3D建图是使得真实世界机器人自主完成任务的基础技术，并且提供了一个公共的接口用于更智能和直观的人类-地图和人类-机器人交互。为了完成该任务，需要选择地图表示。一个特别有用的表示是建立面向物体的地图。我们认为这是一种自然和高效的方式，同时对于机器人的场景理解，规划和交互也最为重要；并且它对于人机交互也高度适合。

在一个物体级别的地图中，组成一个物体的几何元素被组装成一个实例，并且作为独立单元被标号和推理，和那些独立标记稠密几何的方法如面元或点完全不同。该方法也自然地铺平了到交互和动态物体推理的道路，虽然我们的系统当前假设一个静态环境，并且不致力于跟踪单个动态物体。

在本文我们展示了一个面向物体的在线SLAM，专注使用RGB-D数据的室内场景理解。我们致力于产生物体实例具有语义标号的TSDF重建，没有强调场景中物体类型的先验知识。我们使用Mask R-CNN来产生2D实例掩膜预测，并且实时融合这些掩膜到TSDF重建中（见图1），得到一个3D“体素掩膜”来融合实例前景（见图3）。

图1. Fusion++，包括在公开fr2_desk序列中的位姿图和被发现的物体

图3. 物体体素前景。注意到低于0.5的值未被渲染。

不像许多稠密重建系统，如DTAM，Kinitunuous，ElasticFusion等，我们不尝试保持整个场景的稠密表示。我们维护的地图只包括重建的物体实例。这个允许刚体TSDF体素用于高质量的重建，与位姿图系统的灵活性组合在一起，并且没有涉及到内部TSDF分解。每个物体包含在单独的体素中，允许每个有不同的，合适的分辨率。这允许以相对较小的内存使用跟踪大场景，和高保真度重建通过排除空闲区域的大体素。一个临时保存的局部TSDF被用于辅助跟踪和建模遮挡。

我们捕捉室内办公场景的重复回环，从而在偶尔约束较差的ICP跟踪条件下评估系统。场景包含大量多种物体，不仅展示了方法的一般性，而且对于评估内存和运行时间到多物体的可扩展性具有帮助。虽然没有实时性的优化，在我们的办公室序列中系统达到了4-8Hz操作表现（除了重定位/图优化模块），并且我们对于充分优化后的实时性操作具有很强的信心。我们也定量评估了我们方法相对一个RGB-D SLAM基准平台上的基线方法的轨迹误差表现的提升。

本工作中，我们做出如下贡献：

• 一个一般的面向物体的SLAM系统，将地图表示成多分辨率的3D实例重建。
• 利用体素前景掩膜鲁棒地融合逐帧实例检测结果，同时丢失检测通过“存在”概率被考虑。
• 我们展示了在全局一致的闭环物体SLAM地图中的高质量物体重建。

2. 相关工作

对于重建，我们跟随Curlesshe Levoy的TSDF架构和Newcombe et al.的KinectFusion方法。我们的物体级别重建方法与Zhou和Koltum的方法相关，其中“兴趣点”被检测，目标是重建场景从而保持这些区域的细节，并在环境的其余地方分布漂移和配准误差。在我们的方法中我们类似地致力于优化物体重建的质量并允许位姿图边缘处的残差误差被丢弃。

SLAM++是一个早期的RGB-D面向物体的建图系统。他们使用点对特征用于物体检测和一个位姿图用于全局优化。缺点是物体的实例集合和非常细节的几何形状必须提前知道，并在运行前离线预处理。Stuckler和Behnke也跟踪提前学习的物体模型通过对齐它们到一个多分辨率面元地图中。Tateno等使用一个预训练的物体数据库来生成描述子，当时他们使用一个KinectFusion TSDF来增量分割重建的TSDF体素区域，并直接和数据库中的其它物体匹配3D描述子。

大量物体发现的方法被提出。和我们最相关的是Choudhary等，他们使用发现物体作为路标在类似我们的位姿图中定位相机，虽然他们只使用点云中心，我们的位姿图物体路标边是全6DoF SE(3)约束由稠密体素上的ICP提供。他们展示方法提高了SLAM结果通过检测回环。但是，不像我们的工作，他们使用点云而不是TSDFs，并且没有训练一个物体检测器，而是使用Trevoer的无监督的分割方法。

另一个物体发现的方法是通过相同场景连续建图间的稠密变化检测。不像这些系统，我们的系统被设计为在线使用，并且在物体被检测前不需要变化发生在场景中。这些方法和我们的方法互补，提供了CNN精修的监督信号，并且产生额外的物体数据库滤波机制。

在纯RGB SLAM的物体检测中，Pillai和Leonard使用ORB-SLAM来辅助物体识别。它们使用一个半稠密地图来产生物体建议，并组合多视角的检测证据用于物体检测和分类。MO-SLAM专注于通过复制的物体发现。他们使用ORB描述子来搜索路标集合，可以通过单个刚体变换进行组合。该方法类似我们的重定位方法，使用BRISK特征但是使用深度进行了增强。

和我们工作非常接近的是Sunderhauf等，他们提出一个面向物体的建图系统，由使用边界框检测和一个无监督几何分割算法得到的实例组成。虽然这个前提非常相关，但是有许多不同。他们使用一个单独的SLAM系统，ORB-SLAM2，而我们的系统被发现的物体实例紧耦合到SLAM系统中。我们也融合实例到单独的具有来自2D实例掩膜检测的前景掩膜的TSDF体素中。

大量最近的工作已经被发布。Pham等融合整个场景的TSDF并使用一个CNN和CRF语义标记体素。为了分割实例，除了融合原来的实例检测，它们选择在3D中聚类语义标号体素。该方法是稠密3D语义建图后自然想到的下一步，但是不适合物体级别位姿图优化和作为嵌入到共享TSDF中的实例重建。它也需要语义识别作为物体发现的前提，被证明是有问题的对于相似的和在相邻区域中未识别的物体。

图2. Fusion++系统综述

3. 方法

我们的流程图在图2中可视化。根据RGB-D输入，一个粗糙的背景TSDF被初始化用于局部跟踪和遮挡处理（3.3节）。如果位姿变化足够或者系统出现丢失，重定位（3.4节）和位姿图优化（3.5节）被执行来达到一个新的相机定位，并且粗糙的TSDF被重置。在一个分离的线程中，RGB帧通过Mask R-CNN被处理，并且检测被过滤并与现存地图匹配（3.2节）。当没有匹配出现，新的TSDF物体实例被创建，固定大小，并添加到地图中用于局部跟踪，全局图优化和重定位。在未来的帧中，关联的前景检测被融入到物体的3D“前景”掩膜中，包含语义和存在概率（3.1节）。

3.1 TSDF物体实例

我们的地图由分离的TSDF中重建的物体实例组成，每个物体具有一个由变换定义的位姿，  ，其映射一个点从从物体坐标系  的坐标  到世界坐标系  的坐标  。为了记号方便，在合适时假设坐标是归一化的（比如在坐标变换时），但是当需要显式表示时，使用斜体  。物体实例帧在体素中心有一个原点，并且边长为  。

初始化和调整大小：通过3.2节描述的过程，不匹配的检测被用于初始化一个合适大小和位置的TSDF。在第k帧中，每个检测  产生一个二值掩膜  。我们利用深度地图 将所有掩膜的图片坐标  投影到  中，

 (1)

其中  表示3x3内参矩阵，  是相机位姿估计。

为了鲁棒地确定当前掩膜的TSDF大小（其偶尔包含很远的背景表面），我们没有直接接受点云的最大值和最小值。替代地，我们使用第10%和90%的点云（分别对于每个轴）来定义点  和  ，用于计算体素中心  和体素大小  。我们使用m=1.5用于补偿腐蚀并提供额外的填充。

每个实例TSDF在给定轴方向有初始的固定分辨率  ，选择为64，同时  用于计算体素的物理大小  。因此，小物体将会被细节地重建，大物体被粗糙地重建，使得在给定内存占用的情形下地图尽可能地有用。

在操作过程中，匹配的物体可能需要重新分配大小为包含额外区域的新检测。为了完成这个，上面提到的当前掩膜的点云和当前TSDF重建生成的类似腐蚀的点云融合起来。同时包含它们的3D体素像以前一样被用于计算新的体素中心和大小。在重新分配大小时为了避免aliasing，我们通过  的离散倍数来平移体素中心，并保持相同的  ，但是增加  ，同时维护一个均匀的parity。我们也限制最大体素分辨率（某个轴的体素个数）为128个，如果  ，那我们重新初始化体素并限制物体大小的最大值为3m。

在初始化一个实例之前我们需要体素中心在相机的5m内，并且一个3D轴对齐的有界框和任何地图中其他已存在的体素的交并比（IoU）<0.5。当一个物体中心被移除，位姿图节点和关联的测量也会如3.5节描述的那样更新。

积分：对于从深度图  积分到  ，我们采用类似KinectFusion的方法。  存储在每个离散体素位置  ，包含当前的归一化截断有向距离值  和它的关联权重  。如果投影到相机坐标系的  的深度低于深度测量加上截断距离，  （这里选择为  ），那么测量融合以加权平均的方式进行。积分在每一帧中执行，其中TSDF体素是可见的，当体素像素的50%都合法地跟踪到，并且ICP RMSE<0.03（这些误差尺度详细在3.3节中描述）。这是为了在相机帧可能出现漂移时维持实例的重建质量。

值得注意的是上述表面积分在整个体素中进行，不管是否为掩膜区域。为了存储对应实例的“前景”的体素，我们也需要融合实例掩膜检测。我们将正或负检测看做从一个隐式前景概率中二项实验采样得到的结果，  （  前景）。我们存储前景  和非前景  在一个参数为  的beta分布共轭先验中计数，参数初始化为  。当一个新检测和一个在如上阶段距离内的深度测量匹配，那么我们也使用对应的掩膜  更新检测计数：

 (2)

 (3)

其中  表示投影。最终，为了计算一个体素是否为前景的一部分，我们计算期望，

 (4)

并使用一个决策阈值  。该部分的可视化如图3所示。

图3. 物体体素前景。注意低于0.5的值没有呈现

Raycasting：对于跟踪，数据关联和可视化，我们呈现深度，法向量，顶点，RGB和物体指标。在每个物体体素  内 ，我们沿着射线方向移动  （在  时移动  ，其中  是使用  归一化后的SDF）并搜索在  方向的过零点其中  （二者的值都通过临近体素的三线性插值来平滑表示）。我们存储这些交点的最近邻的射线长度以防止搜索到另外一个体素中。

这导致不是前景的遮挡表面不会遮挡该射线。如果背景TSDF是可得到的，并且或者没有和前景物体相交，或者交点在背景TSDF中相交，交点远大于5cm，那么就替代地使用背景TSDF射线交点。

存在概率：为了避免冗余的实例，我们使用Beta分布建模每个实例的存在概率为  ，和前景掩膜的方式相同。对于任意可以可视化预测实例的一帧（即raycasted图片由至少  个像素属于该实例），如果实例和一个检测相关联，那么它的存在概率增加，如果不存在相关检测，则增加不存在计数  。如果  低于0.1，实例将被删除，与该节点相关的边从位姿图移除（在3.5节描述）。

语义标号：每个TSDF也存储一个潜在类别编号  的概率分布，Mask R-CNN在给定图片  的类别上提供概率分布  。我们发现标准贝叶斯方案的乘积式：

 (5)

其中  是一个归一化常数，经常会导致一个过度自信的类概率分布，得分不适合用于在物体检测中排序。替代地，我们通过简单的平均融合多次相关的检测：

 (6)

其产生一个更均匀的类概率分布。

3.2 检测和数据关联

对一个包含实例i的给定帧k，Mask R-CNN模型的检测有一个二值掩膜  和一个类概率分布  。一个前向传播需要250ms，即使该系统不是实时的，这仍然表示了一个具有重要意义的瓶颈，从而可以在一个并行线程中执行。对于CPU内存效率，我们采用前100个检测（根据区域建议网络“物体”得分排序）并且滤除离图片边界过近的掩膜（20个像素），同时满足  和  。

在局部跟踪后（3。3节）我们对于当前视角下的实例  ，使用估计的相机位姿和地图中已经初始化的TSDFs来raycast一个二值掩膜  。我们映射每个检测  到一个单独实例  通过计算二者的相交区域作为检测区域的比例，  并分配检测到最大的相交， ，其中  ，否则检测不分配。对于积分步骤，每个映射到相同实例的检测通过取检测掩膜的并集和类别概率的均值被组合。

3.3 分层的局部跟踪

我们维护一个实例-不可知的粗糙背景TSDF，  来维持局部帧-模型跟踪和处理遮挡。它具有  的分辨率，体素大小为2cm。它的初始点  ，是沿着相机坐标系  2.56cm来避免浪费的体素。体素被重置当新的初始化点出现在之前体素中心的椭圆阈值（1.28cm）外，即  。

我们组合背景TSDF和单个实例来raycast（3.1节）一个“分层的”参考坐标系，记作  ，具有一个顶点地图，  ，一个法向量地图，  ，和一个物体顶点地图，  ，从之前相机位姿  ，顶点和法向量定义在世界坐标系  中。到当前帧的变换记作  ，通过对其双线性插值并投影后的当前深度地图到呈现的地图，通过ICP使用投影数据关联和点面误差  ：

 (7)

 (8)

 (9)

其中  包含任何具有对应顶点和法向量的  ，其中有一个对应的  具有一个合法的顶点和法向量，并且其中  和  。

我们使用高斯牛顿算法最小化非线性最小二乘问题。我们在之前的估计加上扰动  处线性化  ，其中  。  维雅克比，  的每行对应一个给定  的残差 ：

 (10)

高斯牛顿迭代可以被应用如下（迭代下标为t）:

 (11)

 (12)

6x6Hessian矩阵近似，  ，和6x1误差雅克比，  在GPU上并行得到，并在CPU上使用SVD和替换解决。我们使用一个三层从粗到精的金字塔方案，每层使用5次高斯牛顿迭代。

我们在CPU上执行一个额外的化简来产生相同的等式系统到与每个  中的实例相关的像素，  中为了为子图优化和产生逐实例的误差尺度。误差尺度是ICP RMSE，  ，和合法跟踪体素的比例  。它们被用于实例积分和来检测是否局部跟踪丢失了。我们考虑局部跟踪丢失了当全局ICP RMSE大于0.05m或者当至少10%图片包含实例TSDFs并且少于一半的像素被合法跟踪，在这种情况下重定位模块启动。

3.4 重定位

如果系统跟丢了或者我们重置了粗糙的TSDF，我们执行重定位来对齐当前帧到当前的实例集合（如果有的话）。我们发现只使用体素重建的直接稠密ICP方法对于宽基线重定位不能产生准确的结果，因为它们对初始位姿敏感，小物体经常产生歧义在没有纹理的约束情况下。即使替代的稠密方法可能是有用的，我们采用稀疏BRISK特征的快照方法（检测阈值为10）。对于一个给定的物体检测，如果15度角度差内没有存在的物体快照，我们从那个位姿处增加一个新的物体快照（见图4）。

图4. 一个实例周围的重定位快照

为了重定位，我们对每个实例执行3D-3D RANSAC，其中我们不产生大于0.6的预测类别分布。我们使用OpenGV具有至少5个内点特征的来单独匹配每个物体。如果我们在这个场景中发现一个或者多个匹配物体，我们在该场景的每个点处执行最终的3D-3D RANSAC（从所有物体和背景中）具有最小50个内点特征（5cm内）来达到最终的相机位姿。位姿被用于得到一个新的地图的参考图片来产生为子图优化需要的约束。

3.5 物体级别位姿图

我们的位姿图构架类似SLAM++。对于每帧Mask R-CNN检测（包括粗糙的TSDF重置），我们增加一个新的相机位姿节点到地图中。当一个新的实例，下标  ，被初始化，一个对应的由固定在物体体素中心，  的坐标系定义的路标节点被增加到图中。第一个相机位姿节点被固定下来，并被定义为世界坐标系  的原点。每个节点包括从物体到世界的全SE(3)变换，  或者相机到世界坐标系的变换，  ，和节点间的SE(3)相对位姿约束测量。

每个相对测量被推导通过应用对应特定物体o的像素的ICP误差项（对于物体-相机约束），或者实例-不可知的背景a（对于相机-相机约束）。为了保证测量和分割集合的二次近似误差函数的最小值相符，一个额外的高斯牛顿步骤使用分割的  被执行（见3.3节）来产生“虚拟的”相机节点间的相对位姿测量  ，和相机和路标物体间的  。对于图因子的结果测量误差为：

 (13)

 (14)

对于每个相对测量，我们通过  近似逆测量协方差。但是，由于ICP算法和为子图优化算法的扰动方式不同，我们需要通过考虑局部扰动间的相对关系变换协方差矩阵。图优化器通过  建模扰动  到相对位姿测量（对于  也是类似）。为了保证我们的信息矩阵适当地对应扰动  ，有必要变换  。如等式12所示，  对应扰动  。  和  的关系如下：

 (15)

 (16)

 (17)

其中  是  的共轭，从而  。相机节点的推导导致新的信息矩阵的变换，  (18)

最终位姿图中的最小化误差是，给定测量，状态和信息矩阵的情况下，所有相机到物体边  和相机到相机边  的和

 (19)

其中L  表示鲁棒Huber核。我们在g2o中使用稀疏Cholesky分解和LM算法解决。在优化后，我们更新实例TSDFs和相机的位姿在初始化新的到位姿的粗糙TSDF之前，并继续局部跟踪。

如3.1节所述，当一个路标被重新分配大小后，它的中心  ，可以被分配从  到新坐标系  的变换  。在这种情况下，我们也变换对应的节点变量，  和边测量  。