【原創】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.\]