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

（转载请注明出处）

这几篇笔记打算对Bundle Adjustment问题的数学描述进行深入探讨。主要是对利用非线性最小二乘（Gauss-Newton法和Levenberg-Marquardt法等）对该问题进行迭代求解时，其增量方程的形式进行剖析，分析稀疏BA的由来。

对BA问题采用由特殊（典型视觉BA问题）到一般（general最小二乘问题）再到特殊（VIO中的BA）的顺序进行分析。

本篇首先介绍典型视觉BA问题。

场景如下：假设有3帧图像对应的相机状态({{f{x}}_{ci}})（(i = 1,2,3)），以及4个地图点坐标({{f{x}}_{pj}})（(j=1,2,3,4)），先不管某帧图像是否对某个地图点有观测，我们暂时认为所有帧都可能观测到所有地图点，后面再来分析当每帧只观测到部分特征点时的情况（这将带来额外的稀疏性）。

此时，BA问题用数学描述如下$$
ag{1}label{1}
min mathop {arg }limits_{f{x}} sumlimits_{i = 1}^3 {sumlimits_{j = 1}^4 {left| {{{f{z}}{ij}} - pmb{pi} left( {{{f{x}}{ci}},{{f{x}}{fj}}} ight)} ight|{{{pmb{Omega }}_{ij}}}^2} }

[其中：$f{x}$是所有状态的集合，即${f{x}} = {left[ {egin{array}{*{20}{c}}{{f{x}}_{c1}^T}&{{f{x}}_{c2}^T}&{{f{x}}_{c3}^T}&{{f{x}}_{p1}^T}&{{f{x}}_{p2}^T}&{{f{x}}_{p3}^T}&{{f{x}}_{p4}^T}end{array}} ight]^T}$；${{{f{z}}_{ij}}}$表示第i帧对第j个地图点的观测（可以是像素坐标也可以是相机系归一化坐标【只取前两维】）；$pmb{pi}left( ullet ight)$为相机投影模型（是否包含内参取决于${{{f{z}}_{ij}}}$是否为像素坐标）；${{pmb{Omega }}_{ij}}$表示第i帧对第j个地图点观测的信息矩阵，即协方差的逆，一般认为两个维度观测独立，因此${{pmb{Omega }}_{ij}}$是一个2x2维的对角阵。 将观测与观测的计算值之差写作残差的形式，即令]

ag{2}label{2}
{{f{r}}{ij}} = {{f{z}}{ij}} - pmb{pi} left( {{{f{x}}{ci}},{{f{x}}{fj}}} ight)

[则要最小化的指标函数可写作 ]

ag{3}label{3}
egin{array}{l}
sumlimits_{i = 1}^3 {sumlimits_{j = 1}^4 {left| {{{f{z}}{ij}} - pmb{pi} left( {{{f{x}}{ci}},{{f{x}}{fj}}} ight)} ight|{{{pmb{Omega }}{ij}}}^2} }
= sumlimits{i = 1}^3 {sumlimits_{j = 1}^4 {left| {{{f{r}}{ij}}} ight|{{{pmb{Omega }}_{ij}}}^2} }
end{array}

[令 ]

ag{4}label{4}
f{f} = {f{f}}left( {f{x}} ight) = {left[ {egin{array}{*{20}{c}}
{{f{r}}{11}^T}& cdots &{{f{r}}{14}^T}&{ cdots cdots }&{{f{r}}{31}^T}& cdots &{{f{r}}{34}^T}
end{array}} ight]^T}

[以及 ]

ag{5}label{5}
{pmb{Omega }} = left[ {egin{array}{*{20}{c}}
{{{pmb{Omega }}{11}}}&{f{0}}&{}& cdots &{}&{}&{f{0}}
{f{0}}& ddots &{}&{}&{}&{}&{}
{}&{}&{{{pmb{Omega }}{14}}}&{}&{}&{}&{}
vdots &{}&{}& ddots &{}&{}& vdots
{}&{}&{}&{}&{{{pmb{Omega }}{31}}}&{}&{}
{}&{}&{}&{}&{}& ddots &{f{0}}
{f{0}}&{}&{}& cdots &{}&{f{0}}&{{{pmb{Omega }}{34}}}
end{array}} ight]

[则指标函数可写作 ]

ag{6}label{6}
egin{array}{l}
sumlimits_{i = 1}^3 {sumlimits_{j = 1}^4 {left| {{{f{r}}{ij}}} ight|{{{pmb{Omega }}{ij}}}^2} }
= sumlimits{i = 1}^3 {sumlimits_{j = 1}^4 {{f{r}}{ij}^T{{pmb{Omega }}{ij}}{{f{r}}{ij}}} }
= {{f{f}}^T}{pmb{Omega}}{f{f}}
= left| {f{f}} ight|{pmb{Omega }}^2
end{array}

[则式$eqref{1}$表示的优化问题可写作 ]

ag{7}label{7}
min mathop {arg }limits_{f{x}} left| {{f{f}}left( {f{x}} ight)} ight|_{pmb{Omega }}^2

[下面我们以用Gauss-Newton法对其进行求解为例，进行分析。 Gauss-Newton法的原理如下：该方法是一种迭代优化方法，针对上一步迭代得到的状态估计值${{{f{x}}^ - }}$，我们需要求一个增量${delta {f{x}}}$来对${{{f{x}}^ - }}$进行修正，得到新的状态估计值${{f{x}}^ + }$，从而使得指标函数更加逼近最小，状态估计值更加逼近最优。这三者之间满足下述关系]

ag{8}label{8}
{{f{x}}^ + } = {{f{x}}^ - } oplus delta {f{x}}

[其中$oplus$表示流形上的加法（考虑到姿态作为状态，其增量不能直接相加，优化时一般对旋转矩阵的切空间元素——so(3)李代数【或对位姿采用se(3)李代数】进行增量求解，在切空间上补偿增量后再转换回旋转矩阵，这个过程用流形上的加法，即$oplus$来表示）。 每一步迭代中，增量${delta {f{x}}}$的具体求法如下。对${f{f}}left( {{{f{x}}^ - } oplus delta {f{x}}} ight)$进行泰勒展开（***这里存在以${{{f{x}}^ - }}$为自变量展开的常规思路，以及以${delta {f{x}}}$为自变量展开的扰动模型两种，严谨来说应当采用扰动模型进行展开，但最终得到的形式看上去和常规思路得出的结果差不多，本文将对此不作过多分析，后文也不作类似“关于状态增量的Jacobian”这样严谨的表述。关于$oplus$符号和扰动模型将另外专门写一篇笔记来说明***），得到下述形式]

ag{9}label{9}
egin{array}{l}
{f{f}}left( {{{f{x}}^ + }} ight)
= {f{f}}left( {{{f{x}}^ - } oplus delta {f{x}}} ight)
approx {f{f}}left( {{{f{x}}^ - }} ight) + {f{J}}delta {f{x}}
end{array}

[其中$f{J}$为$f{f}$关于状态$f{x}$的Jacobian矩阵。 令${f{f}}left( {{{f{x}}^ - }} ight) = {f{e}}$，代入式eqref{9}，则指标函数可近似为]

ag{10}label{10}
egin{array}{l}
{left( {{f{e}} + {f{J}}delta {f{x}}} ight)^T}{pmb{Omega }}left( {{f{e}} + {f{J}}delta {f{x}}} ight)
= {{f{e}}^T}{pmb{Omega}f{e}} + 2{{f{e}}^T}{pmb{Omega}f{J}}delta {f{x}} + delta {{f{x}}^T}{{f{J}}T}{pmb{Omega}f{J}}delta {f{x}}
end{array}

[使上述指标函数取得极小值即是当前迭代的目标方向，取极值时指标函数关于增量${delta {f{x}}}$的导数应当为0。照此思路，对上述近似指标函数关于${delta {f{x}}}$求导并令其为0，可以得到增量方程如下 ]

ag{11}label{11}
{{f{J}}^T}{pmb{Omega}f{J}}delta {f{x}} = - {{f{J}}^T}{pmb{Omega}f{e}}

[每一步迭代，都需要求解这样一个增量方程，可以看到，当状态很多时，这将是一个规模很大的方程。 对于视觉BA来说，式$eqref{11}$所示增量方程的系数矩阵往往具有稀疏性，使得可以用特殊手段来进行加速求解。下面我们从Jacobian矩阵$f{J}$入手，来分析该矩阵的稀疏性。 根据式$eqref{4}$可知，$f{J}$可以按行分块，每一块表示残差${f{r}}_{ij}$关于状态$f{x}$的Jacobian，由于残差${f{r}}_{ij}$只与第i帧相机状态以及第j个地图点坐标有关，因此这样的每一小块Jacobian将具有稀疏的结构——只有关于对应的相机状态和地图点坐标的Jacobian部分非零。 记${f{r}}_{ij}$关于第i帧相机状态的Jadobian为${f{A}}_{ij}$（当第i帧对第j个地图点无观测时，不存在${f{r}}_{ij}$这个残差，该矩阵为$f{0}$），关于第j个地图点位置的Jacobian为${f{B}}_{ij}$（当第i帧对第j个地图点无观测时，不存在${f{r}}_{ij}$这个残差，该矩阵为$f{0}$），则Jacobian矩阵$f{J}$为]

ag{12}label{12}
{f{J}} = left[ {egin{array}{*{20}{c}}
{{{f{A}}{11}}}&{f{0}}&{f{0}}&{{{f{B}}{11}}}&{f{0}}&{f{0}}&{f{0}}
{{{f{A}}{12}}}&{f{0}}&{f{0}}&{f{0}}&{{{f{B}}{12}}}&{f{0}}&{f{0}}
{{{f{A}}{13}}}&{f{0}}&{f{0}}&{f{0}}&{f{0}}&{{{f{B}}{13}}}&{f{0}}
{{{f{A}}{14}}}&{f{0}}&{f{0}}&{f{0}}&{f{0}}&{f{0}}&{{{f{B}}{14}}}
{f{0}}&{{{f{A}}{21}}}&{f{0}}&{{{f{B}}{21}}}&{f{0}}&{f{0}}&{f{0}}
{f{0}}&{{{f{A}}{22}}}&{f{0}}&{f{0}}&{{{f{B}}{22}}}&{f{0}}&{f{0}}
{f{0}}&{{{f{A}}{23}}}&{f{0}}&{f{0}}&{f{0}}&{{{f{B}}{23}}}&{f{0}}
{f{0}}&{{{f{A}}{24}}}&{f{0}}&{f{0}}&{f{0}}&{f{0}}&{{{f{B}}{24}}}
{f{0}}&{f{0}}&{{{f{A}}{31}}}&{{{f{B}}{31}}}&{f{0}}&{f{0}}&{f{0}}
{f{0}}&{f{0}}&{{{f{A}}{32}}}&{f{0}}&{{{f{B}}{32}}}&{f{0}}&{f{0}}
{f{0}}&{f{0}}&{{{f{A}}{33}}}&{f{0}}&{f{0}}&{{{f{B}}{33}}}&{f{0}}
{f{0}}&{f{0}}&{{{f{A}}{34}}}&{f{0}}&{f{0}}&{f{0}}&{{{f{B}}{34}}}
end{array}} ight]

[我们将增量方程写作如下形式 ]

ag{13}label{13}
{f{H}}delta {f{x}} = - {f{d}}

[下面来分析上式中$f{H}$矩阵和$f{d}$矩阵的结构。 将式$eqref{5}$和式$eqref{12}$代入，同时注意到$pmb{Omega}$及其所有对角块都为对称阵，有]

ag{14}label{14}
{f{H}} = {{f{J}}^T}{pmb{Omega}f{J}} = left[ {egin{array}{*{20}{c}}
{sumlimits_{j = 1}^4 {{f{A}}{1j}^T{{pmb{Omega }}{1j}}{{f{A}}{1j}}} }&{f{0}}&{f{0}}&{{f{A}}{11}^T{{pmb{Omega }}{11}}{{f{B}}{11}}}&{{f{A}}{12}^T{{pmb{Omega }}{12}}{{f{B}}{12}}}&{{f{A}}{13}^T{{pmb{Omega }}{13}}{{f{B}}{13}}}&{{f{A}}{14}^T{{pmb{Omega }}{14}}{{f{B}}{14}}}
{f{0}}&{sumlimits{j = 1}^4 {{f{A}}{2j}^T{{pmb{Omega }}{2j}}{{f{A}}{2j}}} }&{f{0}}&{{f{A}}{21}^T{{pmb{Omega }}{21}}{{f{B}}{21}}}&{{f{A}}{22}^T{{pmb{Omega }}{22}}{{f{B}}{22}}}&{{f{A}}{23}^T{{pmb{Omega }}{23}}{{f{B}}{23}}}&{{f{A}}{24}^T{{pmb{Omega }}{24}}{{f{B}}{24}}}
{f{0}}&{f{0}}&{sumlimits{j = 1}^4 {{f{A}}{3j}^T{{pmb{Omega }}{3j}}{{f{A}}{3j}}} }&{{f{A}}{31}^T{{pmb{Omega }}{31}}{{f{B}}{31}}}&{{f{A}}{32}^T{{pmb{Omega }}{32}}{{f{B}}{32}}}&{{f{A}}{33}^T{{pmb{Omega }}{33}}{{f{B}}{33}}}&{{f{A}}{34}^T{{pmb{Omega }}{34}}{{f{B}}{34}}}
{{f{B}}{11}^T{{pmb{Omega }}{11}}{{f{A}}{11}}}&{{f{B}}{21}^T{{pmb{Omega }}{21}}{{f{A}}{21}}}&{{f{B}}{31}^T{{pmb{Omega }}{31}}{{f{A}}{31}}}&{sumlimits_{i = 1}^3 {{f{B}}{i1}^T{{pmb{Omega }}{i1}}{{f{B}}{i1}}} }&{f{0}}&{f{0}}&{f{0}}
{{f{B}}{12}^T{{pmb{Omega }}{12}}{{f{A}}{12}}}&{{f{B}}{22}^T{{pmb{Omega }}{22}}{{f{A}}{22}}}&{{f{B}}{32}^T{{pmb{Omega }}{32}}{{f{A}}{32}}}&{f{0}}&{sumlimits_{i = 1}^3 {{f{B}}{i2}^T{{pmb{Omega }}{i2}}{{f{B}}{i2}}} }&{f{0}}&{f{0}}
{{f{B}}{13}^T{{pmb{Omega }}{13}}{{f{A}}{13}}}&{{f{B}}{23}^T{{pmb{Omega }}{23}}{{f{A}}{23}}}&{{f{B}}{33}^T{{pmb{Omega }}{33}}{{f{A}}{33}}}&{f{0}}&{f{0}}&{sumlimits_{i = 1}^3 {{f{B}}{i3}^T{{pmb{Omega }}{i3}}{{f{B}}{i3}}} }&{f{0}}
{{f{B}}{14}^T{{pmb{Omega }}{14}}{{f{A}}{14}}}&{{f{B}}{24}^T{{pmb{Omega }}{24}}{{f{A}}{24}}}&{{f{B}}{34}^T{{pmb{Omega }}{34}}{{f{A}}{34}}}&{f{0}}&{f{0}}&{f{0}}&{sumlimits_{i = 1}^3 {{f{B}}{i4}^T{{pmb{Omega }}{i4}}{{f{B}}_{i4}}} }
end{array}} ight]

[以及 ]

ag{15}label{15}
{f{d}} = left[ {egin{array}{*{20}{c}}
{sumlimits_{j = 1}^4 {{f{A}}{1j}^T{{pmb{Omega }}{1j}}{f{r}}{1j}^ - } }
{sumlimits{j = 1}^4 {{f{A}}{2j}^T{{pmb{Omega }}{2j}}{f{r}}{2j}^ - } }
{sumlimits{j = 1}^4 {{f{A}}{3j}^T{{pmb{Omega }}{3j}}{f{r}}{3j}^ - } }
{sumlimits{i = 1}^3 {{f{B}}{i1}^T{{pmb{Omega }}{i1}}{f{r}}{i1}^ - } }
{sumlimits{i = 1}^3 {{f{B}}{i2}^T{{pmb{Omega }}{i2}}{f{r}}{i2}^ - } }
{sumlimits{i = 1}^3 {{f{B}}{i3}^T{{pmb{Omega }}{i3}}{f{r}}{i3}^ - } }
{sumlimits{i = 1}^3 {{f{B}}{i4}^T{{pmb{Omega }}{i4}}{f{r}}_{i4}^ - } }
end{array}} ight]

[其中${f{r}}_{ij}^{-}$表示第i帧关于第j个地图点的残差在状态为${f{x}}^{-}$时的值。 根据式$eqref{14}$和式$eqref{15}$，我们将式$eqref{13}$表示的增量方程进行如下分块]

ag{16}label{16}
left[ {egin{array}{{20}{c}}
{f{U}}&{f{W}}
{{{f{W}}^T}}&{f{V}}
end{array}} ight]left[ {egin{array}{{20}{c}}
{delta {{f{x}}_c}}
{delta {{f{x}}_p}}
end{array}} ight] = - left[ {egin{array}{*{20}{c}}
{f{u}}
{f{v}}
end{array}} ight]

[其中 ]

ag{17}label{17}
egin{array}{l}
{f{U}} = left[ {egin{array}{{20}{c}}
{{{f{U}}_1}}&{f{0}}&{f{0}}
{f{0}}&{{{f{U}}_2}}&{f{0}}
{f{0}}&{f{0}}&{{{f{U}}_3}}
end{array}} ight]
= left[ {egin{array}{{20}{c}}
{sumlimits_{j = 1}^4 {{f{A}}{1j}^T{{pmb{Omega }}{1j}}{{f{A}}{1j}}} }&{f{0}}&{f{0}}
{f{0}}&{sumlimits{j = 1}^4 {{f{A}}{2j}^T{{pmb{Omega }}{2j}}{{f{A}}{2j}}} }&{f{0}}
{f{0}}&{f{0}}&{sumlimits{j = 1}^4 {{f{A}}{3j}^T{{pmb{Omega }}{3j}}{{f{A}}_{3j}}} }
end{array}} ight]
end{array}

[]

ag{18}label{18}
egin{array}{l}
{f{V}} = left[ {egin{array}{{20}{c}}
{{{f{V}}_1}}&{f{0}}&{f{0}}&{f{0}}
{f{0}}&{{{f{V}}_2}}&{f{0}}&{f{0}}
{f{0}}&{f{0}}&{{{f{V}}_3}}&{f{0}}
{f{0}}&{f{0}}&{f{0}}&{{{f{V}}_4}}
end{array}} ight]
= left[ {egin{array}{{20}{c}}
{sumlimits_{i = 1}^3 {{f{B}}{i1}^T{{pmb{Omega }}{i1}}{{f{B}}{i1}}} }&{f{0}}&{f{0}}&{f{0}}
{f{0}}&{sumlimits{i = 1}^3 {{f{B}}{i2}^T{{pmb{Omega }}{i2}}{{f{B}}{i2}}} }&{f{0}}&{f{0}}
{f{0}}&{f{0}}&{sumlimits{i = 1}^3 {{f{B}}{i3}^T{{pmb{Omega }}{i3}}{{f{B}}{i3}}} }&{f{0}}
{f{0}}&{f{0}}&{f{0}}&{sumlimits{i = 1}^3 {{f{B}}{i4}^T{{pmb{Omega }}{i4}}{{f{B}}_{i4}}} }
end{array}} ight]
end{array}

[]

ag{19}label{19}
egin{array}{l}
{f{W}} = left[ {egin{array}{{20}{c}}
{{{f{W}}{11}}}&{{{f{W}}{12}}}&{{{f{W}}{13}}}&{{{f{W}}{14}}}
{{{f{W}}{21}}}&{{{f{W}}{22}}}&{{{f{W}}{23}}}&{{{f{W}}{24}}}
{{{f{W}}{31}}}&{{{f{W}}{32}}}&{{{f{W}}{33}}}&{{{f{W}}{34}}}
end{array}} ight]
= left[ {egin{array}{{20}{c}}
{{f{A}}{11}^T{{pmb{Omega }}{11}}{{f{B}}{11}}}&{{f{A}}{12}^T{{pmb{Omega }}{12}}{{f{B}}{12}}}&{{f{A}}{13}^T{{pmb{Omega }}{13}}{{f{B}}{13}}}&{{f{A}}{14}^T{{pmb{Omega }}{14}}{{f{B}}{14}}}
{{f{A}}{21}^T{{pmb{Omega }}{21}}{{f{B}}{21}}}&{{f{A}}{22}^T{{pmb{Omega }}{22}}{{f{B}}{22}}}&{{f{A}}{23}^T{{pmb{Omega }}{23}}{{f{B}}{23}}}&{{f{A}}{24}^T{{pmb{Omega }}{24}}{{f{B}}{24}}}
{{f{A}}{31}^T{{pmb{Omega }}{31}}{{f{B}}{31}}}&{{f{A}}{32}^T{{pmb{Omega }}{32}}{{f{B}}{32}}}&{{f{A}}{33}^T{{pmb{Omega }}{33}}{{f{B}}{33}}}&{{f{A}}{34}^T{{pmb{Omega }}{34}}{{f{B}}{34}}}
end{array}} ight]
end{array}

[]

ag{20}label{20}
{f{u}} = left[ {egin{array}{{20}{c}}
{{{f{u}}_1}}
{{{f{u}}_2}}
{{{f{u}}_3}}
end{array}} ight] = left[ {egin{array}{{20}{c}}
{sumlimits_{j = 1}^4 {{f{A}}{1j}^T{{f{Omega }}{1j}}{f{r}}{1j}^ - } }
{sumlimits{j = 1}^4 {{f{A}}{2j}^T{{f{Omega }}{2j}}{f{r}}{2j}^ - } }
{sumlimits{j = 1}^4 {{f{A}}{3j}^T{{f{Omega }}{3j}}{f{r}}_{3j}^ - } }
end{array}} ight]

[]

ag{21}label{21}
{f{v}} = left[ {egin{array}{{20}{c}}
{{{f{v}}_1}}
{{{f{v}}_2}}
{{{f{v}}_3}}
{{{f{v}}_4}}
end{array}} ight] = left[ {egin{array}{{20}{c}}
{sumlimits_{i = 1}^3 {{f{B}}{i1}^T{{pmb{Omega }}{i1}}{f{r}}{i1}^ - } }
{sumlimits{i = 1}^3 {{f{B}}{i2}^T{{pmb{Omega }}{i2}}{f{r}}{i2}^ - } }
{sumlimits{i = 1}^3 {{f{B}}{i3}^T{{pmb{Omega }}{i3}}{f{r}}{i3}^ - } }
{sumlimits{i = 1}^3 {{f{B}}{i4}^T{{pmb{Omega }}{i4}}{f{r}}_{i4}^ - } }
end{array}} ight]

[如果采用常规思路蛮干，这时直接对$f{H}$矩阵求逆即可解出状态增量，但BA问题中，由于相机帧数和地图点数往往很多（尤其是地图点数量），直接对$f{H}$矩阵求逆运算量非常大，对于要求实时性的SLAM问题是不可取的。一般情况下，BA问题中的地图点个数将远多于相机位状态数（图像帧数）。此时，对于式$eqref{16}$，可使用Schur Complement将地图点先marginalize掉，从而先行求解相机状态的增量，然后再回代求解地图点位置的增量。这一方法利用了$f{H}$矩阵的稀疏性，避免了对一个规模巨大的矩阵直接求逆，从而大大提高了BA问题的优化求解速度。 使用Schur Complement进行marginalization后，式$eqref{16}$变成如下形式]

ag{22}label{22}
left[ {egin{array}{{20}{c}}
{{f{U}} - {f{W}}{{f{V}}^{ - 1}}{{f{W}}^T}}&{f{0}}
{{{f{W}}^T}}&{f{V}}
end{array}} ight]left[ {egin{array}{{20}{c}}
{delta {{f{x}}_c}}
{delta {{f{x}}_p}}
end{array}} ight] = - left[ {egin{array}{*{20}{c}}
{{f{u}} - {f{W}}{{f{V}}^{ - 1}}{f{v}}}
{f{v}}
end{array}} ight]

[这个过程的计算量主要耗费在对$f{V}$阵求逆，但注意到$f{V}$阵的每个对角块是一个3x3维方阵，因此计算量不会特别大。 此时，面临着求解下面这个方程的问题]

ag{23}label{23}
left( {{f{U}} - {f{W}}{{f{V}}^{ - 1}}{{f{W}}^T}} ight)delta {{f{x}}_c} = - left( {{f{u}} - {f{W}}{{f{V}}^{ - 1}}{f{v}}} ight)

[求解这个方程将占据每次迭代计算的绝大部分计算量。 下面我们来看看这个方程的系数矩阵（即${{f{U}} - {f{W}}{{f{V}}^{ - 1}}{{f{W}}^T}}$）的构造。首先，根据式$eqref{17}$$eqref{18}$$eqref{19}$可知，${{f{U}} - {f{W}}{{f{V}}^{ - 1}}{{f{W}}^T}}$将是一个对称阵。其主对角块（一般）非零，由于$f{U}$是一个对角块矩阵，因此${{f{U}} - {f{W}}{{f{V}}^{ - 1}}{{f{W}}^T}}$的非主对角块部分由矩阵${f{S}} = {f{W}}{{f{V}}^{ - 1}}{{f{W}}^T}$矩阵的非主对角块决定。我们来看矩阵$f{S}$的结构，令]

ag{24}label{24}
egin{array}{l}
{f{S}} = {f{W}}{{f{V}}^{ - 1}}{{f{W}}^T}
= left[ {egin{array}{*{20}{c}}
{{{f{S}}{11}}}&{{{f{S}}{12}}}&{{{f{S}}{13}}}
{{f{S}}{12}^T}&{{{f{S}}{22}}}&{{{f{S}}{23}}}
{{f{S}}{13}^{T}&{{f{S}}_{23}}T}&{{{f{S}}{33}}}
end{array}} ight]
end{array}

[代入式$eqref{17}$$eqref{18}$$eqref{19}$，有 ]

ag{25}label{25}
{{f{S}}{{i_1}{i_2}}} = sumlimits{j = 1}^4 {{{f{W}}_{{i_1}j}}{f{V}}j^{ - 1}{f{W}}{{i_2}j}^T}

[式$eqref{23}$的右边为${{f{u}} - {f{W}}{{f{V}}^{ - 1}}{f{v}}}$，我们再来看看它的每个元素块如何构成。令 ]

ag{26}label{26}
{f{s}} = left[ {egin{array}{*{20}{c}}
{{{f{s}}_1}}
{{{f{s}}_1}}
{{{f{s}}_1}}
end{array}} ight] = {f{W}}{{f{V}}^{ - 1}}{f{v}}

[代入式$eqref{18}$$eqref{19}$$eqref{21}$，可得 ]

ag{27}label{27}
{{f{s}}i} = sumlimits{j = 1}^4 {{{f{W}}_{ij}}{f{V}}_j^{ - 1}{{f{v}}_j}}

[注意不要忘记代回$ - left( {{f{u}} - {f{s}}} ight)$中 我们可以发现，当BA中所有相机帧对所有地图点都有观测时，式$eqref{23}$将是完全稠密的。但对于很多情况而言，BA中大多数情况下并不是所有帧都对所有地图点有观测。后面的分析中我们将得知，当某两帧间没有共视地图点时，将为${f{U}}-{f{S}}$矩阵带来稀疏性。 对于式$eqref{23}$所示的有一定稀疏性的方程，一般采用Cholesky Fractorization或者Preconditioned Conjugate Gradient（PCG）方法进行求解，这些方法可以在一定程度上利用有限的稀疏性进行加速求解。 求解出相机状态的增量后，再回代到下述方程中则可求解得到地图点位置增量]

ag{28}label{28}
{f{V}}delta {{f{x}}_p} = - {f{v}} - {{f{W}}^T}delta {{f{x}}_c}

[由于${f{V}}^{-1}$之前已经求解过，所以这一步的计算量非常小。 下面来分析一下当并不是每一帧都能观测到所有地图点时，式$eqref{13}$和$eqref{23}$的稀疏性。 我们这样来思考，无论某个帧对某地图点是否有观测，我们都将相应的残差项加入到整个指标函数（式$eqref{1}$）的计算中，只不过没有观测时残差始终为0，即式$eqref{4}$中相应的${f{r}}_{ij}$为$f{0}$矩阵。假设一共有m组相机状态以及n个地图点，则此时式$eqref{12}$所表示的Jacobian矩阵$f{J}$将由(mn)x(m+n)个块组成。如果某个残差${f{r}}_{ij}$不存在，那么它对对应的第i帧和第j个地图点的Jacobian（${f{A}}_{ij}$和${f{B}}_{ij}$）也为0。（此外，信息矩阵中对应的${pmb{Omega}}$也可以认为为$f{0}$，但它是否为$f{0}$已经不重要） * 某些帧不包含某些地图点时，式$eqref{13}$的稀疏性 （1）$f{H}$矩阵 矩阵$f{H}$由$f{J}$和$pmb{Omega}$构成（参考式$eqref{14}$$eqref{16}$$eqref{17}$$eqref{18}$$eqref{19}$）。 对于$f{U}$矩阵，根据其元素块的形式可知（式$eqref{17}$），其每一个对角块可理解为“**对应的相机状态关于所有其能观测到的地图点的测量残差的近似Hessian求和**”（此时对角块中应当改为对$j in {{cal P}_i}$求和，其中${{cal P}_i}$表示第i帧能观测到的所有地图点的集合），由于出现在BA中的相机帧是一定有观测地图点的，因此这些对角块将始终非零（不考虑数值原因导致的零）。 对于$|bf{V}$矩阵，根据其元素块的形式可知（式$eqref{18}$），其每一个对角块可理解为“**对应的地图点关于所有能观测到它的相机状态的测量残差的近似Hessian求和**”（此时对角块中应当改为对$i in {{cal V}_j}$求和，其中${{cal V}_j}$表示能观测到第j个地图点的所有帧的集合），由于出现在BA中的地图点是一定能被相机帧观测到的，因此这些对角块也将始终非零（不考虑数值原因导致的零）。 对于$f{W}$矩阵，根据其元素块的形式可知（式$eqref{19}$），其中${f{W}}_{ij}$矩阵块是否为$f{0}$，由第i帧是否对第j个地图点有观测决定，若无观测则${f{A}}_ij$和${f{B}}_ij$都为$f{0}$，于是对应的${f{W}}_{ij}$为$f{0}$。 也就是说在BA中某些相机状态与地图点不存在联系时将对$f{H}$矩阵带来额外的稀疏性。 （2）$f{d}$矩阵 矩阵$f{d}$由$f{J}$、$pmb{Omega}$以及上一步迭代的残差构成（参考式$eqref{15}$$eqref{16}$$eqref{20}$$eqref{21}$）。 类似对$f{U}$的分析，$f{u}$的每一个行矩阵块可理解为“**对应的相机状态关于所有其能观测到的地图点的测量残差近似Hessian求和**”（各矩阵块应当改为对$j in {{cal P}_i}$求和），因此这些块将始终非零。 类似对于$f{V}$的分析，$f{v}$的每一个行矩阵可理解为“**对应的地图点关于所有能观测到它的相机状态的测量残差近似Hessian求和**”（各矩阵块应当改为对$i in {{cal V}_j}$求和），因此这些块也将始终非零。 BA中某些相机与地图点不存在联系，对$f{d}$的稀疏性没有影响，它始终是稠密的。 * 某些帧不包含某些地图点时，式$eqref{23}$的稀疏性 （1）${f{U}} - {f{S}}$矩阵 根据前面的分析可知，矩阵${f{U}} - {f{S}}$的主对角元素将始终为非零块（由$f{U}$主导）。 根据式$eqref{19}$可知，当第i帧对第j个地图点没有观测时（此时${f{A}}_{ij}$和${f{B}}_{ij}$都为$f{0}$），${f{W}}_{ij}$为$f{0}$。因此，考虑式$eqref{25}$的形式（此时应当改为对$j in {{cal P}_{{i_1}}} cap {{cal P}_{{i_2}}}$求和，${{cal P}_{{i_1}}} cap {{cal P}_{{i_2}}}$表示第$i_1$帧和第$i_2$帧共视地图点的集合），只有当第$i_1$帧和第$i_2$帧完全没有共视地图点时，${f{S}}_{{i_1}{i_2}}$才会为$f{0}$。 由此可知，矩阵${f{U}} - {f{S}}$的稀疏性取决于**是否有不同的两帧间完全没有共视地图点**。在对小范围（比如滑动时间窗）进行BA解算时，一般帧与帧间将共享大量共视点，此时前述矩阵将是不怎么稀疏的；对于场景跨越幅度较大的BA解算，则前述矩阵将比较稀疏。 （2）${f{u}} - {f{s}}$矩阵 根据式$eqref{26}$$eqref{27}$可知，$f{s}$的每一个元素可理解为“**对应的相机状态关于所有其能观测到的地图点求和**”（各矩阵块应当改为对$j in {{cal P}_i}$求和），这些块将始终非零。因此矩阵${f{u}} - {f{s}}$将始终是一个稠密矩阵。 综上所述，在纯视觉BA优化时，每次迭代过程中的步骤如下： (1) 求取每一个重投影残差关于对应帧和地图点的Jacobian，按照式$eqref{12}$的形式归类，对于无联系的帧和地图点，对应位置填$f{0}$（主要是归类，并不一定要把这样一个$f{J}$矩阵写出来）； (2) 按照式$eqref{17}$～$eqref{21}$和式$eqref{24}$～$eqref{27}$（注意应更正求和的对象），构造式$eqref{23}$代表的方程； (3) 对上述方程进行求解，得到增量$delta {{f{x}}_c}$； (4) 将$delta {{f{x}}_c}$其回代到式$eqref{28}$中，得到增量$delta {{f{x}}_p}$。 上述步骤中，最主要的计算量来自于(3)，即求解方程式$eqref{23}$。该式的系数矩阵其主对角块是非零的，其他非对角块是否为零，取决于对应的两帧是否完全没有共视地图点。当帧间联系不紧密时（指容易出现无共视地图点的两帧），该方程是稀疏的，可以利用Cholesky Fractorization或者PCG方法进行求解。而当帧间联系紧密时，该方程是比较稠密的，将退化为一般情况。 【参考文献】 【1】《视觉SLAM十四讲》——高翔 【2】Liu H, Li C, Zhang G, et al. Robust Keyframe-based Dense SLAM with an RGB-D Camera[J]. arXivpreprint arXiv:1711.05166, 2017.]