DynaSLAM II

Bescós 2021 · 论文

一句话总结 — 紧耦合的多目标跟踪与SLAM:不像DynaSLAM那样直接丢弃动态物体,而是在同一个光束法平差中联合优化相机位姿、静态地图以及运动刚体物体的轨迹。

问题

剔除动态物体(DynaSLAM、DS-SLAM)可以保护相机跟踪,但也丢弃了信息——而”包括自动驾驶、多机器人协作以及增强/虚拟现实等大多数场景,都需要周围环境的显式运动信息以辅助决策与场景理解。“因此DynaSLAM II颠覆了前一篇论文的理念:不再过滤掉动态内容,而是在一个针对双目和RGB-D配置的紧耦合优化中,将运动物体的轨迹与相机联合估计。

方法与架构

基于ORB-SLAM2构建。每一帧:进行逐像素实例分割,并在双目图像对之间匹配ORB特征。静态特征与前一帧/地图进行匹配以初始化相机;动态特征(位于车辆/行人/动物实例上)则与局部地图中的物体点进行匹配——若已知物体速度,则通过恒速模型的重投影匹配,否则通过限定在重叠度最高实例内的暴力匹配。当一个实例的大多数关键点匹配到某物体的点时,该实例便继承该物体的轨迹id,并辅以CNN 2D框的IoU匹配加以确认;由于匹配对象是地图中的物体而非前一帧,遮挡问题得以处理。新物体的SE(3)位姿初始化为其三维点质心,旋转设为单位阵。

含物体的重投影误差。 经典的静态残差$\mathbf{e}^{i,l} = \mathbf{u}^l_i - \pi_i(\mathbf{T}^i_{CW}\,\bar{\mathbf{x}}^l_W)$对动态点重新表述为

$$\mathbf{e}^{i,j,k}_{\mathrm{repr}} = \mathbf{u}^j_i - \pi_i\big(\mathbf{T}^i_{CW}\,\mathbf{T}^{k,i}_{WO}\,\bar{\mathbf{x}}^{j,k}_O\big),$$

其中$\mathbf{T}^{k,i}_{WO}$是相机$i$观测到物体$k$时该物体的位姿,$\bar{\mathbf{x}}^{j,k}_O$是固定在物体坐标系中的一个三维点。因此物体点保持唯一:参数数量从$N = 6N_c + 3N_cN_oN_{op}$(每帧独立的点云)缩减为$N' = 6N_c + 6N_cN_o + 3N_oN_{op}$。

含物体的BA。 相机、静态点、物体位姿、物体点以及物体速度$\mathbf{v}^k_i, \mathbf{w}^k_i \in \mathbb{R}^3$被联合优化(经Huber鲁棒化),并附加两个因子:恒速误差$\mathbf{e}^{i,k}_{vcte} = \big[\mathbf{v}^k_{i+1}-\mathbf{v}^k_i;\ \mathbf{w}^k_{i+1}-\mathbf{w}^k_i\big]$,以及一个将速度与位姿、点相关联的耦合项,

$$\mathbf{e}^{i,j,k}_{vcte,XYZ} = \big(\mathbf{T}^{k,i+1}_{WO} - \mathbf{T}^{k,i}_{WO}\,\Delta\mathbf{T}^{i,i+1}_{O_k}\big)\,\bar{\mathbf{x}}^{j,k}_O, \qquad \Delta\mathbf{T}^{i,i+1}_{O_k} = \begin{bmatrix} \mathrm{Exp}(\mathbf{w}^k_i\,\Delta t) & \mathbf{v}^k_i\,\Delta t \\ \mathbf{0} & 1 \end{bmatrix}.$$

当相机跟踪或物体跟踪变弱时会生成关键帧;由物体触发的关键帧会在2秒的时间窗口内对该物体和相机进行优化。Schur补求解的代价为$O(N_c^3 + N_c^2 N_{mp} + N_c N_o N_{op})$。

解耦的三维边界框。 与CubeSLAM不同,轨迹估计不需要等待边界框:每条轨迹只需一次通过RANSAC拟合边界框(通过物体点拟合两个互相垂直的平面,未观测维度采用基于类别的先验),随后在一个时间窗口内通过最小化其图像投影与CNN 2D框之间的距离进行松弛优化——仅当至少3个关键帧观测到该物体时才进行,对无法观测到的视角采用软性的维度/位姿先验。

实验结果

• KITTI跟踪(自身运动):平均ATE为1.54 m,优于ORB-SLAM2的2.33 m,但略逊于DynaSLAM的1.45 m;在交通繁忙的序列0020中,为1.36 m,远优于ORB-SLAM2的16.80 m。当动态类别物体处于静止(停车)状态时,DynaSLAM II会保留它们(速度≈0)并优于DynaSLAM——后者会盲目删除其特征。平均RPE为0.053 m/帧,在每个序列上都优于VDO-SLAM(0.084),不过VDO-SLAM的旋转RPE更低(0.036对0.043°/帧);ClusterVO则大体相当。
• 多目标跟踪:在KITTI 3D物体基准上,其MOTP在原始精度上不及单视角CNN加精细化方法以及稠密双目系统,但在截断和遮挡情况下的性能下降远小于这些方法,代价是发现的边界框数量较少(基于特征的稀疏性);被跟踪的车辆能保留大部分轨迹,而行人的轨迹精度较低(非刚性)。
• 计时:在最多同时存在2个物体时(序列0003)约12 fps,在最多存在20个物体时(序列0020)约10 fps(不含分割CNN耗时)——是唯一能实时运行的、经过对比的联合SLAM+MOT系统。

对SLAM的意义

DynaSLAM II代表了动态SLAM的第二代:一旦剔除动态内容的问题得到充分解决,研究前沿便转向利用动态内容。其核心实证结论——跟踪动态物体本身有利于相机跟踪,而不仅仅是有利于场景理解——加上以物体为中心的参数化方式以及将轨迹与边界框解耦的思路,构成了以驾驶为导向以及具身智能SLAM系统所依循的模板,与同期的VDO-SLAM并驾齐驱。

相关条目

• DynaSLAM
• VDO-SLAM
• CubeSLAM
• MID-Fusion
• ORB-SLAM2
• MonST3R