VIO滑窗中的FEJ算法理解

参考高博PPT中的例子。
过程综述：
如上图所示，在t∈[0, k]时刻，滑窗中的状态向量为$X^k$，包含6个状态量：${\xi }_i\text{, }i\in \left[1,6\right]$及其对应的观测量。在k时刻，对$X^k$进行边缘化处理，marg掉在${\xi }_1$及其观测，在$k^{{}^{\prime }}$时刻，加入新的观测和状态量${\xi }_7$。

FEJ算法的步骤：

1. marg发生前，优化过的滑窗都包含哪些信息？
在第k时刻，对$X^k$用最小二乘法优化完以后，要marg掉变量${\xi }_1$，被marg的状态向量即$X^k$(此处PPT原文为$X_m$，但按照本人的思维惯性，认为$X_m$的含义应该表示的是marg完成以后去掉的变量及其对应测量，因此PPT原文中对应的 $X_m$将在下文同改为$X^k$)，剩余的变量${\xi }_i\text{, }i\in \left[2,5\right]$记为$X_r$。
marg前，状态量$X$以及对应的测量$S$构建的最小二乘信息矩阵为：$$b\left(k\right)=\left[\begin{array}{c}{b}_{mm}\left(k\right)\\ b_{mr}\left(k\right)\end{array}\right]=-\sum _{\left(i,j\right)\in S}{J}_{ij}^{\mathrm{T}}\left(k\right){\Sigma }_{ij}^{-1}{r}_{ij}\left(k\right)$$ $$\Lambda \left(k\right)=\left[\begin{array}{cc}{\Lambda }_{mm}\left(k\right)& {\Lambda }_{mr}\left(k\right)\\ {\Lambda }_{rm}\left(k\right)& {\Lambda }_{rr}\left(k\right)\end{array}\right]=\sum _{\left(i,j\right)\in S}{J}_{ij}^{\mathrm{T}}\left(k\right){\Sigma }_{ij}^{-1}{J}_{ij}\left(k\right)$$

2. marg发生后，留下的到底是什么信息？
marg发生后， $X^k$向量中所有的变量以及对应的测量将被丢弃。同时，这部分信息通过marg操作传递给了保留状态量$X_r$。marg变量的信息跟${\xi }_6$不相关。即$X^k$→$X_r$。
注：本人更倾向于转换一词，因为 $X^k$和$X_r$对应的信息矩阵的维数不同，比如本例中，marg前信息矩阵${\Lambda }^k\left(k\right)$的维度为6×6，marg后信息矩阵${\Lambda }_p\left(k\right)$的维度4×4，维度不同，无法传递。
$$b_p\left(k\right)=b_{mr}\left(k\right)-{\Lambda }_{rm}\left(k\right){\Lambda }_{mm}^{-1}\left(k\right)b_{mm}\left(k\right)$$ $${\Lambda }_p\left(k\right)={\Lambda }_{rr}\left(k\right)-{\Lambda }_{rm}\left(k\right){\Lambda }_{mm}^{-1}\left(k\right){\Lambda }_{mr}\left(k\right)$$下标p表示prior，即这些信息将构建一个关于$X_r$的先验信息。
此处先验叫法的理解，marg完成后，滑窗中的状态量减少，后面还要补充新的变量和观测，相对于后面加入的变量和观测，$X_r$就是新滑窗的先验。
先验
我们可以从$b_p\left(k\right)$ 和${\Lambda }_p\left(k\right)$中反解除一个残差$r_p\left(k\right)$（如：$r_{23}$，$r_{24}$，$r_{34}$，$r_{45}$，$r_{35}$）和对应的雅克比矩阵$J_p\left(k\right)$，但意义不大。需要注意的是，随着剩余状态量$X_r\left(k\right)$的后续不断优化变化(因为后面会加入新的变量和观测，组成新的变量关系)，残差$r_p\left(k\right)$和$b_p\left(k\right)$也将跟着变化，但雅克比$J_p\left(k\right)$则固定不变了(原因在下文)。

3. 新测量信息和旧测量信息构建新的系统
第$k^{{}^{\prime }}$时刻，加入新的变量${\xi }_7$以及对应的观测(记作$X_n$)，新的残差$r_{27}$和先验信息$b_p\left(k\right)$ 、${\Lambda }_p\left(k\right)$以及残差$r_{56}$构建新的最小二乘问题，开始新一轮的最小二乘优化：
$$b\left(k^{{}^{\prime }}\right)={\Pi }^{\mathrm{T}}{b}_p\left(k\right)-\sum _{\left(i,j\right)\in S_a\left(k^{{}^{\prime }}\right)}{J}_{ij}^{\mathrm{T}}\left(k^{{}^{\prime }}\right){\Sigma }_{ij}^{-1}{r}_{ij}\left(k^{{}^{\prime }}\right)$$ $$\Lambda \left(k^{{}^{\prime }}\right)={\Pi }^{\mathrm{T}}{\Lambda }_p\left(k\right)\Pi +\sum _{\left(i,j\right)\in S_a\left(k^{{}^{\prime }}\right)}{J}_{ij}^{\mathrm{T}}\left(k^{{}^{\prime }}\right){\Sigma }_{ij}^{-1}{J}_{ij}\left(k^{{}^{\prime }}\right)$$
其中$\Pi =\left[\begin{array}{cc}{I}_{dim{X}_r}& 0\end{array}\right]$用来将矩阵的维度进行扩张。$S_a$用来表示除被marg掉的测量意外的其他测量，如$r_{56}$、$r_{27}$。

出现的问题：
· 由于marg的变量以及对应的测量$X^k$已经被丢弃，先验信息${\Lambda }_p\left(k\right)$中关于$X_r$的雅克比$J_p\left(k\right)$在后续求解中没法更新。
这句话个人理解：$J_p\left(k\right)$=$r_p\left(k\right)$/$\delta x$，因为$X^k$已丢失，其最后一次迭代的$\delta x$也已不在（当然，$x$是知道的），因此即便残差$r_p\left(k\right)$更新变化，$J_p\left(k\right)$也无法再更新。
· 但$X_r$中的部分变量还跟其他残差由联系，如${\xi }_2$、${\xi }_5$。这些残差如$r_{27}$对${\xi }_2$的雅克比会随着${\xi }_2$的迭代更新而不断在最新的线性化点处计算。
这句话个人理解：先验中关于${\xi }_2$、${\xi }_5$的雅克比固定不变，在求解新的最小二乘时，$r_{27}$会对${\xi }_2$进行迭代更新，其雅克比会在最新的线性化点处计算，而该线性化点与marg前$r_{12}$对${\xi }_2$附近的线性化点不同，因此这是矛盾的。

4. 信息矩阵的零空间变化
滑动窗口算法导致的问题
滑动窗口算法和优化的时候，信息矩阵如上式变成了两部分的和，且两部分计算雅克比时线性化点不同。这可能会导致信息矩阵的零空间发生变化，从而在求解时引入错误的信息。
比如：求解单目SLAM 进行Bundle Adjustment 优化时，问题对应的信息矩阵$\Lambda \left(k^{{}^{\prime }}\right)$不满秩，对应的零空间为N, 用高斯牛顿求解时有
$$\Lambda \delta x=b$$ $$\Lambda \delta x+N\delta x=b$$
即存在多个$\delta x$使得$$\mathbf{N}\delta x=0$$
若增量$\delta x$在零空间维度下变化，并不会改变我们的残差。这意味着系统可以有多个满足最小化损失函数（残差）的解$x$。

参考文献：

1. 高翔、贺一家、崔华坤的手写VIO
2. Tue-Cuong Dong-Si and Anastasios I Mourikis. “Consistency analysis for sliding-window visual odometry”. In: 2012 IEEE
   International Conference on Robotics and Automation. IEEE. 2012, pp. 5202–5209.