【原创】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_{i = 1}^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}}_{14}^T}&{ \cdots \cdots }&{{\bf{r}}_{31}^T}& \cdots &{{\bf{r}}_{34}^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_{i = 1}^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} {{\bf{x}}^ + } = {{\bf{x}}^ - } \oplus \delta {\bf{x}} \]其中\(\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} {{\bf{J}}^T}{\pmb{\Omega}\bf{J}}\delta {\bf{x}} = - {{\bf{J}}^T}{\pmb{\Omega}\bf{e}} \]每一步迭代，都需要求解这样一个增量方程，可以看到，当状态很多时，这将是一个规模很大的方程。

对于视觉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}}\delta {\bf{x}} = - {\bf{d}} \]下面来分析上式中\(\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}}_{11}^T{{\pmb{\Omega }}_{11}}{{\bf{B}}_{11}}}&{{\bf{A}}_{12}^T{{\pmb{\Omega }}_{12}}{{\bf{B}}_{12}}}&{{\bf{A}}_{13}^T{{\pmb{\Omega }}_{13}}{{\bf{B}}_{13}}}&{{\bf{A}}_{14}^T{{\pmb{\Omega }}_{14}}{{\bf{B}}_{14}}}\\ {\bf{0}}&{\sum\limits_{j = 1}^4 {{\bf{A}}_{2j}^T{{\pmb{\Omega }}_{2j}}{{\bf{A}}_{2j}}} }&{\bf{0}}&{{\bf{A}}_{21}^T{{\pmb{\Omega }}_{21}}{{\bf{B}}_{21}}}&{{\bf{A}}_{22}^T{{\pmb{\Omega }}_{22}}{{\bf{B}}_{22}}}&{{\bf{A}}_{23}^T{{\pmb{\Omega }}_{23}}{{\bf{B}}_{23}}}&{{\bf{A}}_{24}^T{{\pmb{\Omega }}_{24}}{{\bf{B}}_{24}}}\\ {\bf{0}}&{\bf{0}}&{\sum\limits_{j = 1}^4 {{\bf{A}}_{3j}^T{{\pmb{\Omega }}_{3j}}{{\bf{A}}_{3j}}} }&{{\bf{A}}_{31}^T{{\pmb{\Omega }}_{31}}{{\bf{B}}_{31}}}&{{\bf{A}}_{32}^T{{\pmb{\Omega }}_{32}}{{\bf{B}}_{32}}}&{{\bf{A}}_{33}^T{{\pmb{\Omega }}_{33}}{{\bf{B}}_{33}}}&{{\bf{A}}_{34}^T{{\pmb{\Omega }}_{34}}{{\bf{B}}_{34}}}\\ {{\bf{B}}_{11}^T{{\pmb{\Omega }}_{11}}{{\bf{A}}_{11}}}&{{\bf{B}}_{21}^T{{\pmb{\Omega }}_{21}}{{\bf{A}}_{21}}}&{{\bf{B}}_{31}^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}}_{12}^T{{\pmb{\Omega }}_{12}}{{\bf{A}}_{12}}}&{{\bf{B}}_{22}^T{{\pmb{\Omega }}_{22}}{{\bf{A}}_{22}}}&{{\bf{B}}_{32}^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}}_{13}^T{{\pmb{\Omega }}_{13}}{{\bf{A}}_{13}}}&{{\bf{B}}_{23}^T{{\pmb{\Omega }}_{23}}{{\bf{A}}_{23}}}&{{\bf{B}}_{33}^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}}_{14}^T{{\pmb{\Omega }}_{14}}{{\bf{A}}_{14}}}&{{\bf{B}}_{24}^T{{\pmb{\Omega }}_{24}}{{\bf{A}}_{24}}}&{{\bf{B}}_{34}^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_{j = 1}^4 {{\bf{A}}_{2j}^T{{\pmb{\Omega }}_{2j}}{\bf{r}}_{2j}^ - } }\\ {\sum\limits_{j = 1}^4 {{\bf{A}}_{3j}^T{{\pmb{\Omega }}_{3j}}{\bf{r}}_{3j}^ - } }\\ {\sum\limits_{i = 1}^3 {{\bf{B}}_{i1}^T{{\pmb{\Omega }}_{i1}}{\bf{r}}_{i1}^ - } }\\ {\sum\limits_{i = 1}^3 {{\bf{B}}_{i2}^T{{\pmb{\Omega }}_{i2}}{\bf{r}}_{i2}^ - } }\\ {\sum\limits_{i = 1}^3 {{\bf{B}}_{i3}^T{{\pmb{\Omega }}_{i3}}{\bf{r}}_{i3}^ - } }\\ {\sum\limits_{i = 1}^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}{*{20}{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}{*{20}{c}} {\sum\limits_{j = 1}^4 {{\bf{A}}_{1j}^T{{\pmb{\Omega }}_{1j}}{{\bf{A}}_{1j}}} }&{\bf{0}}&{\bf{0}}\\ {\bf{0}}&{\sum\limits_{j = 1}^4 {{\bf{A}}_{2j}^T{{\pmb{\Omega }}_{2j}}{{\bf{A}}_{2j}}} }&{\bf{0}}\\ {\bf{0}}&{\bf{0}}&{\sum\limits_{j = 1}^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}{*{20}{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_{i = 1}^3 {{\bf{B}}_{i2}^T{{\pmb{\Omega }}_{i2}}{{\bf{B}}_{i2}}} }&{\bf{0}}&{\bf{0}}\\ {\bf{0}}&{\bf{0}}&{\sum\limits_{i = 1}^3 {{\bf{B}}_{i3}^T{{\pmb{\Omega }}_{i3}}{{\bf{B}}_{i3}}} }&{\bf{0}}\\ {\bf{0}}&{\bf{0}}&{\bf{0}}&{\sum\limits_{i = 1}^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}{*{20}{c}} {{\bf{A}}_{11}^T{{\pmb{\Omega }}_{11}}{{\bf{B}}_{11}}}&{{\bf{A}}_{12}^T{{\pmb{\Omega }}_{12}}{{\bf{B}}_{12}}}&{{\bf{A}}_{13}^T{{\pmb{\Omega }}_{13}}{{\bf{B}}_{13}}}&{{\bf{A}}_{14}^T{{\pmb{\Omega }}_{14}}{{\bf{B}}_{14}}}\\ {{\bf{A}}_{21}^T{{\pmb{\Omega }}_{21}}{{\bf{B}}_{21}}}&{{\bf{A}}_{22}^T{{\pmb{\Omega }}_{22}}{{\bf{B}}_{22}}}&{{\bf{A}}_{23}^T{{\pmb{\Omega }}_{23}}{{\bf{B}}_{23}}}&{{\bf{A}}_{24}^T{{\pmb{\Omega }}_{24}}{{\bf{B}}_{24}}}\\ {{\bf{A}}_{31}^T{{\pmb{\Omega }}_{31}}{{\bf{B}}_{31}}}&{{\bf{A}}_{32}^T{{\pmb{\Omega }}_{32}}{{\bf{B}}_{32}}}&{{\bf{A}}_{33}^T{{\pmb{\Omega }}_{33}}{{\bf{B}}_{33}}}&{{\bf{A}}_{34}^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}{*{20}{c}} {\sum\limits_{j = 1}^4 {{\bf{A}}_{1j}^T{{\bf{\Omega }}_{1j}}{\bf{r}}_{1j}^ - } }\\ {\sum\limits_{j = 1}^4 {{\bf{A}}_{2j}^T{{\bf{\Omega }}_{2j}}{\bf{r}}_{2j}^ - } }\\ {\sum\limits_{j = 1}^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}{*{20}{c}} {\sum\limits_{i = 1}^3 {{\bf{B}}_{i1}^T{{\pmb{\Omega }}_{i1}}{\bf{r}}_{i1}^ - } }\\ {\sum\limits_{i = 1}^3 {{\bf{B}}_{i2}^T{{\pmb{\Omega }}_{i2}}{\bf{r}}_{i2}^ - } }\\ {\sum\limits_{i = 1}^3 {{\bf{B}}_{i3}^T{{\pmb{\Omega }}_{i3}}{\bf{r}}_{i3}^ - } }\\ {\sum\limits_{i = 1}^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}{*{20}{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}}_{12}^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_{j = 1}^4 {{{\bf{W}}_{{i_1}j}}{\bf{V}}_j^{ - 1}{\bf{W}}_{{i_2}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_{j = 1}^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}}\delta {{\bf{x}}_p} = - {\bf{v}} - {{\bf{W}}^T}\delta {{\bf{x}}_c} \]由于\({\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.

转载于:https://www.cnblogs.com/xiaochen-qiu/p/9768949.html