OKVIS 系列文章:
- OKVIS 笔记:位姿变换及其局部参数类
- OKVIS 笔记:后端状态量参数结构
- OKVIS 笔记:后端架构简述
- OKVIS 笔记:边缘化原理和策略
- OKVIS 笔记:边缘化实现
建议阅读:
目录
边缘化原理
边缘化在很多文献里都提到了。 对于 VIO 系统来说,边缘化的目的是把旧的状态量从状态估计窗口中移除,保证运行效率; 同时,需要把移除的状态量的信息保留下来,作为当下窗口的先验,尽可能避免信息丢失。 边缘化主要利用的技巧是 Schur Complement(舒尔补)。 这套技巧在有些不需要剔除旧变量的场合也会用到,比如 g2o 论文 [1] 中的 Systems Having Special Structure 一节, 通过将高度稀疏的 landmark 部分 marginalize,先求解 dense 部分,回头再求解 landmark 部分,可以大幅提高运算速度。
下面的分析中,式子的编号将与 OKVIS 论文 [4] 中保持一致,但形式上有改动。
图优化中,状态估计总可以转换为求解一个 H-b 方程系统;将当下状态估计窗口下的 H-b 系统记为
其中, 对应保留的状态量, 对应需要边缘化的量。 利用舒尔补,可以将边缘化的量的信息「归并到」保留量的信息中:
于是,对于移除边缘化状态量之后的系统,只需基于 和 求解即可。
不过,为了保持系统的一致性,需要在优化迭代的过程中维持相关量的最初线性点,使用 First Estimate Jacobian (FEJ)。 关于 FEJ 的原理和深入分析,请参看 [2] [3] 。 简而言之,就是对于同一批状态量,未开始优化迭代时,计算其相关的 Jacobians(最初线性点); 进入优化迭代后,必须一直使用这些 Jacobians。 这意味着,优化迭代过程中 H 矩阵一直不变。 稍微麻烦一点的是 b 向量。因 ,而误差量 受估计值 影响, 所以每一步迭代之后, 改变,b 也会改变。
OKVIS 使用一阶线性化来估计 b 的更新,避免重算 值。 记最初线性点的状态估计值为 ,到当下估计量的偏移值为 ,即
这里的 为广义上的两个状态量的叠加,与《位姿变换及其局部参数类》文中的有所不同。 于是, 时的 b 可以估计为:
即
将其代入式 (22b),结合式 (22a),有
定义 , 在初次线性化时就可以求出。
于是,边缘化过程实际上包括两个步骤:初次线性化时的式 (22a),以及 ;优化过程中每一步迭代后的式 (26)。
其代码实现,见《边缘化实现》一文,
尤其是 marginalizeOut()
和 EvaluateWithMinimalJacobians()
两个函数。
OKVIS 边缘化策略
接下来介绍 OKVIS 的边缘化策略,即状态量估计窗口管理策略。 此部份 [5] 中亦有介绍。
首先,OKVIS 会维持 S 个最新 Frame 和 M 个最新 Keyframe,如下图所示。
最新的 S 个 Frame 称为 temporal window。 当不断有新的 Frame 加进来,或是普通 Frame,或是 Keyframe,就需要移除掉一些 Frame 或 Keyframe, 保持 S 和 M 的数目不变。
边缘化一开始时,旧的 M 个 Keyframe 都处在 marginalization window 中 (marg window 中最末还有第 M+1 个 Frame,稍后可能被 marg 掉), 先 marg 掉它们的 speed/Bias 项,如下图所示:
当新进来一个 Frame 时,记为 ; 这时, 帧既是既有 marg window 的最后一帧,又是 temporal window 中的最先一帧; 如果它不是 Keyframe,这意味着 temporal window 中的普通 Frame 多了一帧, 于是 marg 掉 ,如下图:
这种情况下不会有 landmark 被 marg 掉。 不过,与 相关的 observation 需要丢掉, 并且需要在 marginalization 之前丢掉,避免边缘化后的 fill-in 破坏系统中 landmark 部分的稀疏度(参看 [2])。
如果 是 Keyframe,新的 Keyframe 不宜被 marg,于是就 marg 最老的一帧 Keyframe ,如下图:
同时,会把被 观测到、且不被 或 之后的 Frame 观测到的 landmark 也 marg 掉。 当然, 对不会被 marg 的 landmark 的 observation 项也要在 marginalization 前丢掉,避免 fill-in。
当有一帧被 marg 掉,marg window 就往后扩容一帧,保持边缘化窗口大小始终一致,如上面 Fig 8、Fig 9 两图所示。
边缘化策略的代码实现,见《边缘化实现》一文,
尤其是 applyMarginalizationStrategy()
函数。
参考文献
[1] R. Kümmerle, G. Grisetti, H. Strasdat, K. Konolige and W. Burgard, “G2o: A general framework for graph optimization,” 2011 IEEE International Conference on Robotics and Automation, Shanghai, 2011, pp. 3607-3613. [PDF]
[2] 白巧克力亦唯心:SLAM 中的 marginalization 和 Schur complement ↗
[3] 知乎:如何理解 SLAM 中的 First-Estimates Jacobian?↗
[4] S. Leutenegger, S. Lynen, M. Bosse, R. Siegwart, P. Furgale, “Keyframe-based visual-inertial odometry using nonlinear optimization”, Int. Journal of Robotics Research (IJRR), 2014.
[5] Xingyin-Fu:OKVIS 笔记 ↗