OKVIS 笔记:边缘化原理和策略

2018-03-22

OKVIS 系列文章:

建议阅读:

目录

边缘化原理

边缘化在很多文献里都提到了。 对于 VIO 系统来说,边缘化的目的是把旧的状态量从状态估计窗口中移除,保证运行效率; 同时,需要把移除的状态量的信息保留下来,作为当下窗口的先验,尽可能避免信息丢失。 边缘化主要利用的技巧是 Schur Complement(舒尔补)。 这套技巧在有些不需要剔除旧变量的场合也会用到,比如 g2o 论文 [1] 中的 Systems Having Special Structure 一节, 通过将高度稀疏的 landmark 部分 marginalize,先求解 dense 部分,回头再求解 landmark 部分,可以大幅提高运算速度。

下面的分析中,式子的编号将与 OKVIS 论文 [4] 中保持一致,但形式上有改动。

图优化中,状态估计总可以转换为求解一个 H-b 方程系统;将当下状态估计窗口下的 H-b 系统记为

[H11H12H21H22][δχ1δχ2]=[b1b2](21)\begin{bmatrix} \rm H_{11} & \rm H_{12} \\ \rm H_{21} & \rm H_{22} \end{bmatrix} \begin{bmatrix} \delta \chi_1 \\ \delta \chi_2 \end{bmatrix} = \begin{bmatrix} \rm b_1 \\ \rm b_2 \end{bmatrix} \tag{21}

其中,H11,b1\rm H_{11}, b_1 对应保留的状态量, H22,b2\rm H_{22}, b_2 对应需要边缘化的量。 利用舒尔补,可以将边缘化的量的信息「归并到」保留量的信息中:

H11=H11H12H221H21(22a)\rm H_{11}^{*} = H_{11} - H_{12}H_{22}^{-1}H_{21} \tag{22a} b1=b1H12H221b2(22b)\rm b_{1}^{*} = b_{1} - H_{12}H_{22}^{-1}b_{2} \tag{22b}

于是,对于移除边缘化状态量之后的系统,只需基于 H11\rm H_{11}^{\ast}b1\rm b_{1}^{\ast} 求解即可。

不过,为了保持系统的一致性,需要在优化迭代的过程中维持相关量的最初线性点,使用 First Estimate Jacobian (FEJ)。 关于 FEJ 的原理和深入分析,请参看 [2] [3] 。 简而言之,就是对于同一批状态量,未开始优化迭代时,计算其相关的 Jacobians(最初线性点); 进入优化迭代后,必须一直使用这些 Jacobians。 这意味着,优化迭代过程中 H 矩阵一直不变。 稍微麻烦一点的是 b 向量。因 b=JTΩe\rm b=-J^T\Omega e,而误差量 e\rm e 受估计值 xˉ\rm \bar{x} 影响, 所以每一步迭代之后,xˉ\rm \bar{x} 改变,b 也会改变。

OKVIS 使用一阶线性化来估计 b 的更新,避免重算 e\rm e 值。 记最初线性点的状态估计值为 x0\rm x_{0},到当下估计量的偏移值为 Δχ\rm \Delta\chi,即

xˉ=Exp(Δχ)x0\rm \bar{x} = Exp(\Delta \chi)\boxplus x_{0}

这里的 \boxplus 为广义上的两个状态量的叠加,与《位姿变换及其局部参数类》文中的有所不同。 于是,xˉ\rm \bar{x} 时的 b 可以估计为:

b=b0+bΔχx0Δχ=b0JTΩeΔχΔχ=0Δχ=b0HΔχ(24)\begin{aligned} \rm b &= \rm b_{0} + \frac{\partial \rm b}{\partial\Delta\chi}\Big|_{\rm x_0} \Delta\chi\\ &= \rm b_{0} - J^T\Omega \frac{\partial\rm e}{\partial\Delta\chi}\Big|_{\Delta\chi=0} \Delta\chi \\ &= \rm b_{0} - H\Delta\chi \tag{24} \end{aligned}

[b1b2]=[b1,0b2,0][H11H12H21H22][Δχ1Δχ2](25)\begin{bmatrix} \rm b_{1} \\ \rm b_{2} \end{bmatrix} = \begin{bmatrix} \rm b_{1,0} \\ \rm b_{2,0} \end{bmatrix} - \begin{bmatrix} \rm H_{11} & \rm H_{12}\\ \rm H_{21} & \rm H_{22} \end{bmatrix} \begin{bmatrix} \Delta\chi_{1} \\ \Delta\chi_{2} \end{bmatrix} \tag{25}

将其代入式 (22b),结合式 (22a),有

b1=b1,0H12H221b2,0H11Δχ1(26)\rm b_{1}^{\ast} = b_{1,0}-H_{12}H_{22}^{-1}b_{2,0}-H_{11}^{\ast}\Delta\chi_{1} \tag{26}

定义 b1,0=b1,0H12H221b2,0\rm b_{1,0}^{\ast}=b_{1,0}-H_{12}H_{22}^{-1}b_{2,0}b1,0\rm b_{1,0}^{\ast} 在初次线性化时就可以求出。 于是,边缘化过程实际上包括两个步骤:初次线性化时的式 (22a),以及 b1,0\rm b_{1,0}^{\ast};优化过程中每一步迭代后的式 (26)。 其代码实现,见《边缘化实现》一文, 尤其是 marginalizeOut()EvaluateWithMinimalJacobians() 两个函数。

OKVIS 边缘化策略

接下来介绍 OKVIS 的边缘化策略,即状态量估计窗口管理策略。 此部份 [5] 中亦有介绍。

首先,OKVIS 会维持 S 个最新 Frame 和 M 个最新 Keyframe,如下图所示。

time-window

最新的 S 个 Frame 称为 temporal window。 当不断有新的 Frame 加进来,或是普通 Frame,或是 Keyframe,就需要移除掉一些 Frame 或 Keyframe, 保持 SM 的数目不变。

边缘化一开始时,旧的 M 个 Keyframe 都处在 marginalization window 中 (marg window 中最末还有第 M+1 个 Frame,稍后可能被 marg 掉), 先 marg 掉它们的 speed/Bias 项,如下图所示:

okvis-fig7

当新进来一个 Frame 时,记为 xc\rm x^c; 这时,xcS\rm x^{c-S} 帧既是既有 marg window 的最后一帧,又是 temporal window 中的最先一帧; 如果它不是 Keyframe,这意味着 temporal window 中的普通 Frame 多了一帧, 于是 marg 掉 xcS\rm x^{c-S},如下图:

okvis-fig8

这种情况下不会有 landmark 被 marg 掉。 不过,与 xcS\rm x^{c-S} 相关的 observation 需要丢掉, 并且需要在 marginalization 之前丢掉,避免边缘化后的 fill-in 破坏系统中 landmark 部分的稀疏度(参看 [2])。

如果 xcS\rm x^{c-S} 是 Keyframe,新的 Keyframe 不宜被 marg,于是就 marg 最老的一帧 Keyframe xk1\rm x^{k_1},如下图:

okvis-fig9

同时,会把被 xk1\rm x^{k_1} 观测到、且不被 xcS\rm x^{c-S}xcS\rm x^{c-S} 之后的 Frame 观测到的 landmark 也 marg 掉。 当然,xk1\rm x^{k_1} 对不会被 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 笔记

授权协议 (CC) BY-NC-SA | 订阅 RSS | 邮箱 xmann_zh@foxmail.com | 回到顶部 🫠 |
已有892位小伙伴到此一游