RGB-D SLAM in Dynamic Environments Using Static Point Weighting论文笔记

论文链接
IEEE ROBOTICS AND AUTOMATION LETTERS
OCTOBER 2017
代码链接

文章主要内容

本文基于帧到关键帧配对的方法，并在仅使用depth edge points（特征点名，不知道如何确切翻译）的情况下，设计了一个性能优异的实时视觉里程计。文章主要目的是减少动态物体的影响，提出了静态质量的概念（用于衡量某点为静态环境一部分的可能性），并将其整合到IAICP（intensity assisted iterative closet point）方法中，极大程度上减小了跟踪误差。

文章指出当前RGB-D视觉里程计的设计方法主要可以分为两类，一类是稠密的视觉里程计，即优化最小能量函数的方法，但其易受动态物体的影响；另一类是基于匹配（当前源和目标之间的匹配）的方法，但是当动态特征点多余静态特征点时，可能会导致对静态特征点错误的位姿估计。

视觉里程计框架

框架如下图所示

$A$ 总览：
对进入里程计的每一帧，首先提取它的foreground edge points，将其用作里程计的后续估计，并选择每次的第N帧作为关键帧。对每一个关键帧（edge point），计算它的静态质量，用来估计其属于静态环境的可能性。然后用IAICP算法估计关键帧到当前帧的相对变化。最终，根据估计的移动来更新关键帧的静态质量。

---

$B$ Foreground Depth Edge Extraction：
depth edge points和它们周围的点之间有较大的深度不连续性。其中的典型代表为foreground edge points（在其他物体前并用来体现物体边界，对移动的相机有很好的稳定性）和occluded edge points（在其他物体后面，由遮挡产生，对相机的移动敏感，在对相机的轨迹估计中需要剔除）。使用depth edge points相对于使用均匀采样点方法，可以获得更多的正确匹配。
记点云为$P_t$，$B_t$包含了foreground edge point的序号，每个点和它周围四个点之间的深度差异 { h i } i = 1 4 \{ h_i \}^4_{i=1} {hi​}i=14​计算公式如下：

$e_Z=(0,0,1,0{)}^T$用来提取一个点的深度值，四个偏移矢量$<o_1,o_2,o_3,o_4>$可以表示为$<(0,b{)}^T,(0,-b{)}^T,(b,0{)}^T,(-b,0{)}^T>$，这里b取4。
为了剔除occulded edge points，筛选出foreground edgepoints，采用下面两个条件：

这里分别设置$t_b$和$t_f$为0.015m和0.04m。

---

$C$Static Weight Estimation：
这里，记源点云（输入点云）为$P_{src}$，目标点云为$P_{tgt}$，通过上面介绍，可知我们仅针对foreground depth edge points计算其静态质量（ { P s r c ( i ) } i ∈ B s r c \{P_{src}(i)\}_{i∈B_{src}} {Psrc​(i)}i∈Bsrc​​，$B_{src}$是edge points的序号），记每一点的静态质量为$w_i^{src,tgt}$。通过计算$P_{src}(i)$和其对应点$P_{tgt}(c(i))$的欧式距离，公式如下：

其中，$c(i)\in B_{tgt}$，${\hat{T}}_{src}^{tgt}$是估计的变换矩阵。
通过这一公式，可将静态点和动态点进行区分（静态点的$d_i$为0或者很小），静态质量$w_i^{src,tgt}$通过t分布求得：

$v_0$为t分布的自由度，文中设为10，均值${\mu }_D$设为0，${\sigma }_D$为方差。
算法的流程可由下图表示：

下面我们来计算特征点的静态质量，记最新的关键帧为$P_k$，其中$k$为索引，上一个关键帧为$P_{k-N}$，$t$时刻的最新帧为$P_t$，$P_k$的静态质量$w_s(i)$可以定义为：

$D$Intensity Assisted Iterative Closest Point：
设源帧（输入图像的帧）为$<P_{src},I_{src}>$，目标帧为$<P_{tgt},I_{tgt}>$，用IACIP方法估计源点云到目标点云的变换$T^{\ast }$，$T^{\ast }$通常用单位阵或者运动预测进行初始化，通过循环搜索的方式最优化。对每一个$P_{src}(i)$，对应的匹配点$P_{tgt}$的索引值记作$c(i)$:

然后，计算增量变换$T_k$：

$W_i$为权重系数，求解方法如下所示：

最终用下式对$T^{\ast }$进行更新：

回环检测

当新的一帧关键帧$P_k$被建立，我们从之前随机的十帧中$P_r$中寻找回环检测，当下述三个条件满足时，我们认为在$P_k$和$P_r$之间实现了回环检测。
1）几何近似，即两帧之间的距离不会相隔太远：

2）两帧之间应该有至少30%的共视部分：

3）前后项连续检查：

实验结果