（转载请注明出处）
下面来讨论使用IMU预积分框架的VIO/VISLAM，在进行优化求解时，其增量方程的结构。
不失一般性，我们不区分相机状态和IMU状态（暂时不考虑优化IMU-CAM外参。可以认为预积分是将IMU数据进行坐标转换后在相机系下进行的；或者认为对地图点的观测经坐标系转换后表达在载体系下）。
优化问题表述如下：\[ \tag{1} \label{1} \begin{array}{*{20}{l}} {\min \mathop {\arg }\limits_{\bf{x}} \left\{ {\sum\limits_{i = 1}^{{N_c}} {\sum\limits_{j \in {P_i}} {\left\| {{{\bf{f}}_{vis}}\left( {{{\bf{R}}_i},{{\bf{t}}_i},{{\bf{p}}_j}} \right)} \right\|_{{{\pmb{\Omega }}_{vis}}}^2} } + \sum\limits_{i = 1}^{{N_c} - 1} {\left\| {{{\bf{f}}_{\Delta R}}\left( {{{\bf{R}}_i},{{\bf{R}}_{i + 1}},\delta {\bf{b}}{g_i}} \right)} \right\|_{{{\pmb{\Omega }}_{\Delta R}}}^2} } \right.}\\ { + \sum\limits_{i = 1}^{{N_c} - 1} {\left\| {{{\bf{f}}_{\Delta v}}\left( {{{\bf{R}}_i},{{\bf{v}}_i},\delta {\bf{b}}{g_i},\delta {\bf{b}}{a_i},{{\bf{v}}_{i + 1}}} \right)} \right\|_{{{\pmb{\Omega }}_{\Delta v}}}^2} + \sum\limits_{i = 1}^{{N_c} - 1} {\left\| {{{\bf{f}}_{\Delta t}}\left( {{{\bf{R}}_i},{{\bf{t}}_i},{{\bf{v}}_i},\delta {\bf{b}}{g_i},\delta {\bf{b}}{a_i},{{\bf{t}}_{i + 1}}} \right)} \right\|_{{{\pmb{\Omega }}_{\Delta t}}}^2} }\\ {\left. { + \sum\limits_{i = 1}^{{N_c} - 2} {\left\| {{\bf{b}}{g_{i + 1}} - {\bf{b}}{g_i}} \right\|_{{{\pmb{\Omega }}_{bg}}}^2} + \sum\limits_{i = 1}^{{N_c} - 2} {\left\| {{\bf{b}}{a_{i + 1}} - {\bf{b}}{a_i}} \right\|_{{{\pmb{\Omega }}_{ba}}}^2} + \left\| {{{\bf{f}}_{prior}}\left( {\bf{x}} \right)} \right\|_{{{\pmb{\Omega }}_{prior}}}^2} \right\}} \end{array} \]其中，\(N_c\)表示参与优化的帧（关键帧）数；\({\cal P}_i\)表示第i帧可以观测到的地图点的集合；由于用符号\(\bf{p}\)描述了地图点坐标，因此用\(\bf{t}\)描述相机位置，位置的预积分符号也相应的改为\(\Delta \bf{t}\)。
\({{{\pmb{\Omega }}_{vis}}}\)表示重投影误差信息矩阵（2x2维）。
\({{{\pmb{\Omega }}_{\Delta R}}}\)表示相邻关键帧间的旋转预积分（\(\Delta \bf{R}\)）的信息矩阵；\({{{\pmb{\Omega }}_{\Delta v}}}\)表示相邻关键帧间的速度预积分（\(\Delta \bf{v}\)）的信息矩阵；\({{{\pmb{\Omega }}_{\Delta t}}}\)表示相邻关键帧间的位置预积分（\(\Delta \bf{p}\)）的信息矩阵。这三个信息矩阵参见《预积分总结与公式推导》的预积分噪声协方差部分，在Forster的原始paper中，推导了一个三者整合的协方差递推方程。
\({{{\pmb{\Omega }}_{bg}}}\)和\({{{\pmb{\Omega }}_{ba}}}\)分别表示陀螺和加计bias的随机游走信息矩阵，它们的逆是帧间bias随机游走系数对应的高斯分布的协方差矩阵。这两个信息矩阵对应的指标项的意义，是表示每一帧对应的imu传感器的bias满足随机游走，也即相邻帧的bias之差满足高斯分布（最原本的是相邻两条imu测量的bias之间满足随机游走）。
\({{{\bf{\Omega }}_{prior}}}\)表示先验约束的信息矩阵。对于初始化来说，该信息矩阵代表了初始帧的初始状态的可信度（因此\(\bf{x}\)里只包括初始帧的状态们）。对于滑动窗VIO的BA来说，这个信息矩阵由如下方式获得：上一个滑动窗BA完成之后，在大\(\bf{H}\)矩阵中，首先将滑出去的帧进行marg，再将不会再被观测到的地图点marg，修正剩余状态的Hessian（使其描述剩余状态关于被marg状态的条件概率。不进行marg前描述的是所有上一个滑动窗中状态的联合概率），这个Hessian就是就是信息矩阵（这里有一个关于信息矩阵与Hessian的关系的说明）。marg这个过程保有了滑出状态的信息，使得前面的估计结果在概率分布上被保留。这样一来，当前滑窗中的部分状态，可以由上一个滑动窗BA的结果，获得具有一致性的边缘概率描述（包括估计结果和修正后的Hessian）。
其实对于先验信息，可以使其只约束当前滑动窗的首帧状态，而不包括其他帧，原理很简单，就是把上一个滑动窗中所有其他状态全都marg掉。但总感觉这样怪怪的，因为上一个滑窗和当前滑窗会有很多重复的状态，只把滑出的状态进行marg看上去会更自然一些。目前看到在VINS里先验是包括了不止首帧状态的很多状态，而ICE-BA从公式上看先验只包括首帧状态。
注意到，对于bias，在预积分指标中采用的是误差状态形式（\(\delta\)），而在bias随机游走指标中采用的是原始状态形式（状态初值+误差状态）。在求取Jacobian时，应当统一针对误差状态进行计算。
此外，由于IMU预积分框架中，一般假设参与优化的前后两帧bias相同，因此第\(N_c\)帧对应的bias没有在式\(\eqref{1}\)中出现，在操作中，迭代完成之后把第\({N_c}-1\)帧的bias估计结果赋值给第\(N_c\)帧（由于这是参与优化的最后一帧，一般不会成为下一次优化中贡献prior约束的帧，因此Hessian用不上，只需求估计结果）。
下面来看增量方程的结构。
仍然沿用“BA问题详解1”中的场景，共存在3组相机状态\({{\bf{x}}_{c_i}}\)（\(i = 1,2,3\)，每组包括姿态\(\bf{R}\)、位置\(\bf{t}\)、速度\(\bf{v}\)以及对应时刻的惯性器件bias——\({\bf{b}}_g\)和\({\bf{b}}_a\)，其中bias使用的是误差量作为状态）和4个地图点位置\({\bf{p}}_j\)（\(ij= 1,2,3,4\)）。除了“帧观测到地图点”这种传统约束外，增加了相邻帧间的预积分约束，以及首帧的prior约束（暂时不考虑prior更复杂的情况）。
根据前两篇笔记对BA问题分析的结果，直接来看Hessian矩阵的结构，如下\[ \tag{2} \label{2} {\bf{H}} = \left[ {\begin{array}{*{20}{c}} {{{\bf{U}}_1}}&{{{\bf{X}}_{12}}}&{\bf{0}}&{{{\bf{Y}}_{11}}}&{{{\bf{Y}}_{12}}}&{{{\bf{Y}}_{13}}}&{{{\bf{Y}}_{14}}}\\ {{\bf{X}}_{12}^T}&{{{\bf{U}}_2}}&{{{\bf{X}}_{23}}}&{{{\bf{Y}}_{21}}}&{{{\bf{Y}}_{22}}}&{{{\bf{Y}}_{23}}}&{{{\bf{Y}}_{24}}}\\ {\bf{0}}&{{\bf{X}}_{23}^T}&{{{\bf{U}}_3}}&{{{\bf{Y}}_{31}}}&{{{\bf{Y}}_{32}}}&{{{\bf{Y}}_{33}}}&{{{\bf{Y}}_{34}}}\\ {{\bf{Y}}_{11}^T}&{{\bf{Y}}_{21}^T}&{{\bf{Y}}_{31}^T}&{{{\bf{V}}_1}}&{\bf{0}}&{\bf{0}}&{\bf{0}}\\ {{\bf{Y}}_{12}^T}&{{\bf{Y}}_{22}^T}&{{\bf{Y}}_{32}^T}&{\bf{0}}&{{{\bf{V}}_2}}&{\bf{0}}&{\bf{0}}\\ {{\bf{Y}}_{13}^T}&{{\bf{Y}}_{23}^T}&{{\bf{Y}}_{33}^T}&{\bf{0}}&{\bf{0}}&{{{\bf{V}}_3}}&{\bf{0}}\\ {{\bf{Y}}_{14}^T}&{{\bf{Y}}_{24}^T}&{{\bf{Y}}_{34}^T}&{\bf{0}}&{\bf{0}}&{\bf{0}}&{{{\bf{V}}_4}} \end{array}} \right] \]其中，\({\bf{V}}_j\)表示第j个地图点的自协Hessian（关于所有能观测到它的相机位姿的Hessian求和），与纯视觉BA中完全一致。
\({\bf{U}}_i\)表示第i个相机状态的自协Hessian，由于相机状态多增加了速度和bias，并且增加了帧间预积分以及bias的随机游走约束（首帧还增加了prior约束），这里与纯视觉中有所不同。我们来看一下\({\bf{U}}_i\)的构造，按照姿态、位置、速度、陀螺bias、加计bias的顺序进行分块，只要某块状态和另一块状态在\(\eqref{1}\)指标中产生了直接联系，我们就将对应非主对角块进行填充，否则该块将为\(\bf{0}\)矩阵。于是可得其构造如下\[ \tag{3} \label{3} {{\bf{U}}_i} = \left[ {\begin{array}{*{20}{c}} {{{\bf{U}}_\phi }}&{{{\bf{U}}_{\phi t}}}&{{{\bf{U}}_{\phi v}}}&{{{\bf{U}}_{\phi bg}}}&{{{\bf{U}}_{\phi ba}}}\\ {{\bf{U}}_{\phi t}^T}&{{{\bf{U}}_t}}&{{{\bf{U}}_{tv}}}&{{{\bf{U}}_{tbg}}}&{{{\bf{U}}_{tba}}}\\ {{\bf{U}}_{\phi v}^T}&{{\bf{U}}_{tv}^T}&{{{\bf{U}}_v}}&{{{\bf{U}}_{vbg}}}&{{{\bf{U}}_{vba}}}\\ {{\bf{U}}_{\phi bg}^T}&{{\bf{U}}_{tbg}^T}&{{\bf{U}}_{vbg}^T}&{{{\bf{U}}_{bg}}}&{{{\bf{U}}_{bgba}}}\\ {{\bf{U}}_{\phi ba}^T}&{{\bf{U}}_{tba}^T}&{{\bf{U}}_{vba}^T}&{{\bf{U}}_{bgba}^T}&{{{\bf{U}}_{ba}}} \end{array}} \right] \]上述矩阵中，各对角块为含有对应状态的所有自协Hessian求和（注意如果存在对该组相机状态的先验，每个主对角块还应加入先验自协Hessian）。注意到，上述矩阵\({\bf{U}}_i\)是完全稠密的（所有非对角块都非零），这是由每一种预积分残差中相机状态各部分的联系导致的（参见式\(\eqref{1}\)，总有一种预积分残差能将某两个相机子状态联系起来）。
接下来回到大\(\bf{H}\)矩阵的层面，由于预积分将相邻帧联系到一起，因此在左上块的非主对角块中（描述相邻帧的非主对角块），将增加一些非零块\({\bf{X}}_{{i_1}{i_2}}\)，我们来看看这类矩阵的具体形式。类似前面获得式\(\eqref{3}\)的思路，我们根据\(\eqref{1}\)中预积分残差中各状态的联系来填充各个矩阵块，得到\({\bf{X}}_{{i_1}{i_2}}\)的形式如下\[ \tag{4} \label{4} {{\bf{X}}_{{i_1}{i_2}}} = \left[ {\begin{array}{*{20}{c}} {{{\bf{X}}_{{\phi _1}{\phi _2}}}}&{{{\bf{X}}_{{\phi _1}{t_2}}}}&{{{\bf{X}}_{{\phi _1}{v_2}}}}&{\bf{0}}&{\bf{0}}\\ {\bf{0}}&{{{\bf{X}}_{{t_1}{t_2}}}}&{\bf{0}}&{\bf{0}}&{\bf{0}}\\ {\bf{0}}&{{{\bf{X}}_{{v_1}{t_2}}}}&{{{\bf{X}}_{{v_1}{v_2}}}}&{\bf{0}}&{\bf{0}}\\ {{{\bf{X}}_{b{g_1}{\phi _2}}}}&{{{\bf{X}}_{b{g_1}{t_2}}}}&{{{\bf{X}}_{b{g_1}{v_2}}}}&{{{\bf{R}}_{b{g_1}b{g_2}}}}&{\bf{0}}\\ {\bf{0}}&{{{\bf{X}}_{b{a_1}{t_2}}}}&{{{\bf{X}}_{b{a_1}{v_2}}}}&{\bf{0}}&{{{\bf{R}}_{b{a_1}b{a_2}}}} \end{array}} \right] \]注意到，由于相邻帧间的部分状态没有被预积分联系起来，因此\({\bf{X}}_{{i_1}{i_2}}\)矩阵中存在不少\(\bf{0}\)块。非零块中，形如\({{\bf{X}}_{{\alpha _1}{\beta _2}}}\)的矩阵由预积分残差引起；而\({{{\bf{R}}_{b{g_1}b{g_2}}}}\)和\({{{\bf{R}}_{b{a_1}b{a_2}}}}\)则由bias的随机游走约束引起。总而言之，\({\bf{X}}_{{i_1}{i_2}}\)这个矩阵是具有稀疏性的。
最后来看\({\bf{Y}}_{ij}\)矩阵。该矩阵反映的是相机状态与地图点位置在指标（重投影误差项）中的联系。纯视觉BA中，相机状态只包括位姿，而VIO中则多出了速度和两组bias，但根据投影模型可知，新增加的这些状态与重投影误差完全无关。因此\({\bf{Y}}_{ij}\)在“本质”上实际与纯视觉BA中的\({\bf{W}}_{ij}\)矩阵是一样的。只不过由于新增加了一些状态，使得\({\bf{Y}}_{ij}\)矩阵多出了一些\(\bf{0}\)块，如下所示\[ \tag{5} \label{5} {{\bf{Y}}_{ij}} = \left[ {\begin{array}{*{20}{c}} {{{\bf{W}}_{ij}}}\\ {{{\bf{0}}_{9 \times 3}}} \end{array}} \right] \]
综上所述，VIO的BA优化中的增量方程，其相机状态进行了扩展，这导致\({\bf{U}}_i\)由6x6维变成了15x15维。而且由于增加了约束，Hessian的左上分块\(\bf{U}\)变得更稠密了。这两点将使得marg掉地图点后的降维增量方程变得更加稠密，计算量增加。
我们来细细体会一下VIO到底比纯视觉BA稠密在哪。可以想像，相机状态只考虑位姿的话，VIO较纯视觉BA而言Hessian只是增加了相邻帧位姿的联系，而在纯视觉BA中一旦把地图点marg掉之后，原本不连接的相机位姿间会产生fill-in，相邻帧一般普遍存在很多共视点，因此反映这些联系的Hessian子块大概率会产生fill-in。也就是说marg掉地图点之后，VIO的降维增量方程（只考虑相机位姿部分），其稠密性与纯视觉BA（的降维增量方程）基本是一模一样的。VIO解降维增量方程增加的计算量，主要是因为每个相机引入了新的状态，而且所有的相机状态因为预积分的约束而产生了链接。主体部分（最稠密）为单个相机新增的各状态之间（式\(\eqref{3}\)）引入的非零块，其他部分为相邻帧间与新增状态有关的非零块（式\(\eqref{4}\)）。