SLAM++

Salas-Moreno 2013 · 论文

一句话总结 — 第一个面向对象的SLAM系统:其地图是一个由六自由度物体位姿构成的显式图,通过”在环内”实时进行的三维物体识别构建而成,以远小于原始几何地图的存储开销实现了强大的稠密预测能力。

问题

在SLAM++之前,实时SLAM系统都基于低层次的基元——点、线、图像块,或非参数化表面(如深度图)——这些基元必须经过鲁棒的匹配、几何变换与优化处理。这不仅浪费了大量计算资源,也忽视了领域知识:许多室内场景由重复出现的、特定领域的物体和结构组成(椅子、桌子、显示器)。如果能在SLAM运行的环路本身中利用这些物体的先验知识,那么基于深度数据的物体识别就可以取代原始几何处理——从而得到紧凑、具有语义意义的地图、强有力的回环约束,以及从物体图直接生成的稠密表面预测。

方法与架构

其处理流程(论文图2):实时帧表面测量(顶点图$\mathcal{D}_l$ + 法向图$\mathcal{N}_l$)→ (1) 通过对当前SLAM图$\mathcal{G}$渲染出的稠密多物体场景预测进行ICP配准,来估计实时相机位姿 → (2) 物体检测并插入图中 → (3) 位姿图优化 → (4) 为下一次预测及主动物体搜索渲染表面。

• 物体数据库:每个重复出现的物体先用KinectFusion扫描一次,通过Marching Cubes算法提取网格,清理后以一个规范坐标系存储。
• 实时物体识别(GPU):遵循Drost等人的方法,点对特征(PPF)——即成对定向点之间相对位置/法向的4维描述子——在广义霍夫参数空间中进行投票。投票被打包为64位编码,并在GPU上通过并行排序+归约进行累加;构建16万个PPF的全局模型描述耗时不到5毫秒。检测出的位姿存在约±30°旋转和±50厘米平移的误差,因此每个候选结果在插入之前都会经过第二次ICP估计的精细化与门限筛选。
• 相对物体图的相机跟踪:相机到模型的变换通过对实时深度图中所有有效像素$u \in \Omega$的点到面误差进行迭代最小化来估计,

$$E_c(\mathbf{x}) = \sum_{u\in\Omega} \psi\big(e(u,\mathbf{x})\big), \qquad e(u,\mathbf{x}) = \mathcal{N}_r(u')^\top\big(\exp(\hat{\mathbf{x}})\,\hat{v}_l(u) - v_r(u')\big),$$

其中$\mathbf{x}\in\mathbb{R}^6$为增量李代数扭量,$v_r, \mathcal{N}_r$为预测的顶点/法向图,$u'$为投影关联的像素,$\psi$为鲁棒的Huber惩罚函数。高斯-牛顿法方程($6\times6$,Cholesky分解)最多迭代10次。与KinectFusion的局部模型不同,相对完整、高质量的多物体预测进行跟踪,还使得系统能够屏蔽已被解释的像素,从而使物体搜索只关注尚未被描述的区域(“主动搜索”)。

• 图优化:世界被表示为一个由历史相机位姿$T_{wi}$和物体位姿$T_{wo_j}$(均在$\mathbf{SE}(3)$中)构成的图,带有六自由度相机-物体ICP测量$Z_{i,o_j}$以及相机-相机约束$Z_{i,i+1}$:

$$E_m = \sum_{Z_{i,o_j}} \big\lVert \log\big(Z_{i,o_j}^{-1} \cdot T_{wi}^{-1} \cdot T_{wo_j}\big) \big\rVert_{\Sigma_{i,o_j}} + \sum_{Z_{i,i+1}} \big\lVert \log\big(Z_{i,i+1}^{-1} \cdot T_{wi}^{-1} \cdot T_{wi+1}\big) \big\rVert_{\Sigma_{i,i+1}},$$

其中$\lVert\mathbf{x}\rVert_\Sigma := \mathbf{x}^\top\Sigma^{-1}\mathbf{x}$为马氏距离,$\log(\cdot)$为$\mathbf{SE}(3)$的对数映射,逆协方差由ICP的Hessian矩阵近似:$\Sigma^{-1} = J^\top J$。使用Levenberg-Marquardt算法和稀疏Cholesky求解器(g2o)求解。

• 结构先验:每个物体附加一个一元的地面约束($E_{m\&p}$),将物体固定在从首次观测中检测到的一个共同平面上。
• 重定位与大范围回环检测:跟踪丢失后,会跟踪一个新的局部图;一旦该局部图中至少包含3个物体,就将其与长期图进行匹配,方法是将两个图都视为定向点构成的网格(以物体位置为顶点,以物体x轴为法向),送入同一套PPF识别流程。若被移动的物体未通过ICP一致性检验门限,则会被标记为无效,以避免破坏图结构。

实验结果

• 在一次持续约10分钟的实时运行中建图了一个15×10×3米的大型公共房间,包括回环检测与跟踪丢失后的重定位;共建图34个物体实例(两种椅子类型、两种圆桌类型),仅受地面先验约束。
• 在一台游戏笔记本电脑上对一个10×6×3米房间的系统统计数据:20 fps,132个相机位姿,来自5个物体类别的35个物体,338条图边。
• 内存占用:350 KB的图 + 20 MB的物体数据库,相比之下,同等体积的KinectFusion TSDF需要1.4 GB(每体素4字节,每米128体素)——压缩比约为1/70,而预测能力相近。
• 展示了被移动物体的检测能力(不一致的物体会被标记并排除)以及具有上下文感知能力的AR:虚拟角色可在物体地图中导航并找到落座位置,而无需扫描整个房间。

对SLAM的意义

SLAM++开辟了物体级SLAM这一研究方向,证明了由语义实体构建的地图可以比原始几何地图紧凑几个数量级,同时仍能在物体级别实现回环检测、重定位以及场景理解。它的主要局限——依赖预先构建的已扫描实例数据库——恰恰是其后续研究谱系所去除的部分:Fusion++使用Mask R-CNN实时发现物体,MoreFusion对多物体的六维位姿进行推理,而NodeSLAM与DSP-SLAM则用学习到的形状先验取代网格模型。其核心洞见——语义实体比原始几何构成更强大的地图元素——在现代三维场景图系统中再次浮现。

相关条目

• Fusion++
• MoreFusion
• DSP-SLAM
• KinectFusion
• 位姿图优化