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

（转载请注明出处）

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

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

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

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

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

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

\tag{2}\label{2}
{{\bf{r}}{ij}} = {{\bf{z}}{ij}} - \pmb{\pi} \left( {{{\bf{x}}{ci}},{{\bf{x}}{fj}}} \right)

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

\tag{3}\label{3}
\begin{array}{l}
\sum\limits_{i = 1}^3 {\sum\limits_{j = 1}^4 {\left| {{{\bf{z}}{ij}} - \pmb{\pi} \left( {{{\bf{x}}{ci}},{{\bf{x}}{fj}}} \right)} \right|{{{\pmb{\Omega }}{ij}}}^2} } \
= \sum\limits^3 {\sum\limits_{j = 1}^4 {\left| {{{\bf{r}}{ij}}} \right|{{{\pmb{\Omega }}_{ij}}}^2} }
\end{array}

\[令 \]

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

\[以及 \]

\tag{5}\label{5}
{\pmb{\Omega }} = \left[ {\begin{array}{*{20}{c}}
{{{\pmb{\Omega }}{11}}}&{\bf{0}}&{}& \cdots &{}&{}&{\bf{0}}\
{\bf{0}}& \ddots &{}&{}&{}&{}&{}\
{}&{}&{{{\pmb{\Omega }}{14}}}&{}&{}&{}&{}\
\vdots &{}&{}& \ddots &{}&{}& \vdots \
{}&{}&{}&{}&{{{\pmb{\Omega }}{31}}}&{}&{}\
{}&{}&{}&{}&{}& \ddots &{\bf{0}}\
{\bf{0}}&{}&{}& \cdots &{}&{\bf{0}}&{{{\pmb{\Omega }}{34}}}
\end{array}} \right]

\[则指标函数可写作 \]

\tag{6}\label{6}
\begin{array}{l}
\sum\limits_{i = 1}^3 {\sum\limits_{j = 1}^4 {\left| {{{\bf{r}}{ij}}} \right|{{{\pmb{\Omega }}{ij}}}^2} } \
= \sum\limits^3 {\sum\limits_{j = 1}^4 {{\bf{r}}{ij}^T{{\pmb{\Omega }}{ij}}{{\bf{r}}{ij}}} } \
= {{\bf{f}}^T}{\pmb{\Omega}}{\bf{f}}\
= \left| {\bf{f}} \right|{\pmb{\Omega }}^2
\end{array}

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

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

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

\tag{8}\label{8}

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

\tag{9}\label{9}
\begin{array}{l}
{\bf{f}}\left( {{{\bf{x}}^ + }} \right)\
= {\bf{f}}\left( {{{\bf{x}}^ - } \oplus \delta {\bf{x}}} \right)\
\approx {\bf{f}}\left( {{{\bf{x}}^ - }} \right) + {\bf{J}}\delta {\bf{x}}
\end{array}

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

\tag{10}\label{10}
\begin{array}{l}
{\left( {{\bf{e}} + {\bf{J}}\delta {\bf{x}}} \right)^T}{\pmb{\Omega }}\left( {{\bf{e}} + {\bf{J}}\delta {\bf{x}}} \right)\
= {{\bf{e}}^T}{\pmb{\Omega}\bf{e}} + 2{{\bf{e}}^T}{\pmb{\Omega}\bf{J}}\delta {\bf{x}} + \delta {{\bf{x}}^T}{{\bf{J}}T}{\pmb{\Omega}\bf{J}}\delta {\bf{x}}
\end{array}

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

\tag{11}\label{11}

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

\tag{12}\label{12}
{\bf{J}} = \left[ {\begin{array}{*{20}{c}}
{{{\bf{A}}{11}}}&{\bf{0}}&{\bf{0}}&{{{\bf{B}}{11}}}&{\bf{0}}&{\bf{0}}&{\bf{0}}\
{{{\bf{A}}{12}}}&{\bf{0}}&{\bf{0}}&{\bf{0}}&{{{\bf{B}}{12}}}&{\bf{0}}&{\bf{0}}\
{{{\bf{A}}{13}}}&{\bf{0}}&{\bf{0}}&{\bf{0}}&{\bf{0}}&{{{\bf{B}}{13}}}&{\bf{0}}\
{{{\bf{A}}{14}}}&{\bf{0}}&{\bf{0}}&{\bf{0}}&{\bf{0}}&{\bf{0}}&{{{\bf{B}}{14}}}\
{\bf{0}}&{{{\bf{A}}{21}}}&{\bf{0}}&{{{\bf{B}}{21}}}&{\bf{0}}&{\bf{0}}&{\bf{0}}\
{\bf{0}}&{{{\bf{A}}{22}}}&{\bf{0}}&{\bf{0}}&{{{\bf{B}}{22}}}&{\bf{0}}&{\bf{0}}\
{\bf{0}}&{{{\bf{A}}{23}}}&{\bf{0}}&{\bf{0}}&{\bf{0}}&{{{\bf{B}}{23}}}&{\bf{0}}\
{\bf{0}}&{{{\bf{A}}{24}}}&{\bf{0}}&{\bf{0}}&{\bf{0}}&{\bf{0}}&{{{\bf{B}}{24}}}\
{\bf{0}}&{\bf{0}}&{{{\bf{A}}{31}}}&{{{\bf{B}}{31}}}&{\bf{0}}&{\bf{0}}&{\bf{0}}\
{\bf{0}}&{\bf{0}}&{{{\bf{A}}{32}}}&{\bf{0}}&{{{\bf{B}}{32}}}&{\bf{0}}&{\bf{0}}\
{\bf{0}}&{\bf{0}}&{{{\bf{A}}{33}}}&{\bf{0}}&{\bf{0}}&{{{\bf{B}}{33}}}&{\bf{0}}\
{\bf{0}}&{\bf{0}}&{{{\bf{A}}{34}}}&{\bf{0}}&{\bf{0}}&{\bf{0}}&{{{\bf{B}}{34}}}
\end{array}} \right]

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

\tag{13}\label{13}

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

\tag{14}\label{14}
{\bf{H}} = {{\bf{J}}^T}{\pmb{\Omega}\bf{J}} = \left[ {\begin{array}{*{20}{c}}
{\sum\limits_{j = 1}^4 {{\bf{A}}{1j}^T{{\pmb{\Omega }}{1j}}{{\bf{A}}{1j}}} }&{\bf{0}}&{\bf{0}}&{{\bf{A}}^T{{\pmb{\Omega }}{11}}{{\bf{B}}{11}}}&{{\bf{A}}{12}^T{{\pmb{\Omega }}{12}}{{\bf{B}}{12}}}&{{\bf{A}}^T{{\pmb{\Omega }}{13}}{{\bf{B}}{13}}}&{{\bf{A}}{14}^T{{\pmb{\Omega }}{14}}{{\bf{B}}{14}}}\
{\bf{0}}&{\sum\limits^4 {{\bf{A}}{2j}^T{{\pmb{\Omega }}{2j}}{{\bf{A}}{2j}}} }&{\bf{0}}&{{\bf{A}}^T{{\pmb{\Omega }}{21}}{{\bf{B}}{21}}}&{{\bf{A}}{22}^T{{\pmb{\Omega }}{22}}{{\bf{B}}{22}}}&{{\bf{A}}^T{{\pmb{\Omega }}{23}}{{\bf{B}}{23}}}&{{\bf{A}}{24}^T{{\pmb{\Omega }}{24}}{{\bf{B}}{24}}}\
{\bf{0}}&{\bf{0}}&{\sum\limits^4 {{\bf{A}}{3j}^T{{\pmb{\Omega }}{3j}}{{\bf{A}}{3j}}} }&{{\bf{A}}^T{{\pmb{\Omega }}{31}}{{\bf{B}}{31}}}&{{\bf{A}}{32}^T{{\pmb{\Omega }}{32}}{{\bf{B}}{32}}}&{{\bf{A}}^T{{\pmb{\Omega }}{33}}{{\bf{B}}{33}}}&{{\bf{A}}{34}^T{{\pmb{\Omega }}{34}}{{\bf{B}}{34}}}\
{{\bf{B}}^T{{\pmb{\Omega }}{11}}{{\bf{A}}{11}}}&{{\bf{B}}{21}^T{{\pmb{\Omega }}{21}}{{\bf{A}}{21}}}&{{\bf{B}}^T{{\pmb{\Omega }}{31}}{{\bf{A}}{31}}}&{\sum\limits_{i = 1}^3 {{\bf{B}}{i1}^T{{\pmb{\Omega }}{i1}}{{\bf{B}}{i1}}} }&{\bf{0}}&{\bf{0}}&{\bf{0}}\
{{\bf{B}}^T{{\pmb{\Omega }}{12}}{{\bf{A}}{12}}}&{{\bf{B}}{22}^T{{\pmb{\Omega }}{22}}{{\bf{A}}{22}}}&{{\bf{B}}^T{{\pmb{\Omega }}{32}}{{\bf{A}}{32}}}&{\bf{0}}&{\sum\limits_{i = 1}^3 {{\bf{B}}{i2}^T{{\pmb{\Omega }}{i2}}{{\bf{B}}{i2}}} }&{\bf{0}}&{\bf{0}}\
{{\bf{B}}^T{{\pmb{\Omega }}{13}}{{\bf{A}}{13}}}&{{\bf{B}}{23}^T{{\pmb{\Omega }}{23}}{{\bf{A}}{23}}}&{{\bf{B}}^T{{\pmb{\Omega }}{33}}{{\bf{A}}{33}}}&{\bf{0}}&{\bf{0}}&{\sum\limits_{i = 1}^3 {{\bf{B}}{i3}^T{{\pmb{\Omega }}{i3}}{{\bf{B}}{i3}}} }&{\bf{0}}\
{{\bf{B}}^T{{\pmb{\Omega }}{14}}{{\bf{A}}{14}}}&{{\bf{B}}{24}^T{{\pmb{\Omega }}{24}}{{\bf{A}}{24}}}&{{\bf{B}}^T{{\pmb{\Omega }}{34}}{{\bf{A}}{34}}}&{\bf{0}}&{\bf{0}}&{\bf{0}}&{\sum\limits_{i = 1}^3 {{\bf{B}}{i4}^T{{\pmb{\Omega }}{i4}}{{\bf{B}}_{i4}}} }
\end{array}} \right]

\[以及 \]

\tag{15}\label{15}
{\bf{d}} = \left[ {\begin{array}{*{20}{c}}
{\sum\limits_{j = 1}^4 {{\bf{A}}{1j}^T{{\pmb{\Omega }}{1j}}{\bf{r}}{1j}^ - } }\
{\sum\limits^4 {{\bf{A}}{2j}^T{{\pmb{\Omega }}{2j}}{\bf{r}}{2j}^ - } }\
{\sum\limits^4 {{\bf{A}}{3j}^T{{\pmb{\Omega }}{3j}}{\bf{r}}{3j}^ - } }\
{\sum\limits^3 {{\bf{B}}{i1}^T{{\pmb{\Omega }}{i1}}{\bf{r}}{i1}^ - } }\
{\sum\limits^3 {{\bf{B}}{i2}^T{{\pmb{\Omega }}{i2}}{\bf{r}}{i2}^ - } }\
{\sum\limits^3 {{\bf{B}}{i3}^T{{\pmb{\Omega }}{i3}}{\bf{r}}{i3}^ - } }\
{\sum\limits^3 {{\bf{B}}{i4}^T{{\pmb{\Omega }}{i4}}{\bf{r}}_{i4}^ - } }
\end{array}} \right]

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

\tag{16}\label{16}
\left[ {\begin{array}{{20}{c}}
{\bf{U}}&{\bf{W}}\
{{{\bf{W}}^T}}&{\bf{V}}
\end{array}} \right]\left[ {\begin{array}{{c}}
{\delta {{\bf{x}}_c}}\
{\delta {{\bf{x}}_p}}
\end{array}} \right] = - \left[ {\begin{array}{*{20}{c}}
{\bf{u}}\
{\bf{v}}
\end{array}} \right]

\[其中 \]

\tag{17}\label{17}
\begin{array}{l}
{\bf{U}} = \left[ {\begin{array}{{20}{c}}
{{{\bf{U}}_1}}&{\bf{0}}&{\bf{0}}\
{\bf{0}}&{{{\bf{U}}_2}}&{\bf{0}}\
{\bf{0}}&{\bf{0}}&{{{\bf{U}}_3}}
\end{array}} \right]\
= \left[ {\begin{array}{{c}}
{\sum\limits_{j = 1}^4 {{\bf{A}}{1j}^T{{\pmb{\Omega }}{1j}}{{\bf{A}}{1j}}} }&{\bf{0}}&{\bf{0}}\
{\bf{0}}&{\sum\limits^4 {{\bf{A}}{2j}^T{{\pmb{\Omega }}{2j}}{{\bf{A}}{2j}}} }&{\bf{0}}\
{\bf{0}}&{\bf{0}}&{\sum\limits^4 {{\bf{A}}{3j}^T{{\pmb{\Omega }}{3j}}{{\bf{A}}_{3j}}} }
\end{array}} \right]
\end{array}

\[\]

\tag{18}\label{18}
\begin{array}{l}
{\bf{V}} = \left[ {\begin{array}{{20}{c}}
{{{\bf{V}}_1}}&{\bf{0}}&{\bf{0}}&{\bf{0}}\
{\bf{0}}&{{{\bf{V}}_2}}&{\bf{0}}&{\bf{0}}\
{\bf{0}}&{\bf{0}}&{{{\bf{V}}_3}}&{\bf{0}}\
{\bf{0}}&{\bf{0}}&{\bf{0}}&{{{\bf{V}}_4}}
\end{array}} \right]\
= \left[ {\begin{array}{{c}}
{\sum\limits_{i = 1}^3 {{\bf{B}}{i1}^T{{\pmb{\Omega }}{i1}}{{\bf{B}}{i1}}} }&{\bf{0}}&{\bf{0}}&{\bf{0}}\
{\bf{0}}&{\sum\limits^3 {{\bf{B}}{i2}^T{{\pmb{\Omega }}{i2}}{{\bf{B}}{i2}}} }&{\bf{0}}&{\bf{0}}\
{\bf{0}}&{\bf{0}}&{\sum\limits^3 {{\bf{B}}{i3}^T{{\pmb{\Omega }}{i3}}{{\bf{B}}{i3}}} }&{\bf{0}}\
{\bf{0}}&{\bf{0}}&{\bf{0}}&{\sum\limits^3 {{\bf{B}}{i4}^T{{\pmb{\Omega }}{i4}}{{\bf{B}}_{i4}}} }
\end{array}} \right]
\end{array}

\[\]

\tag{19}\label{19}
\begin{array}{l}
{\bf{W}} = \left[ {\begin{array}{{20}{c}}
{{{\bf{W}}{11}}}&{{{\bf{W}}{12}}}&{{{\bf{W}}{13}}}&{{{\bf{W}}{14}}}\
{{{\bf{W}}{21}}}&{{{\bf{W}}{22}}}&{{{\bf{W}}{23}}}&{{{\bf{W}}{24}}}\
{{{\bf{W}}{31}}}&{{{\bf{W}}{32}}}&{{{\bf{W}}{33}}}&{{{\bf{W}}{34}}}
\end{array}} \right]\
= \left[ {\begin{array}{{c}}
{{\bf{A}}{11}^T{{\pmb{\Omega }}{11}}{{\bf{B}}{11}}}&{{\bf{A}}^T{{\pmb{\Omega }}{12}}{{\bf{B}}{12}}}&{{\bf{A}}{13}^T{{\pmb{\Omega }}{13}}{{\bf{B}}{13}}}&{{\bf{A}}^T{{\pmb{\Omega }}{14}}{{\bf{B}}{14}}}\
{{\bf{A}}{21}^T{{\pmb{\Omega }}{21}}{{\bf{B}}{21}}}&{{\bf{A}}^T{{\pmb{\Omega }}{22}}{{\bf{B}}{22}}}&{{\bf{A}}{23}^T{{\pmb{\Omega }}{23}}{{\bf{B}}{23}}}&{{\bf{A}}^T{{\pmb{\Omega }}{24}}{{\bf{B}}{24}}}\
{{\bf{A}}{31}^T{{\pmb{\Omega }}{31}}{{\bf{B}}{31}}}&{{\bf{A}}^T{{\pmb{\Omega }}{32}}{{\bf{B}}{32}}}&{{\bf{A}}{33}^T{{\pmb{\Omega }}{33}}{{\bf{B}}{33}}}&{{\bf{A}}^T{{\pmb{\Omega }}{34}}{{\bf{B}}{34}}}
\end{array}} \right]
\end{array}

\[\]

\tag{20}\label{20}
{\bf{u}} = \left[ {\begin{array}{{20}{c}}
{{{\bf{u}}_1}}\
{{{\bf{u}}_2}}\
{{{\bf{u}}_3}}
\end{array}} \right] = \left[ {\begin{array}{{c}}
{\sum\limits_{j = 1}^4 {{\bf{A}}{1j}^T{{\bf{\Omega }}{1j}}{\bf{r}}{1j}^ - } }\
{\sum\limits^4 {{\bf{A}}{2j}^T{{\bf{\Omega }}{2j}}{\bf{r}}{2j}^ - } }\
{\sum\limits^4 {{\bf{A}}{3j}^T{{\bf{\Omega }}{3j}}{\bf{r}}_{3j}^ - } }
\end{array}} \right]

\[\]

\tag{21}\label{21}
{\bf{v}} = \left[ {\begin{array}{{20}{c}}
{{{\bf{v}}_1}}\
{{{\bf{v}}_2}}\
{{{\bf{v}}_3}}\
{{{\bf{v}}_4}}
\end{array}} \right] = \left[ {\begin{array}{{c}}
{\sum\limits_{i = 1}^3 {{\bf{B}}{i1}^T{{\pmb{\Omega }}{i1}}{\bf{r}}{i1}^ - } }\
{\sum\limits^3 {{\bf{B}}{i2}^T{{\pmb{\Omega }}{i2}}{\bf{r}}{i2}^ - } }\
{\sum\limits^3 {{\bf{B}}{i3}^T{{\pmb{\Omega }}{i3}}{\bf{r}}{i3}^ - } }\
{\sum\limits^3 {{\bf{B}}{i4}^T{{\pmb{\Omega }}{i4}}{\bf{r}}_{i4}^ - } }
\end{array}} \right]

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

\tag{22}\label{22}
\left[ {\begin{array}{{20}{c}}
{{\bf{U}} - {\bf{W}}{{\bf{V}}^{ - 1}}{{\bf{W}}^T}}&{\bf{0}}\
{{{\bf{W}}^T}}&{\bf{V}}
\end{array}} \right]\left[ {\begin{array}{{c}}
{\delta {{\bf{x}}_c}}\
{\delta {{\bf{x}}_p}}
\end{array}} \right] = - \left[ {\begin{array}{*{20}{c}}
{{\bf{u}} - {\bf{W}}{{\bf{V}}^{ - 1}}{\bf{v}}}\
{\bf{v}}
\end{array}} \right]

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

\tag{23}\label{23}
\left( {{\bf{U}} - {\bf{W}}{{\bf{V}}^{ - 1}}{{\bf{W}}^T}} \right)\delta {{\bf{x}}_c} = - \left( {{\bf{u}} - {\bf{W}}{{\bf{V}}^{ - 1}}{\bf{v}}} \right)

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

\tag{24}\label{24}
\begin{array}{l}
{\bf{S}} = {\bf{W}}{{\bf{V}}^{ - 1}}{{\bf{W}}^T}\
= \left[ {\begin{array}{*{20}{c}}
{{{\bf{S}}{11}}}&{{{\bf{S}}{12}}}&{{{\bf{S}}{13}}}\
{{\bf{S}}^T}&{{{\bf{S}}{22}}}&{{{\bf{S}}{23}}}\
{{\bf{S}}{13}^{T}&{{\bf{S}}_{23}}T}&{{{\bf{S}}{33}}}
\end{array}} \right]
\end{array}

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

\tag{25}\label{25}
{{\bf{S}}{{i_1}{i_2}}} = \sum\limits^4 {{{\bf{W}}_{{i_1}j}}{\bf{V}}j^{ - 1}{\bf{W}}j}^T}

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

\tag{26}\label{26}
{\bf{s}} = \left[ {\begin{array}{*{20}{c}}
{{{\bf{s}}_1}}\
{{{\bf{s}}_1}}\
{{{\bf{s}}_1}}
\end{array}} \right] = {\bf{W}}{{\bf{V}}^{ - 1}}{\bf{v}}

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

\tag{27}\label{27}
{{\bf{s}}i} = \sum\limits^4 {{{\bf{W}}_{ij}}{\bf{V}}_j^{ - 1}{{\bf{v}}_j}}

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

\tag{28}\label{28}

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