深刻理解圖優化與g2o：圖優化篇

時間 2019-12-09

標籤深刻理解優化 g2o 简体版

原文原文鏈接

前言

　　本節咱們將深刻介紹視覺slam中的主流優化方法——圖優化（graph-based optimization）。下一節中，介紹一下很是流行的圖優化庫：g2o。算法

　　關於g2o，我13年寫過一個文檔，然而隨着本身理解的加深，愈加感受不滿意。本着對讀者更負責任的精神，本文給你們從新講一遍圖優化和g2o。除了這篇文檔，讀者還能夠找到一篇關於圖優化的博客： http://blog.csdn.net/heyijia0327 那篇文章有做者介紹的一個簡單案例，而本文則更注重對圖優化和g2o的理解與評註。數據庫

　　本節主要介紹圖優化的數學理論，下節再講g2o的組成方式及使用方法。app

預備知識：優化

　　圖優化本質上是一個優化問題，因此咱們先來看優化問題是什麼。機器學習

　　優化問題有三個最重要的因素：目標函數、優化變量、優化約束。一個簡單的優化問題能夠描述以下：\[ \begin{equation} \min\limits_{x} F(x) \end{equation}\] 其中$x$爲優化變量，而$F(x)$爲優化函數。此問題稱爲無約束優化問題，由於咱們沒有給出任何約束形式。因爲slam中優化問題多爲無約束優化，因此咱們着重介紹無約束的形式。ide

　　當$F(x)$有一些特殊性質時，對應的優化問題也能夠用一些特殊的解法。例如，$F(x)$爲一個線性函數時，則爲線性優化問題（不過線性優化問題一般在有約束情形下討論）。反之則爲非線性優化。對於無約束的非線性優化，若是咱們知道它梯度的解析形式，就能直接求那些梯度爲零的點，來解決這個優化：函數

\[\begin{equation} \frac{{dF}}{{dx}} = 0 \end{equation}\]性能

　　梯度爲零的地方多是函數的極大值、極小值或者鞍點。因爲如今$F(x)$的形式不肯定，咱們只好遍歷全部的極值點，找到最小的做爲最優解。學習

　　可是咱們爲何不這樣用呢？由於不少時候$F(x)$的形式太複雜，致使咱們無法寫出導數的解析形式，或者難以求解導數爲零的方程。所以，多數時候咱們使用迭代方式求解。從一個初值$x_0$出發，不斷地致使當前值附近的，能使目標函數降低的方式（反向梯度），而後沿着梯度方向走出一步，從而使得函數值降低一點。這樣反覆迭代，理論上對於任何函數，都能找到一個極小值點。優化

　　迭代的策略主要體如今如何選擇降低方向，以及如何選擇步長兩個方面。主要有 Gauss-Newton （GN）法和 Levenberg-Marquardt （LM）法兩種，它們的細節能夠在維基上找到，咱們不細說。請理解它們主要在迭代策略上有所不一樣，可是尋找梯度並迭代則是同樣的。編碼

圖優化

　　所謂的圖優化，就是把一個常規的優化問題，以圖（Graph）的形式來表述。

　　圖是什麼呢？

　　圖是由頂點（Vertex）和邊（Edge）組成的結構，而圖論則是研究圖的理論。咱們記一個圖爲$G=\{ V, E \}$，其中$V$爲頂點集，$E$爲邊集。

　　頂點沒什麼可說的，想象成普通的點便可。

　　邊是什麼呢？一條邊鏈接着若干個頂點，表示頂點之間的一種關係。邊能夠是有向的或是無向的，對應的圖稱爲有向圖或無向圖。邊也能夠鏈接一個頂點（Unary Edge，一元邊）、兩個頂點（Binary Edge，二元邊）或多個頂點（Hyper Edge，多元邊）。最多見的邊鏈接兩個頂點。當一個圖中存在鏈接兩個以上頂點的邊時，稱這個圖爲超圖（Hyper Graph）。而SLAM問題就能夠表示成一個超圖（在不引發歧義的狀況下，後文直接以圖指代超圖）。

　　怎麼把SLAM問題表示成圖呢？

　　SLAM的核心是根據已有的觀測數據，計算機器人的運動軌跡和地圖。假設在時刻$k$，機器人在位置$x_k$處，用傳感器進行了一次觀測，獲得了數據$z_k$。傳感器的觀測方程爲：

\[ \begin{equation}{z_k} = h\left( {{x_k}} \right) \end{equation} \]

　　因爲偏差的存在，$z_k$不可能精確地等於$h(x_k)$，因而就有了偏差：

　　\[ \begin{equation} {e_k} = {z_k} - h\left( {{x_k}} \right) \end{equation} \]

　　那麼，若是咱們以$x_k$爲優化變量，以$ \min\limits_x F_k (x_k) = \| e_k \| $爲目標函數，就能夠求得$x_k$的估計值，進而獲得咱們想要的東西了。這實際上就是用優化來求解SLAM的思路。

　　你說的優化變量$x_k$，觀測方程$z_k = h (x_k)$等等，它們具體是什麼東西呢？

　　這個取決於咱們的參數化（parameterazation）。$x$能夠是一個機器人的Pose（6自由度下爲 $4\times 4$的變換矩陣$\mathbf{T}$ 或者 3自由度下的位置與轉角$[x,y,\theta]$，也能夠是一個空間點（三維空間的$[x,y,z]$或二維空間的$[x,y]$）。相應的，觀測方程也有不少形式，如：

機器人兩個Pose之間的變換；
機器人在某個Pose處用激光測量到了某個空間點，獲得了它離本身的距離與角度；
機器人在某個Pose處用相機觀測到了某個空間點，獲得了它的像素座標；

　　一樣，它們的具體形式不少樣化，這容許咱們在討論slam問題時，不侷限於某種特定的傳感器或姿態表達方式。

　　我明白優化是什麼意思了，可是它們怎麼表達成圖呢？

　　在圖中，以頂點表示優化變量，以邊表示觀測方程。因爲邊能夠鏈接一個或多個頂點，因此咱們把它的形式寫成更廣義的 $z_k = h(x_{k1}, x_{k2}, \ldots )$，以表示不限制頂點數量的意思。對於剛纔提到的三種觀測方程，頂點和邊是什麼形式呢？

機器人兩個Pose之間的變換；——一條Binary Edge（二元邊），頂點爲兩個pose，邊的方程爲${T_1} = \Delta T \cdot {T_2}$。
機器人在某個Pose處用激光測量到了某個空間點，獲得了它離本身的距離與角度；——Binary Edge，頂點爲一個2D Pose：$[x,y,\theta]^T$和一個Point：$[\lambda_x, \lambda_y]^T$，觀測數據是距離$r$和角度$b$，那麼觀測方程爲：
\[ \begin{equation}
    \left[
    {\begin{array}{*{20}{c}}
      {r}\\
      {b}
    \end{array}}
\right] = \left[ {
    \begin{array}{*{20}{c}}
      {\sqrt {{{({\lambda _x} - x)}^2} + {{({\lambda _y} - y)}^2}}}\\
      {{{\tan }^{ - 1}}\left( {\frac{{{\lambda _y} - y}}{{{\lambda _x} - x}}} \right) - \theta }
    \end{array}}
\right]
\end{equation}\]
機器人在某個Pose處用相機觀測到了某個空間點，獲得了它的像素座標；——Binary Edge，頂點爲一個3D Pose：$T$和一個空間點$\mathbf{x} = [x,y,z]^T$，觀測數據爲像素座標$z=[u,v]^T$。那麼觀測方程爲：\[ \begin{equation} z = C ( R \mathbf{x} + t ) \end{equation} \]

　　$C$爲相機內參，$R,t$爲旋轉和平移。

　　舉這些例子，是爲了讓讀者更好地理解頂點和邊是什麼東西。因爲機器人可能使用各類傳感器，故咱們不限制頂點和邊的參數化以後的樣子。好比我（喪心病狂地在小蘿蔔身上）既加了激光，也用相機，還用了IMU，輪式編碼器，超聲波等各類傳感器來作slam。爲了求解整個問題，個人圖中就會有各類各樣的頂點和邊。可是無論如何，都是能夠用圖來優化的。

（暗黑小蘿蔔小眼神degined by Orchid Zhang）_{之後找不到工做我就去當插畫算了……}

圖優化怎麼作

　　如今讓咱們來仔細看一看圖優化是怎麼作的。假設一個帶有$n$條邊的圖，其目標函數能夠寫成：

\[ \begin{equation} \min\limits_{x} \sum\limits_{k = 1}^n {{e_k}{{\left( {{x_k},{z_k}} \right)}^T}{\Omega _k}{e_k}\left( {{x_k},{z_k}} \right)} \end{equation}\]

　　關於這個目標函數，咱們有幾句話要講。這些話都是很重要的，請讀者仔細去理解。

$e$ 函數在原理上表示一個偏差，是一個矢量，做爲優化變量$x_k$和$z_k$符合程度的一個度量。它越大表示$x_k$越不符合$z_k$。可是，因爲目標函數必須是標量，因此必須用它的平方形式來表達目標函數。最簡單的形式是直接作成平方：$e(x,z)^T e(x,z)$。進一步，爲了表示咱們對偏差各份量重視程度的不同，還使用一個信息矩陣 $\Omega$ 來表示各份量的不一致性。
信息矩陣 $\Omega$ 是協方差矩陣的逆，是一個對稱矩陣。它的每一個元素$ \Omega_{i,j}$做爲$e_i e_j$的係數，能夠當作咱們對$e_i, e_j$這個偏差項相關性的一個預計。最簡單的是把$\Omega$設成對角矩陣，對角陣元素的大小代表咱們對此項偏差的重視程度。
這裏的$x_k$能夠指一個頂點、兩個頂點或多個頂點，取決於邊的實際類型。因此，更嚴謹的方式是把它寫成$e_k ( z_k, x_{k1}, x_{k2}, \ldots )$，可是那樣寫法實在是太繁瑣，咱們就簡單地寫成如今的樣子。因爲$z_k$是已知的，爲了數學上的簡潔，咱們再把它寫成$e_k(x_k)$的形式。

　　因而整體優化問題變爲$n$條邊加和的形式：

\[ \begin{equation} \min F(x) = \sum\limits_{k = 1}^n {{e_k}{{\left( {{x_k}} \right)}^T}{\Omega _k}{e_k}\left( {{x_k}} \right)} \end{equation}\]

　　重複一遍，邊的具體形式有不少種，能夠是一元邊、二元邊或多元邊，它們的數學表達形式取決於傳感器或你想要描述的東西。例如視覺SLAM中，在一個相機Pose $T_k$ 處對空間點$\mathbf{x}_k$進行了一次觀測，獲得$z_k$，那麼這條二元邊的數學形式即爲$${e_k}\left( {{x_k},{T_k},{z_k}} \right) = {\left( {{z_k} - C\left( {R{x_k} + t} \right)} \right)^T}{\Omega _k}\left( {{z_k} - C\left( {R{x_k} - t} \right)} \right)$$ 單個邊其實並不複雜。

　　如今，咱們有了一個不少個節點和邊的圖，構成了一個龐大的優化問題。咱們並不想展開它的數學形式，只關心它的優化解。那麼，爲了求解優化，須要知道兩樣東西：一個初始點和一個迭代方向。爲了數學上的方便，先考慮第$k$條邊$e_k(x_k)$吧。

　　咱們假設它的初始點爲${{\widetilde x}_k}$，而且給它一個$\Delta x$的增量，那麼邊的估計值就變爲$F_k ( {\widetilde x}_k + \Delta x )$，而偏差值則從 $e_k(\widetilde x)$ 變爲 $e_k({\widetilde x}_k + \Delta x )$。首先對偏差項進行一階展開：

\[ \begin{equation} {e_k}\left( {{{\widetilde x}_k} + \Delta x} \right) \approx {e_k}\left( {{{\widetilde x}_k}} \right) + \frac{{d{e_k}}}{{d{x_k}}}\Delta x = {e_k} + {J_k}\Delta x\end{equation} \]

　　這是的$J_k$是$e_k$關於$x_k$的導數，矩陣形式下爲雅可比陣。咱們在估計點附近做了一次線性假設，認爲函數值是可以用一階導數來逼近的，固然這在$\Delta x$很大時候就不成立了。

　　因而，對於第$k$條邊的目標函數項，有：

　　進一步展開：

$$\begin{array}{lll}{F_k}\left( {{{\widetilde x}_k} + \Delta x} \right) &=& {e_k}{\left( {{{\widetilde x}_k} + \Delta x} \right)^T}{\Omega _k}{e_k}\left( {{{\widetilde x}_k} + \Delta x} \right)\\
& \approx & {\left( {{e_k} + {J_k}\Delta x} \right)^T}{\Omega _k}\left( {{e_k} + J\Delta x} \right)\\
&=& e_k^T{\Omega _k}{e_k} + 2e_k^T{\Omega _k}{J_k}\Delta x + \Delta {x^T}J_k^T{\Omega _k}{J_k}\Delta x\\ &=& {C_k} + 2{b_k}\Delta x + \Delta {x^T}{H_k}\Delta x
\end{array}$$

　　在熟練的同窗看來，這個推導就像$(a+b)^2=a^2+2ab+b^2$同樣簡單（事實上就是好吧）。最後一個式子是個定義整理式，咱們把和$\Delta x$無關的整理成常數項 $C_k$ ，把一次項係數寫成 $2b_k$ ，二次項則爲 $H_k$（注意到二次項係數實際上是Hessian矩陣）。

　　請注意 $C_k$ 實際就是該邊變化前的取值。因此在$x_k$發生增量後，目標函數$F_k$項改變的值即爲$$\Delta F_k = 2b_k \Delta x + \Delta x^T H_k \Delta x. $$

　　咱們的目標是找到$\Delta x$，使得這個增量變爲極小值。因此直接令它對於$\Delta x$的導數爲零，有：

\[ \begin{equation} \frac{{d{F_k}}}{{d\Delta x}} = 2b + 2{H_k}\Delta x = 0 \Rightarrow {H_k}\Delta x = - b_k \end{equation} \]

　　因此歸根結底，咱們求解一個線性方程組：\[ \begin{equation} H_k \Delta x = -b_k \end{equation} \]

　　若是把全部邊放到一塊兒考慮進去，那就能夠去掉下標，直接說咱們要求解$$ H \Delta x = -b. $$

　　原來算了半天它只是個線性的！線性的誰不會解啊！

　　讀者固然會有這種感受，由於線性規劃是規劃中最爲簡單的，連小學生都會解這麼簡單的問題，爲什麼21世紀前SLAM不這樣作呢？——這是由於在每一步迭代中，咱們都要求解一個雅可比和一個海塞。而一個圖中常常有成千上萬條邊，幾十萬個待估計參數，這在之前被認爲是沒法實時求解的。

　　那爲什麼後來又能夠實時求解了呢？

　　SLAM研究者逐漸認識到，SLAM構建的圖，並不是是全連通圖，它每每是很稀疏的。例如一個地圖裏大部分路標點，只會在不多的時刻被機器人看見，從而創建起一些邊。大多數時候它們是看不見的（就像後宮怨女同樣）。體如今數學公式中，雖然整體目標函數$F(x)$有不少項，但某個頂點$x_k$就只會出如今和它有關的邊裏面！

　　這會致使什麼？這致使許多和$x_k$無關的邊，好比說$e_j$，對應的雅可比$J_j$就直接是個零矩陣！而整體的雅可比$J$中，和$x_k$有關的那一列大部分爲零，只有少數的地方，也就是和$x_k$頂點相連的邊，出現了非零值。

　　相應的二階導矩陣$H$中，大部分也是零元素。這種稀疏性能很好地幫助咱們快速求解上面的線性方程。出於篇幅咱們先不細說這是如何作到的了。稀疏代數庫包括SBA、PCG、CSparse、Cholmod等等。g2o正是使用它們來求解圖優化問題的。

　　要補充一點的是，在數值計算中，咱們能夠給出雅可比和海塞的解析形式進行計算，也可讓計算機去數值計算這兩個陣，而咱們只須要給出偏差的定義方式便可。

流形

　　等一下老師！上面推導還有一個問題！

　　很好，小蘿蔔同窗，請說說是什麼問題。

　　咱們在討論給目標函數$F(x)$一個增量$\Delta x$時，直接就寫成了$F(x+\Delta x)$。可是老師，這個加法可能沒有定義！

　　小蘿蔔同窗看到了一個嚴重的問題，這確實是在先前的討論中忽略掉了。因爲咱們不限制頂點的類型，$x$在參數化以後，極可能是沒有加法定義的。

　　最簡單的就是常見的四維變換矩陣$T$或者三維旋轉矩陣$R$。它們對加法並不封閉，由於兩個變換陣之和並非變換陣，兩個正交陣之和也不是正交陣。它們乘法的性質很是好，可是確實沒有加法，因此也不能像上面討論的那樣去求導。

　　可是，若是圖優化不能處理$SE(3)$或$SO(3)$中的元素，那將是十分使人沮喪的，由於SLAM要估計的機器人軌跡必須用它們來描述啊。

　　回想咱們先前講過的李代數知識。雖然李羣 $SE(3)$ 和 $SO(3)$ 是沒有加法的，可是它們對應的李代數 $\mathfrak{se}(3), \mathfrak{so}(3)$ 有啊！數學一點地說，咱們能夠求它們在正切空間裏的流形上的梯度！若是讀者以爲理解困難，咱們就說，經過指數變換和對數變換，先把變換矩陣和旋轉矩陣轉換成李代數，在李代數上進行加法，而後再轉換到本來的李羣中。這樣咱們就完成了求導。

　　這樣的好處是咱們徹底不用從新推導公式。這件事比咱們想的更加簡單。在程序裏，咱們只需從新定義一個優化變量$x$的增量加法便可。若是$x$是一個$SE(3)$裏的變換矩陣，咱們就遵照剛纔講的李代數轉換方式。固然，若是$x$是其餘什麼奇怪的東東，只要定義了它的加法，程序就會自動去計算如何求它的雅可比。