带有物体级重定位的视觉惯性多实例动态SLAM

Ren Y, Xu B, Choi C L, et al. Visual-Inertial Multi-Instance Dynamic SLAM with Object-level Relocalisation[C]//2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2022: 11055-11062.

0. 摘要

提出了一个紧耦合的视觉惯性物体级多实例动态SLAM系统，可以在极端动态场景中优化相机位姿、速度、IMU漂移并且重建环境的3D稠密物体级地图。在鲁棒的传感器和物体检测技术的支持下，系统通过增量地将颜色、深度、语义和前景物体概率融合到每个物体中，以鲁棒地跟踪和重建任意物体的几何、语义和运动。另外，当物体丢失或被移出相机视场时，系统可以在重新观测到时可靠地恢复其位姿。作者在现实世界数据序列中定性和定量地测试了系统的鲁棒性和准确性。

1. 引言

物体层面的跟踪和建图不需要依赖于什么物体是静态的先验信息，而是通过推断来实现，可以应对包含很多潜在动态物体的大型静态环境。

先前的动态物体SLAM工作的局限性：（1）预先设定动态、静态物体的语义标签；（2）假定动态物体不会占据相机视场主体；（3）只在非重复性动态设定中评估，造成重复的物体建模。

本文，以紧耦合方式读取IMU、RGB-D数据，估计相机位姿、速度 和 IMU漂移，这个过程中保持重力方向可观测，对于创建有意义的物体地图来说非常重要，进一步的，为了在一个更大场景鲁棒跟踪物体位姿，通过使用基于关键帧的特征匹配恢复先前建构建的模型，引入物体重定位步骤。它在物体级地图和新的观测之间建立可靠的数据关联，减少了机器人在大范围探索时物体地图的重复。

主要的贡献有三个方面：

• 第一个紧耦合稠密RGB-D-Inertial物体级多实例动态SLAM。
• 提出了一种物体重定位方法，以恢复在视场中消失又出现的物体。
• 在从小尺度到房间大小的动态环境中进行了广泛测试，验证有效性和鲁棒性。

2. 相关工作

2.1 物体级动态SLAM

  从[13]开始，物体级动态SLAM被认为是一个SLAM领域的一个重要议题，因为它对机器人理解周围包含动态物体的环境来说非常重要。这样一个系统的目标是检测和跟踪动态物体，同时在这样的动态环境中鲁棒地估计传感器位姿。有了稠密SLAM领域的极大进步[2],[14](KinectFusion,ElasticFusion)，近年来跟踪物体的不同方法也被纳入进来。[6](Co-Fusion)的工作中，使用运动残差跟踪环境中移动的物体，[7](MaskFusion)中使用实例分割持续地将颜色和几何信息融合到每个物体独立的地图中。上述工作的物体地图表示是基于ElasticFusion[14]中构建的表面，而表面不能提供未占用的空间信息以辅助机器人应用。相反，MID-Fusion[8]利用高效存储的八叉树表示SDF，并且进一步对检测到的物体进行语义融合。在提高这些系统的稳健性和准确性方面已经取得了重大进展。EM-Fusion[15]提出直接将物体SDF与输入帧对齐来估计物体位姿。另一方面，DetectFusion[16]，将2D物体检测和3D几何分割结合起来，以检测和分割语义上未知的物体的运动。
  与视觉静态SLAM类似，而不是想大多数稠密SLAM系统那样交替优化跟踪和建图（如[17](DSO)中所解释的那样），另一个方向是在地图和位姿制定联合概率推断，以获得更高的物体跟踪精度[18]，但需要牺牲稠密地图表示。DynaSLAM-II[9]使用稀疏点云表示物体，并且在一个位姿图优化中联合优化相机位姿、物体位姿以及几何。ClusterSLAM[19]将物体检测和跟踪定义为路标运动的聚类，并将其作为批优化问题来解决。在此之后，他们将其重新制定为在线VO SLAM，同时考虑语义检测[20](ClusterVO)。
  以前的工作已经探索了使用一个运动模型来跟踪物体，如[7],[8](MaskFusion,MID-Fusion)，例如，使用一个零速度模型跟踪物体，[9](DynaSLAM-II)中的工作使用了恒定速度模型，而[20],[21](ClusterVO,State Estimation for Robotics)使用了加速时的白噪声先验。然而，这些运动假设可能不足以描述任何物体的真实运动，特别是当物体在相机视图中消失再重新出现的时候。在这项工作中，我们放弃了对运动先验的需求，并将基于关键帧的特征匹配和稠密残差验证阶段相结合，制定了物体重定位管线。

2.2 视觉惯性里程计/slam

  近年来，通过融合IMU和相机数据来估计机器人位姿越来越受欢迎，因为它在许多机器人应用中被证明是稳健和准确的。VIO/VI-SLAM通常有两种方法：松耦合系统和紧耦合系统，早期的VIO系统通过IMU和相机观测独立地估计机器人位姿，然后后期融合在一起，这种松耦合方法能够降低计算复杂性，如[22]。然而，随着处理能力的提高，最近最先进的VIO系统采样紧耦合方法，通过考虑所有状态之间的所有相关性联合优化所有状态，同时利用IMU和相机观测，如[23],[24](LSVO,MSCKF)。
  另一种对视觉惯性估计问题的分类策略是评估其是使用了递归滤波还是批处理非线性优化的方法。递归滤波方法从视觉数据更新状态，并且只使用IMU测量进行状态传播。MSCKF[23]是第一个介绍基于紧耦合滤波的VIO工作。ROVIO[25]同样采用了紧耦合的滤波方法，但是使用了直接光度残差，而不是间接的特征关联。基于非线性优化的方法联合优化视觉误差项和积分后的IMU观测。通过边缘化操作，OKVIS[24]将优化限制在有限的关键帧滑动窗口，在保持试试速度的同时实现了高精度。OKVIS2[26]进一步进行了扩展，从边缘化的观测构建位姿图因子并且进行了回环闭合。ORB-SLAMIII[27]也是一种最近的实时VI-SLAM系统，然而，在修复旧状态时，它会降低过去估计的不确定性。
  这些稀疏VIO/VI-SLAM系统在各种环境中显示出稳健和准确的估计。然而，重建的地图过于稀疏，不利于安全的机器人导航和有意义的场景理解。一些研究也着眼于进行视觉-惯性稠密建图。VI-ElasticFusion[28]扩展了ElasticFusion[14]，提出了一个能够进行地图变形的稠密RGB-D-惯性SLAM系统。Kimera[29]还使用VIO前端创建了稠密的网格重建，并提供了带有回环矫正的位姿图优化后端。它还可以为场景理解提供语义注释的地图。
  即使稠密的VIO/VI-SLAM系统可以稠密地重建全局场景，但它们通常缺乏对场景中物体的感知。许多研究已经对视觉惯性物体级SLAM进行了研究。使用惯性测量、尺度和重力方向变得可以观测。视觉惯性物体级SLAM可以同时为视觉识别[30]和语义建图[31]提供尺度识别和全局定位。通过将物体姿态和形状包含到紧密耦合滤波VIO系统[32]中，还可以进一步改进传感器姿态估计。
  然而，大多数现有的VIO/VI-SLAM系统只针对静态环境，尽管IMU提供了传感器自我运动的可靠测量。移动的物体被仔细地从估计的状态中排除。最近的一些工作将其扩展到动态场景中，但物体主要由稀疏的3D地标([33],[34])组成。相比之下，本篇工作以紧耦合的非线性优化方式将IMU与RGB-D传感器测量融合在一起，即使在极端动态场景下，也能提供鲁棒的传感器跟踪，同时密集跟踪和重建场景中每个检测到的物体。我们进一步提供了物体重定位步骤来管理重新访问的移动物体。

3. 系统概览

  VI-MID系统在基于八叉树的物体级多实例动态SLAM系统MID-Fusion基础上构建，管线如图2所示，包含跟踪、分割、融合和光线投射四个主要部分，相机位姿通过光度、几何、IMU残差联合优化，在系统运行过程中，动态物体被检测和跟踪，并且通过光度和几何残差细化它们的位姿。最后的融合、光线投射部分与MID-Fusion相同。

4. 符号定义

  论文中将使用三种不同的坐标系：
    $\bullet$ ${\stackrel{\mathcal{F}}{\to }}_W$，世界坐标系，与重力方向对齐的第一帧相机位置。
    $\bullet$ ${\stackrel{\mathcal{F}}{\to }}_C$，RGB-D被观测的相机坐标系。
    $\bullet$ ${\stackrel{\mathcal{F}}{\to }}_S$，IMU数据被观测的传感器坐标系。
  相机和传感器坐标系之间的外部偏移是事先校准的。相机和IMU之间的时间偏移被认为是同步的，可以忽略不计。

5. 跟踪模块

5.1 状态

  每次收到一个新的RGB-D测量，跟踪模块在世界坐标系估计RGB相机的位置 ${}_W{\mathbf{r}}_C\in {\mathbb{R}}^3$ 、朝向 ${\mathbf{q}}_{WC}\in S^3$，传感器坐标系下的IMU速度 ${}_S{\mathbf{v}}_{WS}\in {\mathbb{R}}^3$以及陀螺仪和加速度计的漂移 ${\mathbf{b}}_g\in {\mathbb{R}}^3，{\mathbf{b}}_a\in {\mathbb{R}}^3$。由此组成了特定时刻的系统状态向量 $\mathbf{x}$：
$$\mathbf{x}:={[}_W{\mathbf{r}}_C^T,{\mathbf{q}}_{WC}^T{,}_S{\mathbf{v}}_{WS}^T,{\mathbf{b}}_g^T,{\mathbf{b}}_a^T{]}^T\in {\mathbb{R}}^3\times S^3\times {\mathbb{R}}^3$$
…因此，局部最小坐标表示为：
$$\delta \mathbf{x}=[\delta {\mathbf{r}}^T,\delta {\alpha }^T,\delta {\mathbf{v}}^T,\delta {\mathbf{b}}_g^T,\delta {\mathbf{b}}_a^T{]}^T\in {\mathbb{R}}^{15}$$
  类似地，我们也定义了一个群操作 $\boxminus$ 作为 $⊞$ 操作的逆操作。进一步的细节和更详细的微分处理可以在[24]和[25]中找到。

5.2 RGB-D-Inertial 传感器跟踪

  跟踪问题旨在估计参考状态 ${\mathbf{x}}_R$ 和活动状态 ${\mathbf{x}}_L$ 。这可以通过最小化代价函数 $E_{track}$ 的方式来解决。代价函数包括四项：光度误差(RGB)$E_{\text{photo}}$，稠密点-面ICP误差(Depth)$E_{\text{ICP}}$，惯性误差(IMU)$E_{\text{inertial}}$以及非线性优化框架中的边缘化先验 $E_{\text{prior}}$。图3展示了这个跟踪代价函数的因子图，并且与MID-Fusion[8]进行了比较。每个误差项包含残差之和 $e$，根据各自的测量不确定性 $w$ 加权（边缘化除外），边缘化先验中作者使用了与[28]相同的边缘化方案，${\mathbf{H}}^{\ast }$ 和 ${\mathbf{b}}^{\ast }$ 都是通过边缘化参考状态 ${\mathbf{x}}_R$ 得到的先验。
$$\begin{array}{rl}& E_{\text{track}}({\mathbf{x}}_R,{\mathbf{x}}_L)=E_{\text{photo}}+E_{\text{ICP}}+E_{\text{inertial}}+E_{\text{priors}}\\ & E_{\text{photo}}=\sum _{u_R\in M_R}{w}_p\rho (e_p)\\ & E_{\text{ICP}}=\sum _{u_L\in M_L}{w}_g\rho (e_g)\\ & E_{\text{inertial}}=w_s\rho (\Vert {\mathbf{e}}_s\Vert )\\ & E_{\text{priors}}=\frac{1}{2}({\mathbf{x}}_R\boxminus {\bar{\mathbf{x}}}_R-{\mathbf{H}}^{\ast -1}{\mathbf{b}}^{\ast }{)}^T{\mathbf{H}}^{\ast }({\mathbf{x}}_R\boxminus {\bar{\mathbf{x}}}_R-{\mathbf{H}}^{\ast -1}{\mathbf{b}}^{\ast })\end{array}$$
  其中 $\rho (\cdot )$是一个鲁棒核函数（使用了柯西损失函数），另外 $M_R,M_L$ 是掩膜，用于排除无效对应、遮挡环境和动态物体导致的RGB和深度数据的无效测量。

（1）稠密光度残差：光度残差的目的是最小化参考帧 $I_R$ 和 活动帧 $I_L$ 中观测到的参考像素 ${\mathbf{u}}_R$ 的强度差，其定义如下：
$$e_p(T_{W{C}_L})=I_R[{\mathbf{u}}_R]-I_L[\pi (T_{W{C}_L}^{-1}{T}_{W{C}_R}{\pi }^{-1}({\mathbf{u}}_R,D_R[{\mathbf{u}}_R]))]$$
  其中 $\pi (\cdot )$ 是将相机坐标系中的一个3D点投影到图像平面上的函数，$\pi (\cdot {)}^{-1}$则执行相反的操作，将图像点反投影回3D点。$T_{W{C}_L},T_{W{C}_R}$是RGB相机活动帧与参考帧的相对位姿，$D_R$ 是被渲染出的参考深度图。这与使用最新深度测量的标准光度残差形成了直接对比，当原始输入深度不可用时，这个决定增加了跟踪的鲁棒性，例如相机太接近墙壁或表面时。

（2）ICP残差：除了光度误差，使用[2]中提出的方法，ICP残差通过最小化深度图和渲染的参考深度图之间的点平面深度残差，在RGB-D测量之间增加了额外的几何约束：
$$e_g(T_{W{C}_L})={}_W{\mathbf{n}}^r[{\mathbf{u}}_R]\cdot (T_{W{C}_L}{}_{C_L}\mathbf{v}[{\mathbf{u}}_L]-{}_W{\mathbf{v}}^r[{\mathbf{u}}_R])$$
  其中 ${}_{C_L}\mathbf{v}$ 是通过反向投影在相机坐标中的实时顶点地图，${}_W{\mathbf{v}}^r$ 和 ${}_W{\mathbf{n}}^r$ 是世界坐标系中渲染出的顶点地图和法方向地图。对于活动坐标系中的每个像素 ${}_L\mathbf{u}$，它在参考坐标系中对应的像素 ${\mathbf{u}}_R$ 可以通过投影和重投影计算出来。
$${\mathbf{u}}_R=\pi (T_{W{C}_R}^{-1}{T}_{W{C}_L}{\pi }^{-1}({\mathbf{u}}_R,D_R[{\mathbf{u}}_R]))$$

（3）测量不确定性权重：为了公平地权衡RGB和深度传感器对于RGB相机位姿 $T_{WC}$ 的贡献，需要对获得每个传感器测量值的不确定度。对于RGB相机，假定每个像素的测量不确定度是统一且固定的。对于深度传感器，深度测量值的质量与RGB-D传感器的结构和深度范围有关。使用 [28] 中深度测量不确定度的逆协方差定义，给定基线 $b$，视差 $d$，焦距 $f$以及 x-y 平面 ${\sigma }_{xy}$ 和视差方向 ${\sigma }_z$，$x,y,z$ 坐标上的深度测量的标准差 ${\sigma }_D$ 可以建模为：
$${\sigma }_D(\frac{D_L[{\mathbf{u}}_L]}{f}{\sigma }_{xy},\frac{D_L[{\mathbf{u}}_L]}{f}{\sigma }_{xy},\frac{D_L^2[{\mathbf{u}}_L]}{fb}{\sigma }_z)$$
  因此，使用测量不确定度逆协方差的ICP残差的权重为：
$$w_g=\frac{1}{({}_W{\mathbf{n}}^r{)}^T{}_W{\mathbf{n}}^r{\sigma }_D^T{\sigma }_D}$$

（4）惯性残差：为了使我们的系统对视觉或深度数据具有鲁棒性，我们采用[28]中的方法，并于惯性项中在参考帧与实时帧之间对IMU测量值进行了数值预积分，以估计参考帧和实时帧的RGB相机姿态，以及速度和IMU偏差。惯性残差如下：
$${\mathbf{e}}_S={\hat{\mathbf{x}}}_L({\mathbf{x}}_R)\boxminus {\mathbf{x}}_L$$
  其中 ${\mathbf{x}}_R$ 是参考状态，${\hat{\mathbf{x}}}_L$ 是从参考状态通过IMU测量传播预测的实时状态。

5.3 物体跟踪

  为了处理环境中的物体，我们采用了MID-Fusion [8] 中的方法，在实时坐标系 $T_{C_L{O}_i}$ 中估计第i个物体 $O_i$ 和RGB相机位姿 $C$ 的相对位姿。这一部分计算是与传感器跟踪分离开的。首先，物体掩膜使用Mask R-CNN提取，紧接着进行几何和运动的细化。其次，与之关联的光度和ICP残差项都被参数化到以物体为中心的形式，以估计 $T_{C_L{O}_i}$。与物体关联的ICP残差被修改，以使活动物体帧中的活动顶点图与参考物体帧中的渲染顶点图对齐：
$$e_g({\mathbf{T}}_{C_L{O}_i})={\mathbf{C}}_{W{O}_i}^{-1}{}_W{\mathbf{n}}^r[{\mathbf{u}}_R]\cdot (T_{C_L{O}_i}^{-1}{}_{C_L}{\mathbf{v}[\mathbf{u}]}_L-T_{W{O}_i}^{-1}{}_W{\mathbf{v}}^r[{\mathbf{u}}_R])$$
  类似地，光度残差被重新参数化为：
$$e_p({\mathbf{T}}_{C_L{O}_i})=I_R[{\mathbf{u}}_R]-I_L[\pi ({\mathbf{u}}_L)]$$
  其中 ${\mathbf{u}}_L$ 现在是
$${\mathbf{u}}_L=T_{C_L{O}_i}{T}_{C_R{O}_i}^{-1}{\pi }^{-1}({\mathbf{u}}_R,D_R[{\mathbf{u}}_R])$$
  然后使用Causs-Newton算法对上述以物体为中心的残差进行三层从粗到细的优化。

5.4 物体重定位

  在许多实际应用中，环境中的物体并不总是静态的，RGB-D传感器也不总是可以观察到的，这使得以前丢失的物体在稍后的时间重新观察到具有挑战性。以前的工作并没有解决这个问题。在这项工作中，作者提出了一种基于关键帧的物体重定位方法来处理这种情况。
  与MID-Fusion类似，作者进行物体层面的光线投射，将物体检测结果与已有模型关联起来。对于每个不匹配的物体，我们初始化一个基于八叉树的体素表示的[12]新物体模型。它的物体坐标w.e.t.世界坐标系 $T_{W{O}_i}$ 被设置为单位旋转矩阵和根据深度测量 $D_L$ 得到的平移量。当一个物体与一个已经存在的物体匹配的IoU超过0.8时，我们就将这些颜色、深度和语义概率集成到模型之中。与物体相关联的物体关键帧也会被创建，带有被检测到的BRISK特征点[36]。新的关键帧只在视角变化大于 $10°$ 时才被添加。每一个物体关键帧都包含了相机到物体的相对位姿 $T_{C{O}_i}$、图像关键点 $\mathbf{z}\in {\mathbb{R}}^2$、特征描述符和相机坐标系下的与每个关键点关联的物体点 ${}_C\mathbf{p}\in {\mathbb{R}}^3$ 。
  一个物体离开相机的视场被标记为丢失，并且在每个后续帧中进行重定位。为了节省计算资源，重定位管线仅在物体中心不接近图像边缘的每个不匹配检测时触发，这以重定位阈值 $R(O_i)$ 的形式实现，并定义为：
$$R(O_i)=\frac{S_{\text{current}}(O_i)}{S_{\text{max}}(O_i)}$$
  其中 $S_{\text{current}}(O_i)$ 是当前物体掩膜的大小，$S_{\text{max}}(O_i)$ 是最大物体掩膜大小，我们发现 $R(O_i)>0.7$ 的结果最好。
  当一个丢失的物体被重新观测到且上面的重定位阈值满足要求时，重定位管线如下（如图4所示）：
  1）通过以前的物体关键帧获取物体关键点和特征描述子，以进行迭代，
  2）对物体原有与当前帧新检测到的特征点进行匹配，
  3）找到所有物体关键帧中的最佳匹配，
  4）在最匹配的特征点上进行Pnp求解，以获取被检测物体的相对位姿 $T_{C_R{C}_L}$ ，再用以获得 $T_{C_L{O}_i}$，
  5）物体位姿 $T_{W{O}_i}$ 使用公式(14)(15)进一步细化。如果残差太大，物体匹配结果会视为无效，
  6）一旦物体位姿被恢复，我们将删除新的重复物体模型并检索先前重建的物体模型。

6. 实验结果

6.1 实验设置

6.2 跟踪性能评估

6.3 物体重建

6.4 物体重定位

6.5 运行时间分析

6.6 讨论

7. 总结

  作者介绍了一种能够处理多动态物体的视觉惯性稠密SLAM系统，主要优点：IMU增强了跟踪鲁棒性，减轻Mask R-CNN间歇物体检测的影响（依靠从物体地图获得的渲染掩码持续跟踪环境中的物体）。