【原创】VIO/VISLAM中的BA问题详解2

（转载请注明出处）

这一篇打算分析一下一般情况下，针对多源传感器融合的的非线性最小二乘优化，其增量方程的结构。

假设描述利用M个传感器信息进行融合的优化问题如下：$$
\tag{1} \label{1}
\begin{array}{l}
\min \mathop {\arg }\limits_{\bf{x}} \left( {\left| {{{\bf{f}}1}\left( {\bf{x}} \right)} \right|{{{\pmb{\Omega }}_1}}^2 + \left| {{{\bf{f}}2}\left( {\bf{x}} \right)} \right|{{{\pmb{\Omega }}2}}^2 + \cdots \left| {{{\bf{f}}M}\left( {\bf{x}} \right)} \right|{{{\pmb{\Omega }}M}}^2} \right)\
= \min \mathop {\arg }\limits{\bf{x}} \sum\limits^M {\left| {{{\bf{f}}k}\left( {\bf{x}} \right)} \right|{{{\pmb{\Omega }}_k}}^2}
\end{array}

\[其中${{{\bf{f}}_k}\left( {\bf{x}} \right)}$表示第k个传感器的所有测量残差组成的向量，${\pmb{\Omega }}_k$表示第k个传感器所有残差项的联合信息矩阵。 可以发现，当我们把所有信息矩阵写作一个大联合信息矩阵时，即\]

\tag{2} \label{2}
{\pmb{\Omega }} = \left[ {\begin{array}{*{20}{c}}
{{{\pmb{\Omega }}_1}}&{}&{}\
{}& \ddots &{}\
{}&{}&{{{\pmb{\Omega }}_M}}
\end{array}} \right]

\[则式$\eqref{1}$所示的优化问题总是能表示成下述形式 \]

\tag{3} \label{3}
\min \mathop {\arg }\limits_{\bf{x}} \left| {{\bf{f}}\left( {\bf{x}} \right)} \right|_{\pmb{\Omega }}^2

\[其中有 \]

\tag{4} \label{4}
{\bf{f}}\left( {\bf{x}} \right) = {\left[ {\begin{array}{*{20}{c}}
{{\bf{f}}_1^T\left( {\bf{x}} \right)}& \cdots &{{\bf{f}}_1^T\left( {\bf{x}} \right)}
\end{array}} \right]^T}

\[ 深入的理解了上一篇笔记后，我们应该知道，对于Gauss-Newton法，增量方程的系数矩阵将具有下述形式\]

\tag{5} \label{5}
{\bf{H}} = \left[ {\begin{array}{*{20}{c}}
{{{\bf{H}}{11}}}& \cdots &{{{\bf{H}}{1l}}}& \cdots &{{{\bf{H}}{1L}}}\
\vdots & \ddots &{}&{}& \vdots \
{{\bf{H}}^T}&{}&{{{\bf{H}}{ll}}}&{}&{{{\bf{H}}{lL}}}\
\vdots &{}&{}& \ddots & \vdots \
{{\bf{H}}{1L}^T}& \cdots &{{\bf{H}}^T}& \cdots &{{{\bf{H}}_{LL}}}
\end{array}} \right]

\[对角块元素表示每组状态（例如每个相机位姿速度为一组、每个地图点位置为一组）关于所有与其相关的测量残差（可能来自不同传感器）的近似Hessian求和。 非对角块元素，例如${{\bf{H}}_{mn}}$，则表示所有能将第m组状态和第n组状态联系起来的测量残差的近似Co-Hessian求和。当第m组状态和第n组状态没有关联时，则该块为$\bf{0}$矩阵。例如，假设第m组状态为某个相机状态，第n组状态为某个地图点位置，当该相机观测到该地图点时，则$\bf{H}$矩阵的第mxn块将为非零块；否则将为$\bf{0}$块。 一旦利用部分矩阵块进行marginalization（例如纯视觉BA中将地图点坐标marg掉），则在剩余的降维方程中，原本没有直接测量关系的状态，会因为同时与它俩有关联的状态被marg掉而建立联系。此时降维方程的系数矩阵也有着和$\bf{H}$类似的性质：主对角块仍然是对应状态组关于其所有相关测量残差的近似Hessian求和，非对角块则为两个不同组状态间的Co-Hessian求“和”（有可能做差），部分原本为$\bf{0}$矩阵的非对角块可能因为某个分别与两组状态有关联的第三个状态被marg而成为非零块。 换而言之，越进行marg，剩下的降维方程就将包含越多的信息，因此会变得越稠密。\]