## OKVIS 笔记：边缘化原理和策略

2018-03-22

OKVIS 系列文章：

### 边缘化原理

$\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}$

$\rm H_{11}^{*} = H_{11} - H_{12}H_{22}^{-1}H_{21} \tag{22a}$ $\rm b_{1}^{*} = b_{1} - H_{12}H_{22}^{-1}b_{2} \tag{22b}$

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

$\rm \bar{x} = Exp(\Delta \chi)\boxplus x_{0}$

\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}

$\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}$

$\rm b_{1}^{\ast} = b_{1,0}-H_{12}H_{22}^{-1}b_{2,0}-H_{11}^{\ast}\Delta\chi_{1} \tag{26}$

### 参考文献

[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 笔记