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

（转载请注明出处）

下面来讨论使用IMU预积分框架的VIO/VISLAM，在进行优化求解时，其增量方程的结构。

不失一般性，我们不区分相机状态和IMU状态（暂时不考虑优化IMU-CAM外参。可以认为预积分是将IMU数据进行坐标转换后在相机系下进行的；或者认为对地图点的观测经坐标系转换后表达在载体系下）。

优化问题表述如下：$$
ag{1} label{1}
egin{array}{*{20}{l}}
{min mathop {arg }limits_{f{x}} left{ {sumlimits_{i = 1}^{{N_c}} {sumlimits_{j in {P_i}} {left| {{{f{f}}{vis}}left( {{{f{R}}i},{{f{t}}i},{{f{p}}j}} ight)} ight|{{{pmb{Omega }}{vis}}}^2} } + sumlimits{i = 1}^{{N_c} - 1} {left| {{{f{f}}{Delta R}}left( {{{f{R}}i},{{f{R}}{i + 1}},delta {f{b}}{g_i}} ight)} ight|{{{pmb{Omega }}{Delta R}}}^2} } ight.}
{ + sumlimits_{i = 1}^{{N_c} - 1} {left| {{{f{f}}{Delta v}}left( {{{f{R}}i},{{f{v}}i},delta {f{b}}{g_i},delta {f{b}}{a_i},{{f{v}}{i + 1}}} ight)} ight|{{{pmb{Omega }}{Delta v}}}^2} + sumlimits_{i = 1}^{{N_c} - 1} {left| {{{f{f}}{Delta t}}left( {{{f{R}}i},{{f{t}}i},{{f{v}}i},delta {f{b}}{g_i},delta {f{b}}{a_i},{{f{t}}{i + 1}}} ight)} ight|{{{pmb{Omega }}{Delta t}}}^2} }
{left. { + sumlimits{i = 1}^{{N_c} - 2} {left| {{f{b}}{g_{i + 1}} - {f{b}}{g_i}} ight|{{{pmb{Omega }}{bg}}}^2} + sumlimits_{i = 1}^{{N_c} - 2} {left| {{f{b}}{a_{i + 1}} - {f{b}}{a_i}} ight|{{{pmb{Omega }}{ba}}}^2} + left| {{{f{f}}{prior}}left( {f{x}} ight)} ight|{{{pmb{Omega }}_{prior}}}^2} ight}}
end{array}

[其中，$N_c$表示参与优化的帧（关键帧）数；${cal P}_i$表示第i帧可以观测到的地图点的集合；由于用符号$f{p}$描述了地图点坐标，因此用$f{t}$描述相机位置，位置的预积分符号也相应的改为$Delta f{t}$。 ${{{pmb{Omega }}_{vis}}}$表示重投影误差信息矩阵（2x2维）。 ${{{pmb{Omega }}_{Delta R}}}$表示相邻关键帧间的旋转预积分（$Delta f{R}$）的信息矩阵；${{{pmb{Omega }}_{Delta v}}}$表示相邻关键帧间的速度预积分（$Delta f{v}$）的信息矩阵；${{{pmb{Omega }}_{Delta t}}}$表示相邻关键帧间的位置预积分（$Delta f{p}$）的信息矩阵。这三个信息矩阵参见《预积分总结与公式推导》的预积分噪声协方差部分，在Forster的原始paper中，推导了一个三者整合的协方差递推方程。 ${{{pmb{Omega }}_{bg}}}$和${{{pmb{Omega }}_{ba}}}$分别表示陀螺和加计bias的随机游走信息矩阵，它们的逆是帧间bias随机游走系数对应的高斯分布的协方差矩阵。这两个信息矩阵对应的指标项的意义，是表示每一帧对应的imu传感器的bias满足随机游走，也即相邻帧的bias之差满足高斯分布（最原本的是相邻两条imu测量的bias之间满足随机游走）。 ${{{f{Omega }}_{prior}}}$表示先验约束的信息矩阵。对于初始化来说，该信息矩阵代表了初始帧的初始状态的可信度（因此$f{x}$里只包括初始帧的状态们）。对于滑动窗VIO的BA来说，这个信息矩阵由如下方式获得：上一个滑动窗BA完成之后，在大$f{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组相机状态${{f{x}}_{c_i}}$（$i = 1,2,3$，每组包括姿态$f{R}$、位置$f{t}$、速度$f{v}$以及对应时刻的惯性器件bias——${f{b}}_g$和${f{b}}_a$，其中bias使用的是误差量作为状态）和4个地图点位置${f{p}}_j$（$ij= 1,2,3,4$）。除了“帧观测到地图点”这种传统约束外，增加了相邻帧间的预积分约束，以及首帧的prior约束（暂时不考虑prior更复杂的情况）。 根据前两篇笔记对BA问题分析的结果，直接来看Hessian矩阵的结构，如下]

ag{2} label{2}
{f{H}} = left[ {egin{array}{*{20}{c}}
{{{f{U}}1}}&{{{f{X}}{12}}}&{f{0}}&{{{f{Y}}{11}}}&{{{f{Y}}{12}}}&{{{f{Y}}{13}}}&{{{f{Y}}{14}}}
{{f{X}}{12}^T}&{{{f{U}}2}}&{{{f{X}}{23}}}&{{{f{Y}}{21}}}&{{{f{Y}}{22}}}&{{{f{Y}}{23}}}&{{{f{Y}}{24}}}
{f{0}}&{{f{X}}{23}^T}&{{{f{U}}3}}&{{{f{Y}}{31}}}&{{{f{Y}}{32}}}&{{{f{Y}}{33}}}&{{{f{Y}}{34}}}
{{f{Y}}{11}^{T}&{{f{Y}}_{21}}T}&{{f{Y}}{31}^T}&{{{f{V}}1}}&{f{0}}&{f{0}}&{f{0}}
{{f{Y}}{12}^{T}&{{f{Y}}_{22}}T}&{{f{Y}}{32}^T}&{f{0}}&{{{f{V}}2}}&{f{0}}&{f{0}}
{{f{Y}}{13}^{T}&{{f{Y}}_{23}}T}&{{f{Y}}{33}^T}&{f{0}}&{f{0}}&{{{f{V}}3}}&{f{0}}
{{f{Y}}{14}^{T}&{{f{Y}}_{24}}T}&{{f{Y}}{34}^T}&{f{0}}&{f{0}}&{f{0}}&{{{f{V}}_4}}
end{array}} ight]

[其中，${f{V}}_j$表示第j个地图点的自协Hessian（关于所有能观测到它的相机位姿的Hessian求和），与纯视觉BA中完全一致。 ${f{U}}_i$表示第i个相机状态的自协Hessian，由于相机状态多增加了速度和bias，并且增加了帧间预积分以及bias的随机游走约束（首帧还增加了prior约束），这里与纯视觉中有所不同。我们来看一下${f{U}}_i$的构造，按照姿态、位置、速度、陀螺bias、加计bias的顺序进行分块，只要某块状态和另一块状态在$eqref{1}$指标中产生了直接联系，我们就将对应非主对角块进行填充，否则该块将为$f{0}$矩阵。于是可得其构造如下]

ag{3} label{3}
{{f{U}}i} = left[ {egin{array}{*{20}{c}}
{{{f{U}}phi }}&{{{f{U}}{phi t}}}&{{{f{U}}{phi v}}}&{{{f{U}}{phi bg}}}&{{{f{U}}{phi ba}}}
{{f{U}}{phi t}^T}&{{{f{U}}t}}&{{{f{U}}{tv}}}&{{{f{U}}{tbg}}}&{{{f{U}}{tba}}}
{{f{U}}{phi v}^{T}&{{f{U}}_{tv}}T}&{{{f{U}}v}}&{{{f{U}}{vbg}}}&{{{f{U}}{vba}}}
{{f{U}}{phi bg}^{T}&{{f{U}}_{tbg}}T}&{{f{U}}{vbg}^T}&{{{f{U}}{bg}}}&{{{f{U}}{bgba}}}
{{f{U}}{phi ba}^{T}&{{f{U}}_{tba}}T}&{{f{U}}{vba}^{T}&{{f{U}}_{bgba}}T}&{{{f{U}}{ba}}}
end{array}} ight]

[上述矩阵中，各对角块为含有对应状态的所有自协Hessian求和（注意如果存在对该组相机状态的先验，每个主对角块还应加入先验自协Hessian）。注意到，上述矩阵${f{U}}_i$是完全稠密的（所有非对角块都非零），这是由每一种预积分残差中相机状态各部分的联系导致的（参见式$eqref{1}$，总有一种预积分残差能将某两个相机子状态联系起来）。 接下来回到大$f{H}$矩阵的层面，由于预积分将相邻帧联系到一起，因此在左上块的非主对角块中（描述相邻帧的非主对角块），将增加一些非零块${f{X}}_{{i_1}{i_2}}$，我们来看看这类矩阵的具体形式。类似前面获得式$eqref{3}$的思路，我们根据$eqref{1}$中预积分残差中各状态的联系来填充各个矩阵块，得到${f{X}}_{{i_1}{i_2}}$的形式如下]

ag{4} label{4}
{{f{X}}{{i_1}{i_2}}} = left[ {egin{array}{*{20}{c}}
{{{f{X}}{{phi 1}{phi 2}}}}&{{{f{X}}{{phi 1}{t_2}}}}&{{{f{X}}{{phi 1}{v_2}}}}&{f{0}}&{f{0}}
{f{0}}&{{{f{X}}{{t_1}{t_2}}}}&{f{0}}&{f{0}}&{f{0}}
{f{0}}&{{{f{X}}{{v_1}{t_2}}}}&{{{f{X}}{{v_1}{v_2}}}}&{f{0}}&{f{0}}
{{{f{X}}{b{g_1}{phi 2}}}}&{{{f{X}}{b{g_1}{t_2}}}}&{{{f{X}}{b{g_1}{v_2}}}}&{{{f{R}}{b{g_1}b{g_2}}}}&{f{0}}
{f{0}}&{{{f{X}}{b{a_1}{t_2}}}}&{{{f{X}}{b{a_1}{v_2}}}}&{f{0}}&{{{f{R}}_{b{a_1}b{a_2}}}}
end{array}} ight]

[注意到，由于相邻帧间的部分状态没有被预积分联系起来，因此${f{X}}_{{i_1}{i_2}}$矩阵中存在不少$f{0}$块。非零块中，形如${{f{X}}_{{alpha _1}{eta _2}}}$的矩阵由预积分残差引起；而${{{f{R}}_{b{g_1}b{g_2}}}}$和${{{f{R}}_{b{a_1}b{a_2}}}}$则由bias的随机游走约束引起。总而言之，${f{X}}_{{i_1}{i_2}}$这个矩阵是具有稀疏性的。 最后来看${f{Y}}_{ij}$矩阵。该矩阵反映的是相机状态与地图点位置在指标（重投影误差项）中的联系。纯视觉BA中，相机状态只包括位姿，而VIO中则多出了速度和两组bias，但根据投影模型可知，新增加的这些状态与重投影误差完全无关。因此${f{Y}}_{ij}$在“本质”上实际与纯视觉BA中的${f{W}}_{ij}$矩阵是一样的。只不过由于新增加了一些状态，使得${f{Y}}_{ij}$矩阵多出了一些$f{0}$块，如下所示]

ag{5} label{5}
{{f{Y}}{ij}} = left[ {egin{array}{*{20}{c}}
{{{f{W}}{ij}}}
{{{f{0}}_{9 imes 3}}}
end{array}} ight]

[ 综上所述，VIO的BA优化中的增量方程，其相机状态进行了扩展，这导致${f{U}}_i$由6x6维变成了15x15维。而且由于增加了约束，Hessian的左上分块$f{U}$变得更稠密了。这两点将使得marg掉地图点后的降维增量方程变得更加稠密，计算量增加。 我们来细细体会一下VIO到底比纯视觉BA稠密在哪。可以想像，相机状态只考虑位姿的话，VIO较纯视觉BA而言Hessian只是增加了相邻帧位姿的联系，而在纯视觉BA中一旦把地图点marg掉之后，原本不连接的相机位姿间会产生fill-in，相邻帧一般普遍存在很多共视点，因此反映这些联系的Hessian子块大概率会产生fill-in。也就是说marg掉地图点之后，VIO的降维增量方程（只考虑相机位姿部分），其稠密性与纯视觉BA（的降维增量方程）基本是一模一样的。VIO解降维增量方程增加的计算量，主要是因为每个相机引入了新的状态，而且所有的相机状态因为预积分的约束而产生了链接。主体部分（最稠密）为单个相机新增的各状态之间（式$eqref{3}$）引入的非零块，其他部分为相邻帧间与新增状态有关的非零块（式$eqref{4}$）。]