简介
在 VIO 残差函数的构建 和 VIO 中残差雅可比的推导 中我整理了怎么对视觉以及 IMU 残差以及雅可比进行计算,结合非线性最小二乘的方法基本能够完成后端优化,但是在实际的 SLAM 过程为了限制计算规模通常会采用滑动窗口的形式,通过去除一些较旧的帧来保持计算的实时性,关于边缘化以及滑动窗口的基本原理可以参考 SLAM 中的边缘化以及滑动窗口算法,在 VIO 中对边缘化关键帧时还需要注意一个额外的问题,由于视觉 SLAM 中通常会将特征点纳入优化状态中,因此在进行优化问题求解时,常常需要先将信息矩阵进行边缘化估计节点,然后对特征点进行估计,这样会依赖信息矩阵的稀疏性才能进行快速求解。但对滑动窗口边缘化时会破坏信息矩阵的稀疏性,这篇论文主要概括一下常见的开源框架是怎么解决这一问题。滑动窗口主要涉及两点:关键帧的选取以及对剔除关键帧的选择策略。
Direct Sparse Odometry (DSO)
DSO 中的关键帧选择
DSO 选取关键帧的指标有三个,在进行粗匹配获得前后两帧的转换关系后,进行以下三个指标的判断:
- 判断相机的视场(Field of View)有没有发生变化,具体方法是通过在粗追踪时计算的前后两帧的平均光流平方(
$f:=(\frac{1}{n}\sum_{i}||\mathbf{p} - \mathbf{p}'||^2)^\frac{1}{2}$
)(光流追踪误差太大,表示已经离关键帧很远) - 通过判断是否由于平移导致遮挡,衡量方式为 (
$f_t:=(\frac{1}{n}\sum_{i}||\mathbf{p} - \mathbf{p}'_t||^2)^\frac{1}{2}$
) 这里和前一条的区别时在对$\mathbf{p}'$
进行转换时,只使用平移,旋转部分使用单位矩阵 - 相机曝光时间显著变化也需要添加新的关键帧,判断方式为两帧之间的相对亮度变化:
$a:=|\log{(e^{a_j-a_i}t_jt_i^{-1})}|$
DSO 中的滑动窗口
根据论文中描述,系统总是最多保持 $N_f$
个关键帧(论文中选取 $N_f = 7$
),设 $I_1$
是最新的关键帧, $I_n$
是最旧的关键帧,对剔除关键帧的选择策略如下:
- 保证最新的前两帧不被边缘化
- 对和最新一帧
$I_1$
共同观测到的点(共视点)少于 5% 的关键帧进行边缘化 - 如果剩余的关键帧还大于
$N_f$
对关键帧计算以下“距离分数”(distance score)
公式中 $d(i, j)$
为两帧之间的欧氏距离,$\epsilon$
是一个较小的常数项,这个分数在保持关键帧在三维空间均匀分布(分母项)的同时,优先保留离第一帧较近的关键帧。
$$
s(I_i) = \sqrt{d(i, 1)}\sum_{j\in[3, n]\setminus\{i\}}(d(i, j) + \epsilon)^{-1}
$$
在对关键帧进行边缘化之前俺,先将这些关键帧中的的参考点(hosted points,即以这一帧作为参考帧的点)进行边缘化。除此之外,为了最大程度上保持信息矩阵的稀疏性,对这些关键帧中剩余的观测点去除。作者在论文提到,这种方法在实践中大概会导致一半的残差被丢失。
此外,为了优化问题的可观性不发生变化,DSO 采用了 FEJ 算法以保持所有涉及到已被边缘化的点的残差线性化点的一致,对 $\boldsymbol{b} = -\boldsymbol{J\Sigma}^{-1}\boldsymbol{r}$
的更新也和 SLAM 中的边缘化以及滑动窗口算法 总结的一致。
OKVIS
以下内容主要参考 Keyframe-Based Visual-Inertial SLAM Using Nonlinear Optimization,根据 OKVIS 的 README 还会涉及其他几篇论文,这部分以后补充。
OKVIS 中的关键帧选择
论文中的选择策略是:在 OKVIS 中主要维护两个帧集合,其中一个是时间上按顺序排列的滑动窗口,另一个则是关键帧集合,这两个集合通常会有交叉,即在滑动窗口中有部分帧是关键帧,有部分帧不是关键帧。关键帧的选择思路也很简单,当图片区域检测到的观测点组成的几何空间(Bounding Box?)相对于目前维护的所有关键帧的观测点组成的集合空间占比下降到 50% 到 60% 左右则将当前帧添加为关键帧。思路基本上和 DSO 中的第一点一致,都是判断相机视场有没有变化,细微的区别是 OKVIS 相当于将当前帧的观测点构成的“局部地图”与关键帧集合中的“局部地图”,而 DSO 则是比较前后两帧的观测点集。
OKVIS 中的滑动窗口
OKVIS 中的滑动窗口策略主要就是按顺序边缘化最旧的一阵,此时按最旧帧是否是关键帧可以分两种情况:
- 滑动窗口中最旧一帧是非关键帧时:
如下图所示:粉色矩阵链接的部分是已经被边缘化后的节点,当前滑动窗口中包含 3 个节点,此时一个新节点 $\boldsymbol{x}^c_T$
进入滑动窗口,因此对最旧一帧对应节点进行边缘化。在这种情况下,完成边缘化更新先验因子后直接将该帧的观测去除。这样有利于保证信息矩阵的稀疏性,从而快速求解。
- 滑动窗口中最旧一阵是关键帧时:
在这种情况时,由于待边缘化的帧是关键帧,如果直接将其观测去除,对精度的影响会更加显著,因为连接最旧两帧的相对运动约束会全部丢失,因此论文中采取的做法是将那些在较旧的关键帧能观测到,但是在较新的关键帧观测不到的的观测点去除。这样,通过边缘化通常不会显著破坏信息矩阵的稀疏性。(这里的图片解释跟论文的论述有点匹配不上,$\boldsymbol{x}_T^{c-3}$
应该也要被边缘化,$\boldsymbol{x}_T^{k_1}$
应该在之前就已经被边缘化了。)
同样为了保持全局可观性一致,论文里同样也使用了 FEJ 算法,即对所以涉及到被边缘化节点的残差进行线性展开时线性化点保持一致。
VINS-MONO
VINS-MONO 中的关键帧选择
VINS-MONO 中的关键帧考察以下两个指标:
- 如果两帧之间的视差超过一定阈值,则将新的帧设为关键帧,主要通过比较两个点的在两帧中的归一化平面的距离。
- 如果追踪效果不好,也将该帧设为关键帧,评判标准是追踪到的特征点是否低于一定阈值
VINS-MONO 中的滑动窗口策略
下图展示了 VINS-MONO 在边缘化时使用的策略。主要按照第二新的帧来进行区分:
- 如果第二新的帧是关键帧,它会留在窗口,滑动窗口中最旧的一帧以及其对应的观测会被边缘化掉
- 如果第二新的帧不是关键帧,直接丢弃该帧以及对应的观测以保持信息矩阵的稀疏性,IMU 的测量会保持下来
VINS-MONO 中并没有实现 FEJ 算法,作者认为对于 VIO 而言小的偏移可以接受,因此边缘化带来的负面印象不会很显著。
总结
结合这三个开源来看,三者在选择关键帧的时候有一定的共性,比如会考虑相机的视场有没有变化,由于 DSO 采用直接法,而灰度法对灰度变化明杆,因此 DSO 中还额外考虑相机的曝光时间。而在滑动窗口的维护上,思路上大致相似,为了保持信息矩阵的稀疏性,都会选择将非关键帧的残差丢弃,已经尽量保留下与较新节点有关联的观测。