SLAM優(yōu)化算法：圖優(yōu)化和凸優(yōu)化算法的區(qū)別

小白：師兄師兄，最近我在看SLAM的優(yōu)化算法，有種方法叫“圖優(yōu)化”，以前學(xué)習(xí)算法的時候還有一個優(yōu)化方法叫“凸優(yōu)化”，這兩個不是一個東西吧？

師兄：哈哈，這個問題有意思，雖然它們中文發(fā)音一樣，但是意思差別大著呢！我們來看看英文表達吧，圖優(yōu)化的英文是 graph optimization 或者 graph-based optimization，你看，它的“圖”其實是數(shù)據(jù)結(jié)構(gòu)中的graph。而凸優(yōu)化的英文是 convex optimization，這里的“凸”其實是凸函數(shù)的意思，所以單從英文就能區(qū)分開它們。

小白：原來是這樣，我看SLAM中圖優(yōu)化用的很多啊，我看了一下高博的書，還是迷迷糊糊的，求科普啊師兄

師兄：圖優(yōu)化真的蠻重要的，概念其實不負責(zé)，主要是編程稍微有點復(fù)雜。。

小白：不能同意更多。。，那個代碼看的我一臉懵逼

圖優(yōu)化有什么優(yōu)勢？

師兄：按照慣例，我還是先說說圖優(yōu)化的背景吧。SLAM的后端一般分為兩種處理方法，一種是以擴展卡爾曼濾波（EKF）為代表的濾波方法，一種是以圖優(yōu)化為代表的非線性優(yōu)化方法。不過，目前SLAM研究的主流熱點幾乎都是基于圖優(yōu)化的。

小白：據(jù)我所知，濾波方法很早就有了，前人的研究也很深。為什么現(xiàn)在圖優(yōu)化變成了主流了？

師兄：你說的沒錯。濾波方法尤其是EKF方法，在SLAM發(fā)展很長的一段歷史中一直占據(jù)主導(dǎo)地位，早期的大神們研究了各種各樣的濾波器來改善濾波效果，那會入門SLAM，EKF是必須要掌握的。順便總結(jié)下濾波方法的優(yōu)缺點：

優(yōu)點：在當時計算資源受限、待估計量比較簡單的情況下，EKF為代表的濾波方法比較有效，經(jīng)常用在激光SLAM中。

缺點：它的一個大缺點就是存儲量和狀態(tài)量是平方增長關(guān)系，因為存儲的是協(xié)方差矩陣，因此不適合大型場景。而現(xiàn)在基于視覺的SLAM方案，路標點（特征點）數(shù)據(jù)很大，濾波方法根本吃不消，所以此時濾波的方法效率非常低。

小白：原來如此。那圖優(yōu)化在視覺SLAM中效率很高嗎？

師兄：這個其實說來話長了。很久很久以前，其實就是不到十年前吧(感覺好像很久)，大家還都是用濾波方法，因為在圖優(yōu)化里，Bundle Adjustment（后面簡稱BA）起到了核心作用。但是那會SLAM的研究者們發(fā)現(xiàn)包含大量特征點和相機位姿的BA計算量其實很大，根本沒辦法實時。

小白：??？后來發(fā)生了什么？（認真聽故事ing）

師兄：后來SLAM研究者們發(fā)現(xiàn)了其實在視覺SLAM中，雖然包含大量特征點和相機位姿，但其實BA是稀疏的，稀疏的就好辦了，就可以加速了??！比較代表性的就是2009年，幾個大神發(fā)表了自己的研究成果《SBA：A software package for generic sparse bundle adjustment》，而且計算機硬件發(fā)展也很快，因此基于圖優(yōu)化的視覺SLAM也可以實時了！

小白：厲害厲害！向大牛們致敬！

圖優(yōu)化是什么？

小白：圖優(yōu)化既然是主流，那我可以跳過濾波方法直接學(xué)習(xí)圖優(yōu)化吧，反正濾波方法也看不懂。。

師兄：額，圖優(yōu)化確實是主流，以后有需要你可以再去看濾波方法，那我們今天就只講圖優(yōu)化好啦

小白：好滴，那問題來了，究竟什么是圖優(yōu)化??？

師兄：圖優(yōu)化里的圖就是數(shù)據(jù)結(jié)構(gòu)里的圖，一個圖由若干個頂點（vertex），以及連接這些頂點的邊（edge）組成，給你舉個例子

比如一個機器人在房屋里移動，它在某個時刻 t 的位姿（pose）就是一個頂點，這個也是待優(yōu)化的變量。而位姿之間的關(guān)系就構(gòu)成了一個邊，比如時刻 t 和時刻 t+1 之間的相對位姿變換矩陣就是邊，邊通常表示誤差項。

在SLAM里，圖優(yōu)化一般分解為兩個任務(wù)：

1、構(gòu)建圖。機器人位姿作為頂點，位姿間關(guān)系作為邊。

2、優(yōu)化圖。調(diào)整機器人的位姿（頂點）來盡量滿足邊的約束，使得誤差最小。

下面就是一個直觀的例子。我們根據(jù)機器人位姿來作為圖的頂點，這個位姿可以來自機器人的編碼器，也可以是ICP匹配得到的，圖的邊就是位姿之間的關(guān)系。由于誤差的存在，實際上機器人建立的地圖是不準的，如下圖左。我們通過設(shè)置邊的約束，使得圖優(yōu)化向著滿足邊約束的方向優(yōu)化，最后得到了一個優(yōu)化后的地圖（如下圖中所示），它和真正的地圖（下圖右）非常接近。

小白：哇塞，這個圖優(yōu)化效果這么明顯?。傞_始誤差那么大，最后都校正過來了

師兄：是啊，所以圖優(yōu)化在SLAM中舉足輕重啊，一定得掌握！

小白：好，有學(xué)習(xí)的動力了！我們開啟編程模式吧！

先了解g2o 框架

師兄：前面我們簡單介紹了圖優(yōu)化，你也看到了它的神通廣大，那如何編程實現(xiàn)呢？

小白：對啊，有沒有現(xiàn)成的庫啊，我還只是個“調(diào)包俠”。。

師兄：這個必須有??！在SLAM領(lǐng)域，基于圖優(yōu)化的一個用的非常廣泛的庫就是g2o，它是General Graphic Optimization 的簡稱，是一個用來優(yōu)化非線性誤差函數(shù)的c++框架。這個庫可以滿足你調(diào)包俠的夢想~

小白：哈哈，太好了，否則打死我也寫不出來啊！那這個g2o怎么用呢？

師兄：我先說安裝吧，其實g2o安裝很簡單，參考GitHub上官網(wǎng)：

https://github.com/RainerKuemmerle/g2o

按照步驟來安裝就行了。需要注意的是安裝之前確保電腦上已經(jīng)安裝好了第三方依賴。

小白：好的，這個看起來很好裝。不過問題是，我看相關(guān)的代碼，感覺很復(fù)雜啊，不知如何下手啊

師兄：別急，第一次接觸g2o，確實有這種感覺，而且官網(wǎng)文檔寫的也比較“不通俗不易懂”，不過如果你能捋順了它的框架，再去看代碼，應(yīng)該很快能夠入手了

小白：是的，先對框架了然于胸才行，不然即使能湊合看懂別人代碼，自己也不會寫啊！

師兄：嗯嗯，其實g2o幫助我們實現(xiàn)了很多內(nèi)部的算法，只是在進行構(gòu)造的時候，需要遵循一些規(guī)則，在我看來這是可以接受的，畢竟一個程序不可能滿足所有的要求，因此在以后g2o的使用中還是應(yīng)該多看多記，這樣才能更好的使用這個庫。

小白：記住了。養(yǎng)成記筆記的好習(xí)慣，還要多練習(xí)。

師兄：好，那我們首先看一下下面這個圖，是g2o的基本框架結(jié)構(gòu)。如果你查資料的話，你會在很多地方都能看到?？磮D的時候要注意箭頭類型

1、圖的核心

小白：師兄，這個圖該從哪里開始看？感覺好多東西。。

師兄：如果你想要知道這個圖中哪個最重要，就去看看箭頭源頭在哪里

小白：我看看。。。好像是最左側(cè)的SparseOptimizer？

師兄：對的，SparseOptimizer是整個圖的核心，我們注意右上角的 is-a 實心箭頭，這個SparseOptimizer它是一個Optimizable Graph，從而也是一個超圖（HyperGraph）。

小白：我去，師兄，怎么突然冒出來這么多奇怪的術(shù)語，都啥意思啊？

師兄：這個你不需要一個個弄懂，不然可能黃花菜都涼了。你先暫時只需要了解一下它們的名字，有些以后用不到，有些以后用到了再回看。目前如果遇到重要的我會具體解釋。

小白：好。那下一步看哪里？

2、頂點和邊

師兄：我們先來看上面的結(jié)構(gòu)吧。注意看 has-many 箭頭，你看這個超圖包含了許多頂點（HyperGraph::Vertex）和邊（HyperGraph::Edge）。而這些頂點頂點繼承自 Base Vertex，也就是OptimizableGraph::Vertex，而邊可以繼承自 BaseUnaryEdge（單邊）, BaseBinaryEdge（雙邊）或BaseMultiEdge（多邊），它們都叫做OptimizableGraph::Edge

小白：頭有點暈了，師兄

師兄：哈哈，不用一個個記，現(xiàn)階段了解這些就行。頂點和邊在編程中很重要的，關(guān)于頂點和邊的定義我們以后會詳細說的。下面我們來看底部的結(jié)構(gòu)。

小白：嗯嗯，知道啦！

3、配置SparseOptimizer的優(yōu)化算法和求解器

師兄：你看下面，整個圖的核心SparseOptimizer 包含一個優(yōu)化算法（OptimizationAlgorithm）的對象。OptimizationAlgorithm是通過OptimizationWithHessian 來實現(xiàn)的。其中迭代策略可以從Gauss-Newton（高斯牛頓法，簡稱GN）, Levernberg-Marquardt（簡稱LM法）, Powell's dogleg 三者中間選擇一個（我們常用的是GN和LM）

小白：GN和LM就是我們以前講過的非線性優(yōu)化方法中常用的兩種吧

4、如何求解

師兄：那么如何求解呢？OptimizationWithHessian 內(nèi)部包含一個求解器（Solver），這個Solver實際是由一個BlockSolver組成的。這個BlockSolver有兩個部分，一個是SparseBlockMatrix ，用于計算稀疏的雅可比和Hessian矩陣；一個是線性方程的求解器（LinearSolver），它用于計算迭代過程中最關(guān)鍵的一步HΔx=?b，LinearSolver有幾種方法可以選擇：PCG, CSparse, Choldmod，具體定義后面會介紹

到此，就是上面圖的一個簡單理解。

一步步帶你看懂g2o編程流程

小白：師兄，看完了我也不知道編程時具體怎么編呢！

師兄：我正好要說這個。首先這里需要說一下，我們梳理是從頂層到底層，但是編程實現(xiàn)時需要反過來，像建房子一樣，從底層開始搭建框架一直到頂層。g2o的整個框架就是按照下圖中我標的這個順序來寫的。

高博在十四講中g(shù)2o求解曲線參數(shù)的例子來說明，源代碼地址

https://github.com/gaoxiang12/slambook/edit/master/ch6/g2o_curve_fitting/main.cpp

為了方便理解，我重新加了注釋。如下所示，

typedef?g2o::BlockSolver?>?Block;??//?每個誤差項優(yōu)化變量維度為3，誤差值維度為1

//?第1步：創(chuàng)建一個線性求解器LinearSolver
Block::LinearSolverType*?linearSolver?=?new?g2o::LinearSolverDense();?

//?第2步：創(chuàng)建BlockSolver。并用上面定義的線性求解器初始化
Block*?solver_ptr?=?new?Block(?linearSolver?);??????

//?第3步：創(chuàng)建總求解器solver。并從GN, LM, DogLeg 中選一個，再用上述塊求解器BlockSolver初始化
g2o::OptimizationAlgorithmLevenberg*?solver?=?new?g2o::OptimizationAlgorithmLevenberg(?solver_ptr?);

//?第4步：創(chuàng)建終極大boss 稀疏優(yōu)化器（SparseOptimizer）
g2o::SparseOptimizer?optimizer;?????//?圖模型
optimizer.setAlgorithm(?solver?);???//?設(shè)置求解器
optimizer.setVerbose(?true?);???????//?打開調(diào)試輸出

//?第5步：定義圖的頂點和邊。并添加到SparseOptimizer中
CurveFittingVertex*?v?=?new?CurveFittingVertex();?//往圖中增加頂點
v->setEstimate(?Eigen::Vector3d(0,0,0)?);
v->setId(0);
optimizer.addVertex(?v?);
for?(?int?i=0;?isetId(i);
??edge->setVertex(?0,?v?);????????????????//?設(shè)置連接的頂點
??edge->setMeasurement(?y_data[i]?);??????//?觀測數(shù)值
??edge->setInformation(?Eigen::Matrix::Identity()*1/(w_sigma*w_sigma)?);?//?信息矩陣：協(xié)方差矩陣之逆
??optimizer.addEdge(?edge?);
}

//?第6步：設(shè)置優(yōu)化參數(shù)，開始執(zhí)行優(yōu)化
optimizer.initializeOptimization();
optimizer.optimize(100);

結(jié)合上面的流程圖和代碼。下面一步步解釋具體步驟。

1、創(chuàng)建一個線性求解器LinearSolver

我們要求的增量方程的形式是：H△X=-b，通常情況下想到的方法就是直接求逆，也就是△X=-H.inv*b?？雌饋砗孟窈芎唵危@有個前提，就是H的維度較小，此時只需要矩陣的求逆就能解決問題。但是當H的維度較大時，矩陣求逆變得很困難，求解問題也變得很復(fù)雜。

小白：那有什么辦法嗎？

師兄：辦法肯定是有的。此時我們就需要一些特殊的方法對矩陣進行求逆，你看下圖是GitHub上g2o相關(guān)部分的代碼

如果你點進去看，可以分別查看每個方法的解釋，如果不想挨個點進去看，看看下面我的總結(jié)就行了

LinearSolverCholmod ：使用sparse cholesky分解法。繼承自LinearSolverCCS
LinearSolverCSparse：使用CSparse法。繼承自LinearSolverCCS
LinearSolverPCG ：使用preconditioned conjugate gradient 法，繼承自LinearSolver
LinearSolverDense ：使用dense cholesky分解法。繼承自LinearSolver
LinearSolverEigen：?依賴項只有eigen，使用eigen中sparse Cholesky 求解，因此編譯好后可以方便的在其他地方使用，性能和CSparse差不多。繼承自LinearSolver

2、創(chuàng)建BlockSolver。并用上面定義的線性求解器初始化。

BlockSolver 內(nèi)部包含 LinearSolver，用上面我們定義的線性求解器LinearSolver來初始化。它的定義在如下文件夾內(nèi)：

g2o/g2o/core/block_solver.h

你點進去會發(fā)現(xiàn) BlockSolver有兩種定義方式

一種是指定的固定變量的solver，我們來看一下定義

?using?BlockSolverPL?=?BlockSolver?>;

其中p代表pose的維度（注意一定是流形manifold下的最小表示），l表示landmark的維度

另一種是可變尺寸的solver，定義如下

using?BlockSolverX?=?BlockSolverPL;

小白：為何會有可變尺寸的solver呢？

師兄：這是因為在某些應(yīng)用場景，我們的Pose和Landmark在程序開始時并不能確定，那么此時這個塊狀求解器就沒辦法固定變量，此時使用這個可變尺寸的solver，所有的參數(shù)都在中間過程中被確定

另外你看block_solver.h的最后，預(yù)定義了比較常用的幾種類型，如下所示：

BlockSolver_6_3 ：表示pose 是6維，觀測點是3維。用于3D?SLAM中的BA
BlockSolver_7_3：在BlockSolver_6_3 的基礎(chǔ)上多了一個scale
BlockSolver_3_2：表示pose 是3維，觀測點是2維

以后遇到了知道這些數(shù)字是什么意思就行了

3、創(chuàng)建總求解器solver。并從GN, LM, DogLeg 中選一個，再用上述塊求解器BlockSolver初始化

我們來看g2o/g2o/core/ 目錄下，發(fā)現(xiàn)Solver的優(yōu)化方法有三種：分別是高斯牛頓（GaussNewton）法，LM（Levenberg–Marquardt）法、Dogleg法，如下圖所示，也和前面的圖相匹配

小白：師兄，上圖最后那個OptimizationAlgorithmWithHessian 是干嘛的？

師兄：你點進去 GN、 LM、 Doglet算法內(nèi)部，會發(fā)現(xiàn)他們都繼承自同一個類：OptimizationWithHessian，如下圖所示，這也和我們最前面那個圖是相符的

然后，我們點進去看 OptimizationAlgorithmWithHessian，發(fā)現(xiàn)它又繼承自O(shè)ptimizationAlgorithm，這也和前面的相符

總之，在該階段，我們可以選則三種方法：

g2o::OptimizationAlgorithmGaussNewton
g2o::OptimizationAlgorithmLevenberg?
g2o::OptimizationAlgorithmDogleg?

4、創(chuàng)建終極大boss 稀疏優(yōu)化器（SparseOptimizer），并用已定義求解器作為求解方法。

創(chuàng)建稀疏優(yōu)化器

g2o::SparseOptimizer????optimizer;

用前面定義好的求解器作為求解方法：

SparseOptimizer::setAlgorithm(OptimizationAlgorithm*?algorithm)

其中setVerbose是設(shè)置優(yōu)化過程輸出信息用的

SparseOptimizer::setVerbose(bool?verbose)

不信我們來看一下它的定義

5、定義圖的頂點和邊。并添加到SparseOptimizer中。

這部分比較復(fù)雜，我們下一次再介紹。

6、設(shè)置優(yōu)化參數(shù)，開始執(zhí)行優(yōu)化。

設(shè)置SparseOptimizer的初始化、迭代次數(shù)、保存結(jié)果等。

初始化

SparseOptimizer::EdgeSet&?eset)

設(shè)置迭代次數(shù)，然后就開始執(zhí)行圖優(yōu)化了。

SparseOptimizer::optimize(int?iterations,?bool?online)

小白：終于搞明白g2o流程了！謝謝師兄！必須給你個「好看」啊！

注：以上內(nèi)容部分參考了如下文章，感謝原作者：

https://www.jianshu.com/p/e16ffb5b265d

https://blog.csdn.net/heyijia0327/article/details/47686523

編輯：黃飛

?