DP-SLAM

Posted on 2022-11-29 Edited on 2022-12-13 In Paper Note Waline:

关于论文 DP-SLAM: A visual SLAM with moving probability towards dynamic environments (Information Sciences, 2021) 的阅读总结。

DP-SLAM

论文情况

标题：DP-SLAM: A visual SLAM with moving probability towards dynamic environments
作者：Ao Li, Jikai Wang, Meng Xu, Zonghai Chen
期刊：Information Sciences, 2021
源码：未开源

1 Introduction

动态物体对传统静态假设的 SLAM 系统的影响：

运动物体容易使系统得到错误的数据关联结果，破坏视觉里程测量；
动态物体可能导致错误的位置识别，回环检测的性能也会下降。

前端引入目标检测能力产生的问题：

RANSAC 用于在前端拒绝 outliers，但当移动物体占据图像的大部分时，算法往往会失败；
引入 CNN 进行检测，但由于训练一般是基于公共数据集，所以检测能力有限，可能会将动态点误分类为静态。

本文提出了一种将几何模型和深度学习算法相结合的动态关键点的移动概率传播模型，将运动关键点拒绝定义为运动去除（motion removal）。主要贡献为：

提出了一种新的运动移除方法，实时传播每个关键点的运动概率，克服了几何约束和语义信息的偏差；
对背景进行静态信息填充，得到无动态内容的合成 RGB 帧及对应的深度帧，有利于 VR 应用；
使用 TUM 数据集和 ORB-SLAM2 进行对比。

2 DP-SLAM

2.1 The Approach Overview

Fig.1 为算法的流程图，在预处理阶段，过滤掉移动对象的相关数据，从而减少 SLAM 前端不正确数据的关联。

如 Fig.2，本文的方法结合几何模型和语义分割来去除运动，其结果转化为观测概率。前一帧关键点的移动概率作为先验概率，基于贝叶斯定理，可以根据观测概率和先验概率更新各个关键点的移动概率。然后剔除移动概率高（ $>0.5$ ）的关键点。

2.2 Semantic Segmentation

为实现动态物体的检测，使用 Mask R-CNN[1] 进行目标分割以及像素级的语义分割。使用 Mask R-CNN 识别的动态物体包括人、自行车、汽车、摩托车等，如果需要识别其他物体可以添加训练数据进行模型调整。Mask R-CNN 输入原始 RGB 帧，输出原始帧中每个对象的二进制掩码。

CNN 可能没有精确的分割动态对象（尤其在对象边缘），导致掩码覆盖了静态区域。为此，使用二项逻辑回归计算每个关键点的动态概率，如 Fig.3，掩码中动态概率异常（ $<0.75$ ）的关键点则认为可能位于静态背景上。

具体地，取 $s^t_{\bold{p}_i}$ 为关键点 $\bold{p}_i$ 在时间 $t$ 由语义分割得到的标签，定义为：

$s_{\bold{p}_i}^t = \begin{cases} 1, &\text{if } \bold{p}_i \text{ is determined as dynamic} \\ 0, &\text{otherwise} \end{cases} \tag{1}$

取 $s_t$ 为关键点标签的集合（ $n$ 为关键点的数量）：

$s_t = \{ s_{\bold{p}_1}^t, s_{\bold{p}_2}^t, ..., s_{\bold{p}_n}^t \} \tag{2}$

$t$ 时刻的边界像素为：

$z_t = \{ \bold{b}_1^t, \bold{b}_2^t, ..., \bold{b}_m^t \} \tag{3}$

$b_m^t$ 表示动态物体分割区域的边界点， $m$ 为边界点的数量。所有的边界点都包含在 $z_t$ 中。

关键点 $p_i$ 和分割区域的边界的距离定义为：

$dist(\bold{p}_i, z_t) = \min_{b_j^t \in z_t} \| \bold{p}_i - \bold{b}_j^t \|_2 \tag{4}$

二项逻辑回归模型用于估计 $\bold{p}_i$ 的动态概率（ $\alpha (=0.1)$ 为平滑因子）：

$P(s_{\bold{p}_i}^t | s_t) = \frac{1}{\exp (-\alpha \cdot dist(\bold{p}_i, z_t)) + 1} \tag{5}$

动态概率是指被分割掩码中的关键点与边界边缘的距离越近，误分类的概率越高。因此，如果掩码中的 $\bold{p}_i$ 具有异常的低概率（ $< 0.75$ ），则它更有可能是一个静态关键点，尽管它位于掩码内。

2.3 Epipolar Geometry Constraint

由于 Mask R-CNN 是使用 COCO 数据集训练的，只能确定大约 80 个类别的对象，因此仍然存在一些无法通过语义分割检测到的移动对象。为了解决这一问题，在两个连续帧之间使用对极几何约束来进一步确定动态关键点：

运动物体的匹配关键点不满足对极几何约束，因为这些关键点并不严格地位于相应的极线上。因此，计算匹配关键点到极线的距离，如果距离大于某一阈值，则将匹配点定义为异常值。

Fig.4 表示前一帧 $I_1$ 和当前帧 $I_2$ 之间的对极几何约束：

设 $\bold{p}_i^{\prime}$ 和 $\bold{p}_i$ 分别表示前一帧和当前帧中匹配的关键点对（ $x, y$ 为匹配关键点的像素坐标）：

$\bold{p}_i^{\prime} = [x_{\bold{p}_i^{\prime}}, y_{\bold{p}_i^{\prime}}, 1], \quad \bold{p}_i = [x_{\bold{p}_i}, y_{\bold{p}_i}, 1] \tag{6}$

极线 $\bold{l}_{\bold{p}_i}$ 定义为（ $X, Y, Z$ 表示极线向量， $\bold{F}$ 为基础矩阵）：

$\bold{l}_{\bold{p}_i} = \left[ \begin{matrix} X_{\bold{p}_i^{\prime}} \\ Y_{\bold{p}_i^{\prime}} \\ Z_{\bold{p}_i^{\prime}} \end{matrix} \right] = \bold{F} (\bold{p}_i^{\prime})^T = \bold{F} \left[ \begin{matrix} x_{\bold{p}_i^{\prime}} \\ y_{\bold{p}_i^{\prime}} \\ 1 \end{matrix} \right] \tag{7}$

从匹配点 $\bold{p}_i$ 到极线 $\bold{l}_{\bold{p}_i}$ 的距离定义为：

$d(\bold{p}_i, \bold{l}_{\bold{p}_i}) = \frac{|\bold{p}_i \bold{F} \bold{p}_i^{\prime}|}{\sqrt{\| X_{\bold{p}_i^{\prime}} \|^2 + \| Y_{\bold{p}_i^{\prime}} \|^2}} \tag{8}$

拒绝动态 outliers 的步骤如下：

通过计算当前帧的光流金字塔得到匹配关键点。如果匹配点太接近图像的边缘，或者以匹配点为中心的 $3 \times 3$ 图像块的像素差太大，则删除匹配的关键点对。
使用 RANSAC 算法求得基础矩阵，再计算匹配点到极线的距离，当距离大于一定阈值时（ $>0.75$ ），将匹配点视为 outliers。
通过几何方法确定了动态关键点。

算法如下：

取 $c_{\bold{p}_i}^t$ 为 $t$ 时刻几何方法确定的关键点 $\bold{p}_i$ 的标签， $S$ 为动态点集：

$c_{\bold{p}_i}^t = \begin{cases} 1, &\text{if } \bold{p}_i \in S \\ 0, &\text{otherwise} \end{cases} \tag{9}$

同样，取 $c_t$ 为 $t$ 时刻的关键点标签集， $n$ 为关键点数：

$c_t = \{ c_{\bold{p}_1}^t, c_{\bold{p}_2}^t, ..., c_{\bold{p}_n}^t \} \tag{10}$

由于运动物体的存在，图像中的每个关键点没有严格约束在其对应的极线上：

距离越远，动态概率越大。

因此，假定匹配关键点到对应极线的距离满足高斯分布，使用正态分布概率密度函数来计算关键点的动态概率：

$P(c_{\bold{p}_i}^t | c_t) = \frac{1}{\sqrt{2 \pi} \delta} \exp \left( - \frac{d(\bold{p}_i, \bold{l}_{\bold{p}_i})^2}{2 \delta^2} \right) \tag{11}$

$\delta (=1)$ 为标准差，且分布的期望值为 0。

2.4 Iteratively Moving Probability Update

传统的几何或语义 VIO 只是在单一帧中消除动态点。但是，跟踪动态对象可以大大提高定位性能。本文的方法考虑的视觉里程计的动态离群值拒绝问题远不止一帧：

本文提出了一种动态概率传播算法，该算法结合了几何模型和基于深度学习的算法来去除图像中的动态目标。基于贝叶斯定理，利用过去和当前的信息来评估和消除动态关键点。

定义匹配点 $\bold{p}_i$ 在时刻 $t$ 的真实状态为 $D_t(\bold{p}_i)$ ：

如果 $\bold{p}_i$ 在动态物体的区域内，则定义该点为动态点，即有 $D_t(\bold{p}_i) = 1$ ；否则 $D_t(\bold{p}_i)=0$ 。

由几何模型得到的动态概率 $P(D_t(\bold{p}_i) | c_{\bold{p}_i}^t)$ 和由语义分割得到的动态概率 $P(D_t(\bold{p}_i) | s_{\bold{p}_i}^t)$ 可以联合为：

$\begin{aligned} P(D_t(\bold{p}_i) = 1 | c_{\bold{p}_i}^t, s_{\bold{p}_i}^t) &= \omega P(D_t(\bold{p}_i) = 1 | c_{\bold{p}_i}^t) \\ &+ (1 - \omega) P(D_t(\bold{p}_i) = 1 | s_{\bold{p}_i}^t) \end{aligned} \tag{12}$

$\begin{aligned} P(D_t(\bold{p}_i) = 0 | c_{\bold{p}_i}^t, s_{\bold{p}_i}^t) &= \omega P(D_t(\bold{p}_i) = 0 | c_{\bold{p}_i}^t) \\ &+ (1 - \omega) P(D_t(\bold{p}_i) = 0 | s_{\bold{p}_i}^t) \end{aligned} \tag{13}$

$\omega$ 定义为：

$\omega = \frac{N_c}{N_c + N_s} \tag{14}$

$N_c$ 为当前帧被几何模型所拒绝的 outliers 的数量， $N_s$ 为当前帧被语义分割拒绝的 outliers 的数量（可以将 outliers 理解为相应方法产生的动态关键点）。

假设动态概率的传播具有马尔可夫性，基于贝叶斯理论，关键点的动态概率传播模型可以定义为：

$\begin{aligned} P(D_t(\bold{p}_i) | c_{\bold{p}_i}^t, s_{\bold{p}_i}^t) &= \eta P(c_{\bold{p}_i}^t, s_{\bold{p}_i}^t | D_t(\bold{p}_i)) \\ &\cdot \int P(D_t(\bold{p}_i) | D_{t-1}(\bold{p}_i)) \cdot P(D_{t-1}(\bold{p}_i)) d D_{t-1}(\bold{p}_i) \end{aligned} \tag{15}$

关键点在前一帧中的动态概率被视为先验概率， $\eta$ 为规范化影响因子。观测概率 $P(c_{\bold{p}_i}^t, s_{\bold{p}_i}^t | D_t(\bold{p}_i))$ 表述为：

$\begin{aligned} P(c_{\bold{p}_i}^t, s_{\bold{p}_i}^t | D_t(\bold{p}_i)) &= P(D_t(\bold{p}_i) = 1) \cdot P(c_{\bold{p}_i}^t, s_{\bold{p}_i}^t | D_t(\bold{p}_i) = 1) \\ &+ P(D_t(\bold{p}_i) = 0) \cdot P(c_{\bold{p}_i}^t, s_{\bold{p}_i}^t | D_t(\bold{p}_i) = 0) \end{aligned} \tag{16}$

其中有：

$\begin{aligned} P(c_{\bold{p}_i}^t, s_{\bold{p}_i}^t | D_t(\bold{p}_i) = 0) &= \frac{P(c_{\bold{p}_i}^t, s_{\bold{p}_i}^t, D_t(\bold{p}_i) = 0)}{P(D_t(\bold{p}_i) = 0)} \\ &= \frac{P(c_{\bold{p}_i}^t, s_{\bold{p}_i}^t) P(D_t(\bold{p}_i) = 0 | c_{\bold{p}_i}^t, s_{\bold{p}_i}^t)}{P(D_t(\bold{p}_i) = 0)} \end{aligned} \tag{17}$

$\begin{aligned} P(c_{\bold{p}_i}^t, s_{\bold{p}_i}^t | D_t(\bold{p}_i) = 1) &= \frac{P(c_{\bold{p}_i}^t, s_{\bold{p}_i}^t, D_t(\bold{p}_i) = 1)}{P(D_t(\bold{p}_i) = 1)} \\ &= \frac{P(c_{\bold{p}_i}^t, s_{\bold{p}_i}^t) P(D_t(\bold{p}_i) = 1 | c_{\bold{p}_i}^t, s_{\bold{p}_i}^t)}{P(D_t(\bold{p}_i) = 1)} \end{aligned} \tag{18}$

整个算法如下，由算法 2 所确定的 $U$ 里的每一个关键点都认为是 outliers，需要从当前帧中移除：

根据 (5) 和 (11)，可以获得语义分割动态概率 $P(s_{\bold{p}_i}^t | s_t)$ 和几何模型动态概率 $P(c_{\bold{p}_i}^t | c_t)$ ，从而使用 (12-13) 和 (16-18) 可以求得观测概率 $P(c_{\bold{p}_i}^t, s_{\bold{p}_i}^t | D_t(\bold{p}_i))$ 。再由 (15) 可以更新动态概率 $P(D_t(\bold{p}_i) | c_{\bold{p}_i}^t, s_{\bold{p}_i}^t)$ 。

文中没有明确说明，按照各符号的含义及表达式，认为：
$\begin{aligned} \eta &= 1 / P(c_{\bold{p}_i}^t, s_{\bold{p}_i}^t) \\ P(c_{\bold{p}_i}^t, s_{\bold{p}_i}^t) &= P(c_{\bold{p}_i}^t | c_t) \cdot P(s_{\bold{p}_i}^t | s_t) \\ P(D_t(\bold{p}_i) = 1 | c_{\bold{p}_i}^t) &= P(c_{\bold{p}_i}^t | c_t) \\ P(D_t(\bold{p}_i) = 0 | c_{\bold{p}_i}^t) &= 1 - P(c_{\bold{p}_i}^t | c_t) \\ P(D_t(\bold{p}_i) = 1 | s_{\bold{p}_i}^t) &= P(s_{\bold{p}_i}^t | s_t) \\ P(D_t(\bold{p}_i) = 0 | s_{\bold{p}_i}^t) &= 1 - P(s_{\bold{p}_i}^t | s_t) \\ P(D_{t-1}(\bold{p}_i)) &= P(D_{t-1}(\bold{p}_i) | c_{\bold{p}_i}^{t-1}, s_{\bold{p}_i}^{t-1}) \end{aligned} \tag{批注}$