（轉載請注明出處）
下面來討論使用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的關系的說明](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}}{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}$）。\]
