dynamicVINS论文阅读笔记

Abstract

  本文提出了一种用于动态环境中资源受限机器人的实时 RGB-D 惯性里程计系统，称为 Dynamic-VINS。三个主要线程并行运行：目标检测、特征跟踪和状态优化。

  所提出的 Dynamic-VINS 结合了目标检测和深度信息以进行动态特征识别，并实现了与语义分割相当的性能。 Dynamic-VINS采用基于网格的特征检测，提出了一种快速高效的方法来提取高质量的FAST特征点。 IMU 用于预测运动以进行特征跟踪和运动一致性检查。所提出的方法在公共数据集和实际应用程序上进行了评估，并在动态环境中显示出具有竞争力的定位精度和稳健性。然而，据我们所知，它是目前动态环境中资源受限平台的最佳性能实时 RGB-D 惯性里程计。所提出的系统是开源的：https://github.com/HITSZ-NRSL/Dynamic-VINS.git

Introduction

本文的主要贡献如下：

1. 提出了一种有效的基于优化的 RGB-D 惯性里程计，为动态和复杂环境中资源受限的机器人提供实时状态估计结果。
2. 提出了轻量级的特征检测和跟踪以减轻计算负担。此外，提出了结合目标检测和深度信息的动态特征识别模块，以在复杂和室外环境中提供鲁棒的动态特征识别。
3. 进行验证实验以显示所提出的系统在动态环境中资源受限平台上具有竞争力的准确性、鲁棒性和效率。

System Overview

Methodology

  本研究提出了轻量级、高质量的特征跟踪和检测方法来加速系统。从RGB-D 图像和 IMU 预积分提取的语义和几何信息被应用于动态特征识别和移动一致性检查。漏检补偿模块在漏检情况下对目标检测起到辅助作用。通过移动一致性检查进一步识别未知对象的动态特征。所提出的方法分为五个部分进行详细描述。

Feature Matching

  特征跟踪使用KLT稀疏光流法。IMU测量用来预测特征的运动，更好的特征初始位置估计可以减少光流金字塔的层数，以此来提高特征跟踪的效率。而且通过这种方法，可以有效地丢弃不稳定的特征，例如噪声和运动不一致的动态特征。基本思想如下图所示：

  上述手段可以获得均匀分布的特征，捕捉综合约束，避免在模糊或纹理较弱的区域重复提取不稳定的特征。长期特征跟踪可以借助下面基于网格的特征检测来减少时间消耗。

Grid-Based Feature Detection

  系统需要保持最少数量的特征以确保稳定性。因此，需要不断地从帧中提取特征点。本研究采用基于网格的特征检测。将图像划分为网格，填充每个网格的边界，防止网格边缘的特征被忽略；padding 使当前网格能够获取相邻像素信息以进行特征检测。与遍历整幅图像进行特征检测不同，只有特征匹配不足的格子才会进行特征检测。对于纹理弱而未能检测到特征的网格或者被掩码覆盖的网格将在下一个检测帧中被跳过，以避免重复的无用检测。线程池技术用于开发基于网格的特征检测的并行性能。因此，特征检测的时间消耗大大减少而没有损失。

  FAST特征检测器可以有效地提取特征点，但很容易将噪声视为特征并提取相似的聚类特征。因此，结合上一节特征匹配中的mask和非极大值抑制的思想来选择高质量和均匀分布的FAST特征。

Dynamic Feature Recognition

  通过上述改进，大部分特征点可以稳定跟踪。然而，动态对象的长期跟踪特征总是伴随着异常运动并且对系统引入错误的约束。为了提高效率和降低计算成本，采用了一种实时单级目标检测方法YOLOv3对人、车等多种动态场景元素进行检测。如果检测到的边界框覆盖了图像的较大区域，盲目删除边界框中的特征点可能会导致没有可用的特征来提供约束。因此，类语义分割掩码有助于跟踪不被动态对象遮挡的特征，从而维持系统的运行。
  本文将目标检测与深度信息相结合，实现高效的动态特征识别，达到与语义分割相当的性能。深度相机测得越远，精度就越差。这一问题使得一些充分利用深度信息的方法，如Seed Filling、DBSCAN、K-Means等，在精度较低的深度摄像机下表现不佳，如图5(a)所示。通过将检测到的bounding box中的一组点与深度信息进行集成，可以得到与语义分割相当的性能，如图3所示。
  一个像素的深度d>0则是可用的。考虑到大多数动态对象的bounding box角都对应于背景点，且动态对象通常与背景有较大的深度差距。则第k个动态对象的最大背景深度$${}^K{d}_{max}=max{(}^K{d}_{tl}{+}^K{d}_{tr}{+}^K{d}_{bl}{+}^K{d}_{br})$$其中${}^K{d}_{tl}{,}^K{d}_{tr}{,}^K{d}_{bl}{,}^K{d}_{br}$分别是第K个目标检查框的四个角点的深度值。接着，第K个bounding box的深度阈值${}^K\bar{d}$定义如下$${}^K\bar{d}=\{\begin{array}{ll}\frac{1}{2}{(}^K{d}_{max}{+}^K{d}_c),& \text{if}{}^K{d}_{max}{-}^K{d}_c>ϵ{,}^K{d}_c>0,\\ {}^K{d}_c+ϵ& \text{if}{}^K{d}_{max}{-}^K{d}_c<ϵ{,}^K{d}_c>0,\\ {}^K{d}_{max}& \text{if}{}^K{d}_{max}>0{,}^K{d}_c=0,\\ +\infty & otherwise,\end{array}$$
其中${}^K{d}_c$是检测框中心点的深度值；$ϵ>0$是根据场景中场景动态物体大小预先设定的。如果深度不可用，则采用保守策略，选择无限深度作为阈值。
  在语义掩码上，第K个动态对象包围框覆盖的区域被设置为加权深度${}^K\bar{d}$;没有动态对象的区域设置为0。每个传入特征的深度d与语义掩码上对应像素的深度阈值$\bar{d}$进行比较。如果$d<\bar{d}$，则认为该特征为动态特征。否则，认为该特征为稳定特征。因此，深度值小于加权深度$\bar{d}$的区域构成广义语义掩码，如图4和图5(b)所示。
  考虑到动态物体可能长期存在于视场中，因此不同于直接删除动态特征，我们对动态特征进行跟踪而不用于姿态估计。根据其记录的信息，从特征跟踪线程传入的每一个特征点将被判断是否为历史动态特征。上述方法可以避免盲目删除特征点，同时保证效率。它可以节省检测动态对象上特征的时间，对对象检测的漏检具有鲁棒性，并回收假阳性动态特征，如III-E节所述。

Missed Detection Compensation

  由于目标检测有时可能失败，本文提出的Dynamic-VINS利用之前的检测结果来预测接下来的检测结果，以弥补遗漏的检测。假设相邻帧中的动态对象具有一致的运动。一旦动态物体被检测到，它的像素速度和包围框将被更新。设j为当前检测帧，j−1为前一检测帧，定义第k个动态对象帧间像素速度${}^K{v}^{c_j}$(像素/帧)为$${}^K{v}^{c_j}{=}^K{u}_c^{c_j}{-}^K{u}_c^{c_{j-1}}$$其中${}^K{u}_c^{c_j}{,}^K{u}_c^{c_{j-1}}$分别表示第k个物体检测包围框中心在第j帧和第j−1帧的像素位置。一个加权的预测速度${}^K\hat{v}$定义为$${}^K{\hat{v}}^{c_j+1}=\frac{1}{2}{(}^K{v}^{c_j}{+}^K{\hat{v}}^{c_j})$$随着更新的进行，越老的帧所拥有的权重预测速度${}^K\hat{v}$的权重越小。如果在下一帧中未检测到该对象，其对应的包围框${}^{K}Box$包含有四个角的像素位置${}^K{u}_{tl}{,}^K{u}_{tr}{,}^K{u}_{bl}{,}^K{u}_{br}$，会基于预测速度${}^K\hat{v}$进行如下更新$${}^K{\widehat{Box}}^{c_j+1}{=}^{K}Bo{x}^{c_j}{+}^K{\hat{v}}^{c_j+1},$$当漏检时间超过阈值时，该动态对象的补偿将被放弃。结果如图4所示。该方法提高了目标检测的召回率，有利于动态特征识别的一致性。

Moving Consistency Check

  由于物体检测只能识别人为定义的动态物体，存在漏检问题，状态优化仍然会受到未知移动物体的影响，比如随着人移动的书籍。Dynamic-VINS将IMU预测的位姿与在滑动窗口中优化后的位姿相结合，进行动态特征的识别。
  假设第k个特征首先在第i帧图像中观察到，并被其他m帧图像在滑动窗口中观察到。则滑动窗口中特征观测的平均重投影残差$r_k$定义为$$r_k=\frac{1}{m}\sum _{j\ne i}\Vert u_k^{c_i}-\pi (T_b^c{T}_w^{b_i}{T}_{b_j}^w{T}_c^b{P}_k^{c_j})\Vert$$式中$u_k^{c_i}$为第i帧中对K个特征的观测;$P_k^{c_j}$为第k个特征在第j帧中的三维坐标;$T_c^b$和$T_{b_j}^w$分别是相机坐标到body坐标和第j个body坐标到世界坐标的变换;π代表相机投影模型。当$r_k$超过预设阈值时，第k个特征被认为是动态特征。
  如图7所示，移动一致性检查(MCC)模块可以发现不稳定特征。然而，一些稳定的特征被错误识别(左上图)，站着的人的特征没有被识别(右下图)。阈值越低，不稳定特征的召回率越高。此外，如果重投影误差低于阈值，则具有更多观测值的错误识别的不稳定特征将被回收。

Experimental Results

略