（转载请注明出处）
下面来讨论使用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^{{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^{{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的关系的说明](https://stats.stackexchange.com/questions/68080/basic-question-about-fisher-information-matrix-and-relationship-to-hessian-and-s)）。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}}^T}&{{{\bf{U}}3}}&{{{\bf{Y}}{31}}}&{{{\bf{Y}}{32}}}&{{{\bf{Y}}{33}}}&{{{\bf{Y}}{34}}}\
{{\bf{Y}}T}&{{\bf{Y}}_{21}T}&{{\bf{Y}}{31}^T}&{{{\bf{V}}1}}&{\bf{0}}&{\bf{0}}&{\bf{0}}\
{{\bf{Y}}T}&{{\bf{Y}}_{22}T}&{{\bf{Y}}^T}&{\bf{0}}&{{{\bf{V}}2}}&{\bf{0}}&{\bf{0}}\
{{\bf{Y}}T}&{{\bf{Y}}_{23}T}&{{\bf{Y}}{33}^T}&{\bf{0}}&{\bf{0}}&{{{\bf{V}}3}}&{\bf{0}}\
{{\bf{Y}}T}&{{\bf{Y}}_{24}T}&{{\bf{Y}}^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}}T}&{{\bf{U}}_{tv}T}&{{{\bf{U}}v}}&{{{\bf{U}}{vbg}}}&{{{\bf{U}}{vba}}}\
{{\bf{U}}T}&{{\bf{U}}_{tbg}T}&{{\bf{U}}{vbg}^T}&{{{\bf{U}}{bg}}}&{{{\bf{U}}{bgba}}}\
{{\bf{U}}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_2}}}}&{\bf{0}}&{\bf{0}}&{\bf{0}}\
{\bf{0}}&{{{\bf{X}}{t_2}}}}&{{{\bf{X}}{{v_1}{v_2}}}}&{\bf{0}}&{\bf{0}}\
{{{\bf{X}}{\phi 2}}}}&{{{\bf{X}}{t_2}}}}&{{{\bf{X}}{b{g_1}{v_2}}}}&{{{\bf{R}}b{g_2}}}}&{\bf{0}}\
{\bf{0}}&{{{\bf{X}}{b{a_1}{t_2}}}}&{{{\bf{X}}{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}$）。\]
