VDO-SLAM

Zhang 2020 · 论文

一句话总结 —— 一个具有动态物体感知能力的 SLAM 系统,在单一因子图中联合估计相机轨迹以及刚性运动物体的 SE(3) 运动,无需任何先验物体模型。

问题

机器人在动态环境中的路径规划和避障依赖于了解机器人周围物体的运动方式——然而经典 SLAM 假设世界是静态的,而大多数”动态 SLAM”系统只是检测运动内容并将其丢弃。VDO-SLAM 则将动态刚性物体本身整合进估计问题:它在无需任何关于物体形状或几何模型的先验知识的情况下,识别、跟踪并联合相机估计每个运动物体的完整 SE(3) 运动。

方法与架构

预处理。 实例分割(Mask R-CNN,COCO 权重)分离出潜在的可移动物体;稠密光流(PWC-Net)在各处提供对应关系,最大化占据像素较少的物体上被跟踪的点数,并传播逐物体的标识符(在分割失败时恢复掩码)。输入可以是 RGB-D、由立体转换而来的深度,或使用 MonoDepth2 深度的”基于学习的单目”方案。

运动表示。 给定相机位姿 ${}^{0}\mathbf{X}_k$ 和物体位姿 ${}^{0}\mathbf{L}_k \in \mathrm{SE}(3)$,刚性物体上的点 ${}^{0}\mathbf{m}^i_k$ 满足

$${}^{0}\mathbf{m}^{i}_{k} = {}^{0}\mathbf{L}_{k-1}\; {}^{L_{k-1}}_{k-1}\mathbf{H}_{k}\; {}^{0}\mathbf{L}^{-1}_{k-1}\; {}^{0}\mathbf{m}^{i}_{k-1} = {}^{0}_{k-1}\mathbf{H}_{k}\; {}^{0}\mathbf{m}^{i}_{k-1},$$

因此物体的逐帧运动 ${}^{0}_{k-1}\mathbf{H}_k \in \mathrm{SE}(3)$ 将其自身的点在时间上关联起来,而完全不需要将物体位姿纳入状态——这正是实现无模型运行的关键。

前端估计。 相机位姿通过在静态点上最小化经 Huber 鲁棒化的重投影误差来求解,$\mathbf{e}_i = {}^{I_k}\tilde{\mathbf{p}}^i_k - \pi({}^{0}\mathbf{X}^{-1}_k\,{}^{0}\mathbf{m}^i_{k-1})$;每个物体的运动则通过最小化类似的代价函数来求解,$\mathbf{e}_i = {}^{I_k}\tilde{\mathbf{p}}^i_k - \pi({}^{0}_{k-1}\mathbf{G}_k\,{}^{0}\mathbf{m}^i_{k-1})$,其中 ${}^{0}_{k-1}\mathbf{H}_k = {}^{0}\mathbf{X}_k\,{}^{0}_{k-1}\mathbf{G}_k$,两者都以 $\mathfrak{se}(3)$ 参数化,并用 Levenberg–Marquardt 求解。关键之处在于,光流会与每个运动联合精修(一个正则化项将其锚定到初始光流上),这大大延长了点的跟踪轨迹。物体是否被判定为动态取决于场景流大小(阈值 0.12,若物体超过 30% 的点在运动则判定为动态)。

后端因子图。 相机位姿、静态点、动态点和物体运动在同一个图中通过四种因子类型联合精修:

$$\mathbf{e}_{i,k} = {}^{0}\mathbf{X}^{-1}_k\,{}^{0}\mathbf{m}^i_k - \mathbf{z}^i_k \quad \text{(三维点观测)}, \qquad \mathbf{e}_{i,l,k} = {}^{0}\mathbf{m}^i_k - {}^{0}_{k-1}\mathbf{H}^l_k\,{}^{0}\mathbf{m}^i_{k-1} \quad \text{(三元点-运动因子)},$$

再加上里程计因子和一个平滑运动先验 $\mathbf{e}_{l,k} = ({}^{0}_{k-2}\mathbf{H}^{l}_{k-1})^{-1}\,{}^{0}_{k-1}\mathbf{H}^l_k$,用以惩罚物体运动的突变。使用修改版 g2o 中的 LM 求解。物体的线速度可以直接从运动中得出:$\mathbf{v} \approx {}^{0}_{k-1}\mathbf{t}_{k} - (\mathbf{I}_3 - {}^{0}_{k-1}\mathbf{R}_{k})\,\mathbf{c}_{k-1}$,其中 $\mathbf{c}_{k-1}$ 为物体点云的质心。

实验结果

• Oxford Multimotion(swinging_4_unconstrained,500 帧):相机误差 $E_t$ = 0.0112 m / $E_r$ = 0.77°,相比 MVO 的 0.0314 m / 1.19°(相机估计约提升 35%,在摇摆的箱子上提升 15–40%);物体运动估计精度在大多数箱子上比 ClusterVO 高出两倍以上,不过 ClusterVO 在相机平移上略胜一筹(0.0066 m)。
• KITTI tracking(9 个动态序列):相机精度与 DynaSLAM II 相当(旋转误差略低,平移误差略高);物体运动的平移误差为 0.1–0.3 m,旋转误差为 0.2–1.5°(RGB-D),相比 CubeSLAM 的 >3 m / >3°——在大多数情况下提升了一个数量级。物体在其出现期间超过 80% 的时间被跟踪;估计速度始终接近真值(例如一辆货车在经历 33 帧的分割失败后仍被跟踪,平均速度误差为 2.64 km/h)。
• 消融实验: 联合光流精修产生了远多得多的长跟踪轨迹(例如序列 01 的背景:被跟踪超过 5 帧的点从 237 增加到 5075),并使误差降低约 10%(相机)/约 25%(物体);全局图精修使物体运动误差最多降低 39%(平移)/55%(旋转)。
• 运行时间: 在配备笔记本 CPU(i7 2.6 GHz)的设备上,跟踪速度为每秒 5–8 帧,分割和光流均离线计算。

对SLAM的意义

VDO-SLAM 是”估计而非丢弃”这一动态 SLAM 流派的一个里程碑:动态物体成为一等的估计目标,而不是被当作离群值处理。其无模型的点-运动因子——在无需物体位姿变量的情况下,将刚体上的点在时间上关联起来——是一种优雅的表述,如今已成为动态 SLAM 的标准做法,而其开源发布也使其成为驾驶和移动机器人场景中联合相机/物体估计的参考基线。

相关条目

• DynaSLAM —— 动态内容检测与移除方法
• DynaSLAM II —— 秉持相同理念的紧耦合多物体跟踪与 SLAM
• MID-Fusion —— 基于 RGB-D 的动态物体级稠密跟踪
• MaskFusion —— 运动物体的实时识别与重建
• PWC-Net —— 其所依托的稠密光流前端
• 因子图 —— 承载相机和物体状态的机制