VINS-MONO边缘化策略

        VINS中的滑窗优化策略，将滑出窗外的帧与滑窗内的帧的约束使用边缘化的形式保存为先验误差进行后续非线性优化，以保留约束信息。本文对具体的方案进行记录。
        紧耦合VIO处理的两种误差分别为IMU误差与图像误差，采用LM、GN、Dogleg等方式迭代求解待优化参数增量，以求得最大似然估计，最小化非线性化误差。
        迭代优化的核心即求解增量方程：
$$H\delta x=b$$        其中$H=J^{T}J,b=J^{T}e,e=(J^T{)}^{+}b$.此处的$({)}^{+}$为伪逆，$e$为残差，$J$为残差对于状态的Jacobian。
        由于引入滑窗后，对于滑到窗口之外的帧不再优化其参数，但其与滑窗内的数据仍然存在约束，直接丢掉这些窗外帧会造成约束信息丢失，所以要将其封装成先验信息，加入到大的非线性优化问题中作为一部分误差，这一步即为边缘化。假设要边缘化的状态为$x_2$，要保留的状态为$x_1$，对于上述增量方程，变为：$$\left[\begin{array}{cc}{H}_{11}& H_{12}\\ H_{21}& H_{22}\end{array}\right]\left[\begin{array}{c}\delta x_1\\ \delta x_2\end{array}\right]=\left[\begin{array}{c}{b}_1\\ b_2\end{array}\right]$$        边缘化的方法为舒尔补：$$\begin{gathered} \left[\begin{array}{cc}I& -H_{12}{H}_{22}^{-1}\\ 0& I\end{array}\right]\left[\begin{array}{cc}{H}_{11}& H_{12}\\ H_{21}& H_{22}\end{array}\right]\left[\begin{array}{c}\delta x_1\\ \delta x_2\end{array}\right]=\left[\begin{array}{cc}I& -H_{12}{H}_{22}^{-1}\\ 0& I\end{array}\right]\left[\begin{array}{c}{b}_1\\ b_2\end{array}\right] \\ \left[\begin{array}{cc}{H}_{11}-H_{12}{H}_{22}^{-1}{H}_{21}& 0\\ H_{21}& H_{22}\end{array}\right]\left[\begin{array}{c}\delta x_1\\ \delta x_2\end{array}\right]=\left[\begin{array}{c}{b}_1-H_{12}{H}_{22}^{-1}{b}_2\\ b_2\end{array}\right] \end{gathered}$$        这样只需要求解第一个方程，即$$(H_{11}-H_{12}{H}_{22}^{-1}{H}_{21})\delta x_1=b_1-H_{12}{H}_{22}^{-1}{b}_2,$$        这样既没有损失约束信息，又不需要求解$\delta x_2$,把$H_{11}-H_{12}{H}_{22}^{-1}{H}_{21}$记为$H_0^{\ast }$，$b_1-H_{12}{H}_{22}^{-1}{b}_2$记为$b_0^{\ast }$。
        上式为边缘化后待求解的增量方程，我们要根据这个增量方程恢复出边缘化封装的先验误差的具体形式$e_p$
        随着迭代的过程，状态量被不断更新。但在边缘化时，被边缘化的状态值固定，舒尔补时使用的雅克比即为当时泰勒展开使用该固定的值(x线性化点)求得的雅克比，都需要固定。在VINS中，线性化点处的参数值x0被保存为keep_block_data。此即为EFJ，更多解释详看滑窗优化、边缘化、舒尔补、FEJ及fill-in问题。但在上述方程中，由于$b=J^{T}e$，其中的$e$随着状态的更新而变化，因此$b$也需随之变化。
        状态的更新（此处$\delta x$表示当前参数值相对$x_0$的变化）：$$x^{\prime }=Exp(\delta x)\otimes x_0$$        此时b的更新可以表示为$$\begin{gathered} b=b_0+\frac{\partial b}{\partial x}{|}_{x_0}\delta x \\ =b_0+J^T\frac{\partial e}{\partial x}{|}_{x_0}\delta x \\ =b_0+J^{T}J\delta x=b_0+H\delta x \end{gathered}$$        有$$\begin{gathered} \left[\begin{array}{c}{b}_1\\ b_2\end{array}\right]=\left[\begin{array}{c}{b}_{1,0}\\ b_{2,0}\end{array}\right]+\left[\begin{array}{cc}{H}_{11}& H_{12}\\ H_{21}& H_{22}\end{array}\right]\left[\begin{array}{c}\delta x_1\\ \delta x_2\end{array}\right] \\ {b}_1=b_{1,0}+H_{11}\delta x_1+H_{12}\delta x_2 \\ {b}_2=b_{2,0}+H_{21}\delta x_1+H_{22}\delta x_2 \end{gathered}$$        联立$$b^{\ast }=b_1-H_{12}{H}_{22}^{-1}{b}_2$$        有$$\begin{gathered} b^{\ast }=b_1-H_{12}{H}_{22}^{-1}{b}_2 \\ =(b_{1,0}+H_{11}\delta x_1+H_{12}\delta x_2)-H_{12}{H}_{22}^{-1}(b_{2,0}+H_{21}\delta x_1+H_{22}\delta x_2) \\ =b_{1,0}-H_{12}{H}_{22}^{-1}{b}_{2,0}+(H_{11}-H_{12}{H}_{22}^{-1}{H}_{21})\delta x_1 \\ =b_0^{\ast }+H_0^{\ast }\delta x_1 \end{gathered}$$        由上述推导，原增量方程变为$$\begin{gathered} H_0^{\ast }\delta x=J_l^T{J}_l\delta x=b^{\ast }=b_0^{\ast }+H_0^{\ast }dx \\ =b_0^{\ast }+J_l^T{J}_{l}dx=J_l^T(J_l^T{)}^{+}{b}_0^{\ast }+J_l^T{J}_{l}dx=J_l^T((J_l^T{)}^{+}{b}_0^{\ast }+J_{l}dx) \end{gathered}$$        其中$J_l$为$H_0^{\ast }$分解出的等价雅克比，$\delta x$为每次参数更新增量，$dx$为当前参数值与线性化点$x_0$的差值。
        因此，边缘化后等价的先验误差$$\begin{gathered} e_p=(J_l^T{)}^{+}{b}^{\ast } \\ =(J_l^T{)}^{+}{b}_0^{\ast }+J_{l}dx \end{gathered}$$至此，边缘化的步骤可以总结为：

1. 舒尔补求解$H_0^{\ast }=H_{11}-H_{12}{H}_{22}^{-1}{H}_{21},b_0^{\ast }=b_{1,0}-H_{12}{H}_{22}^{-1}{b}_{2,0}$
2. 分解$H_0^{\ast }$求解$J_l、J_l^T、(J_l^T{)}^{+}$，求解$e_0=(J_l^T{)}^{+}{b}_0^{\ast }$，保存$J_l,e_0$
3. 求解$\delta x$由$H_0^{\ast }\delta x=b_0^{\ast }$（ceres完成）
4. 使用$\delta x$更新$x$,计算当前状态$x$与$x_0$的差值$dx$,更新$e$:$e_p=e_0+J_{l}dx$（costfunction定义residuals，
   同时定义costfunction需要的jacobians，把$J_l$由localsize转为globalsize）
   ceres自动更新$b^{\ast }$:$b^{\ast }=J_l^T{e}_p$:
5. 求解$\delta x$由$H_0^{\ast }\delta x=b^{\ast }$（ceres完成）
6. 循环4,5

附录：

        使用H分解J的方式如下：
$$H=US{U}^T=J^{T}J$$        对H进行奇异值分解，由于H为半正定实矩阵，S也即为特征值矩阵，这样$$\begin{gathered} J=S^{\frac{1}{2}}{U}^T \\ (J^T{)}^{+}=S^{-\frac{1}{2}}{U}^T \end{gathered}$$