解讀《視覺SLAM十四講》，帶你一步一步入門視覺SLAM—— 第 11 講 後端 2

解讀

在上一講的解讀中，我介紹了基於圖的後端優化，我們也瞭解了SLAM優化問題的稀疏性，基於稀疏性我們可以在很短的時間內對特徵點位置相機姿態進行優化。不過更大的場景下，大量的特徵點的存在會嚴重的降低計算效率，導致計算量越來越大以至於無法實時化¹ 。
　　
　　經過觀察發現，在做BA優化的時候，特徵點的優化佔了計算資源的大部分，而實際上經過幾輪優化之後，空間位置估計會收斂到一個值保持不動，而發散的外點通常也就看不到了。因此常常的做法是經過幾次優化之後，將特徵點固定住，然後只對位姿進行優化。

Pose Graph的優化

通過上述方法，我們極大的降低了計算量，於是乎按照上面的思路我們可以構建一個只有軌跡的圖優化，而位姿節點之間的邊，可以由兩個關鍵幀之間通過特徵匹配之後得到的運動估計來給定初始值。於是這種圖優化叫做：位姿圖（Pose Graph）。
　　
　　相信經過前面對於圖優化的多次實踐，你應該有這樣的印象了，那就圖優化需要定義頂點、邊、優化項，已經雅克比矩陣。那麼就讓我們沿着這個思路來推導出圖優化（Pose Graph）的公式吧！
　　
　　非線性優化最首要的就是定義誤差項，我們先來看一下Pose Graph的誤差項：
　　
　　圖優化的節點就是待優化的變量，那麼對於Pose Graph的節點就是相機的位姿，我們以$\xi_1,\cdots,\xi_n$來表示。而邊則是兩個位姿節點之間相對運動的估計，比方說在位姿$\xi_i,\xi_j$之間有一個運動，我們經過前面說的特徵點法或者直接法也好，得到了一個運動估計$\xi_{ij}$，我們求出來的運動肯定和實際的真實運動之間存在誤差，我們目的就像要讓這個誤差最小。
　　
　　我們將上面的運動，表示出來：
$\Delta \xi_{ij}=\xi_i^{-1} \circ \xi_j=\ln(\exp((-\xi_i)^{\wedge}\exp(\xi_j)^{\wedge})^{\vee} \tag 0$你應該能想到（0）式是咋來的吧！
　　
　　理論上來說，（０）式的左右兩邊應該是相等的，但是實際上我們$\Delta \xi_{ij}$是根據VO的方法計算出來的，它並不會那麼幸運沒有一點兒誤差就剛好等於真實值。
　　上面的（０）式子，我們寫成李羣的形式，就是變換矩陣的形式：
$T_{ij}=T_i^{-1}T_{j} \tag 1$既然（0）式因爲誤差不能相等，那麼我們就將它們的差值構建成誤差的形式：
$e_{ij}=\ln(T_{ij}^{-1}T_i^{-1}T_j)^{\vee} \\ =\ln(\exp((-\xi_{ij})^{\wedge})\exp((-\xi_i)^{\wedge})\exp(\xi_j^{\wedge}))^{\vee} \tag2$還記得變換矩陣的性質嗎？它對加法不封閉，所以求它的誤差項，只能用乘法！

我們要優化的變量是$\xi_j,\xi_j$，所以就必須得求得誤差項關於它的導數。根據李代數的左擾動模型，我們可以得到誤差項關於這兩個變量的偏導數：
$\frac{\partial e_{ij}}{\partial \delta \xi_i}=-\mathcal{J}_r^{-1}(e_{ij})Ad(T_j^{-1})$$\frac{\partial e_{ij}}{\partial \delta \xi_j}=\mathcal{J}_r^{-1}(e_{ij})Ad(T_j^{-1}) \tag3$由於$\mathcal{J}_r$的計算過於複雜，所以這裏採用近似：
$\mathcal{J}_r^{-1}(e_{ij}) \approx I + \frac{1}{2} \left[ \begin{matrix} \phi_e^{\wedge} & \rho_e^{\wedge} \\ 0 & \phi_e^{\wedge} \end{matrix} \right] \tag 4$　　上式子當中，SE(3)上的擾動項的李代數爲：$\delta \xi=[\delta \rho,\delta \phi]$，書中沒有解釋（4）式變量的意義，實際上在第四講中對於SE(3)上李代數的求導中，用到了上面的兩個變量。
　　
　　有了每一個誤差項的雅克比矩陣之後，我們就可以構建Pose Graph優化了。本質上還是一個最小二乘問題，優化變量爲各個頂點（相機位姿），邊來自於位姿觀測約束。記$\varepsilon$爲所有邊的集合，那麼總體目標函數爲：
$\min_\xi \frac{1}{2} \sum_{i,j \in \varepsilon} e_{ij}^{T} \Sigma_{ij}^{-1} e_{ij} \tag 5$上式中$\Sigma_{ij}^{-1}$是信息矩陣，它表示的是協方差矩陣的逆。信息矩陣可以理解成我們對誤差各分量重視程度的不一樣。

既然代價函數也有了，每一項的雅克比也有了，那麼只要構造成圖優化，就可以進行優化求解了。

實踐

g2o原生位姿圖

這部分的代碼，主要就是將sphere.g2o中的數據讀入內存中，然後使用g2o自帶的頂點和邊進行優化，代碼非常簡單，就不在多解釋了。已經到了11講了，我覺得大家應該對g2o的用法差不多熟悉了。

李代數上的位姿圖優化

這個代碼的主要目的是通過李代數，自己定義頂點和邊。

$\mathcal{J}_r^{-1}(e_{ij})$的構造代碼如下：

Matrix6d JRInv( SE3 e )
{
    Matrix6d J;
    J.block(0,0,3,3) = SO3::hat(e.so3().log());
    J.block(0,3,3,3) = SO3::hat(e.translation());
    J.block(3,0,3,3) = Eigen::Matrix3d::Zero(3,3);
    J.block(3,3,3,3) = SO3::hat(e.so3().log());
    J = J*0.5 + Matrix6d::Identity();
    return J;
}

如果你還是沒有習慣g2o的使用方式，那麼你就先模仿着寫，多實踐幾次你就會使用了，對着這種循規蹈矩的編程方式，一旦你習慣了，就會非常容易。

*因子圖優化初步

由於因子圖，我還在學習中，所以這部分內容，我暫時就不解讀了，等我學習了這部分內容之後，我再寫！

這裏推薦大家一本比較全面的講因子圖優化的書：
《機器人感知：因子圖在SLAM中的應用》

如果你對因子圖沒興趣，你可以跳過這部分，因爲這部分的應用並沒有圖優化廣泛。

最好的年紀不要辜負最美的自己.

---

1. 《視覺SLAM十四講 從理論到實踐》 ↩︎