研究SLAM的同學應該對g2o并不陌生。用了一段時間之后，一直對其內(nèi)部實現(xiàn)方式不太清楚，今天打算仔細研究一下。
首先祭出官方提供的g2o類層次結(jié)構(gòu)圖。
這個圖需要分塊來看。
左上角HyperGraph提供了頂點和邊構(gòu)成的拓撲圖，它只關注圖的連接關系，不負責優(yōu)化相關的工作。其派生類OptimizableGraph為頂點和邊提供了可優(yōu)化的功能。再往下是OptimizableGraph的派生類SparseOptimizer，顯然，對于構(gòu)建的圖優(yōu)化問題，SparseOptimizer提供了稀疏求解的方案，這也是SLAM能夠達到實時性所依賴的關鍵技術。以上，是優(yōu)化器相關的部分。
再往下，優(yōu)化器包含了一個OptimizationAlgorithm，這是優(yōu)化算法的基類，優(yōu)化器會調(diào)用該優(yōu)化算法實現(xiàn)優(yōu)化。右邊給出了優(yōu)化算法的具體實現(xiàn)，OptimizationWithHessian包含了一系列基于Hessian矩陣求解增量方程的優(yōu)化算法，包括OptimizationAlgorithmGaussNewton、OptimizationAlgorithmLevenberg和OptimizationAlgorithmDogleg。不同的優(yōu)化算法給出了不同的梯度下降策略，也就是說，用不同的方式找出增量的方向和大小，但迭代求解的本質(zhì)是一樣的。
再往右，優(yōu)化算法包含一個Solver，也就是求解器。不論使用了什么優(yōu)化算法，每次迭代都需要求解一個 H?x=g 的增量方程，其中H是Hessian矩陣或其變體，?x是待求的優(yōu)化變量的更新量。那么如何求解這個方程，就是Solver的工作了。在g2o中，只提供了一個實現(xiàn)類BlockSolver<>。所謂塊求解器，就是利用A矩陣的稀疏性，每個優(yōu)化變量和誤差項都體現(xiàn)為固定大小的矩陣塊，可以利用它的一些性質(zhì)加速計算。在塊求解器中，包含了一個SparseBlockMatrix<T>和LinearSolver，其實前者用來存放H矩陣的數(shù)據(jù)，后者用來指定具體的線性求解器。之所以叫線性求解器，是因為增量方程是一個線性方程。g2o提供的線性求解器有三個，分別是LinearSolverCSparse<>、LinearSolverCholmod<>和LinearSolverPCG<>。它們之間的不同大概只是對矩陣求逆的方式不同，可能會有速度上的差異，但結(jié)果一定一致。
最后，右上角的一大部分是頂點和邊，這些比較容易理解，也是我們編程中接觸得最多的，這里不再詳述。
分析完這個圖，基本上g2o的優(yōu)化流程也就差不多清晰了。但有幾個關鍵的環(huán)節(jié)剛才并沒有提到，而且我的理解也不敢保證沒有偏差，寫在下面和大家一起交流。
SLAM中有一個加速增量方程求解的方法，稱為邊緣化。邊緣化是說，如果我們把待優(yōu)化的相機位姿放在H矩陣的左上角，把待優(yōu)化的路標點放在H矩陣的右下角，再把H矩陣分為四塊，就可以對H的矩陣塊進行高斯消元，使得對相機位姿的求解不依賴于路標點。這種方法奏效的原因是因為相機矩陣相比于路標點稀疏得多，因此相機矩陣塊求逆更容易。當然，具體的分析建議閱讀高翔的《視覺SLAM十四講》。這里我們更關注g2o中的實現(xiàn)。邊緣化部分的操作在BlockSolver<>中，塊求解器會把對增量方程的求解分為兩步，先求解相機位姿的增量，再求解路標點的增量。當然，每次求解增量仍然是調(diào)用內(nèi)部的LinearSolver。思路很清晰，但g2o在這里的實現(xiàn)卻有待商榷，塊求解器中根據(jù)頂點是否被邊緣化決定該頂點是位姿頂點還是路標點頂點，也就是說，它默認所有位姿頂點都不被邊緣化，所有路標點頂點都被邊緣化。假如你嘗試不邊緣化路標點頂點，或邊緣化位姿頂點，求解器都會報錯，這就限制了我們優(yōu)化的靈活性。在我看來，這可能是g2o作者實現(xiàn)過程中的一個瑕疵。BlockSolver并沒有體現(xiàn)出其應有的抽象，它應該根據(jù)實際的頂點類型來決定如何實現(xiàn)邊緣化，而不是一股腦地認為只有路標點應該被邊緣化。（注意，這里提到的邊緣化只用于加速增量方程求解，不同于滑動窗口中的邊緣化。）
對于上面的問題，其實并非不能解決。如果我們不用固定大小的BlockSolver_6_3，而是用動態(tài)大小的BlockSolverX，就不會出問題。因為前者默認維度為6的位姿頂點被邊緣化，維度為3的路標點頂點不被邊緣化，不符合維度要求的頂點會導致出錯。而后者并不要求邊緣化的頂點維度為6，也不要求不邊緣化的頂點維度為3，允許同時存在維度為6和維度為3的頂點被邊緣化。但問題解決并不意味著g2o的設計沒有問題，BlockSolver中把頂點維度和是否邊緣化在語義層面綁定了起來，很容易造成誤解。舉個例子，如果我想邊緣化所有位姿頂點，不邊緣化路標點頂點，最簡單的解決方案是使用BlockSolverX。但一般認為，固定矩陣大小可以把一部分運行時時間轉(zhuǎn)移到編譯期。所以我可能需要typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 6>> BlockSolver_3_6;，相當于把BlockSolverTraits的第一個模板參數(shù)_PoseDim設為3，第二個模板參數(shù)_LandmarkDim設為6，也就是把路標點當成位姿，把位姿當成路標點。雖然可以用，但實在太過別扭。
總體上看，我認為g2o是一個結(jié)構(gòu)良好的圖優(yōu)化框架，它的類層次結(jié)構(gòu)提供了很高的可擴展性。但遺憾的是，實際實現(xiàn)的功能并不多，很多類的派生類都只有一個，比如只有SparseOptimizer而沒有DenseOptimizer，只有OptimizationWithHessian而沒有OptimizationWithOthers，只有BlockSolver<>而沒有PlainSolver。以至于g2o只能用于求解視覺SLAM問題，應用范圍有些狹隘。但畢竟g2o沒有企業(yè)在背后支持，不像Ceres有谷歌撐腰，搞不好以后做SLAM的標配會變成Ceres吧，感覺有些可惜。
文末給出了一些詳細的參考資料，比本文更有價值，大家可以參考。
參考資料
《視覺SLAM十四講》 高翔
g2o學習——g2o整體框架 無人的回憶
g2o學習——頂點和邊之外的solver 無人的回憶
A General Framework for Graph Optimization R Kümmerle
