论文阅读：带有物体级重定位的视觉惯性多实例动态SLAM

带有物体级重定位的视觉惯性多实例动态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图，第二行-语义融合，第三行-几何重建

主要的贡献有三个方面：

• 第一个紧耦合稠密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相同。

图2：VI-MID系统管线

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]中找到。

图3：对比MID-Fusion和VI-MID系统中状态变量和观测量的因子图：结合惯性测量引入时间约束，通过速度和IMU偏差增强状态。

5.2 RGB-D-Inertial 传感器跟踪

  跟踪问题旨在估计参考状态 ${\mathbf{x}}_R$ 和活动状态 ${\mathbf{x}}_L$ 。这可以通过最小化代价函数 $E_{track}$ 的方式来解决。代价函数包括四项：光度误差(RGB)$E_{\text{photo}}$，稠密点-面ICP误差(Depth)$E_{\text{ICP}}$，惯性误差(IMU)$E_{}$