研究SLAM的同學(xué)應(yīng)該對(duì)g2o并不陌生宾添。用了一段時(shí)間之后私恬，一直對(duì)其內(nèi)部實(shí)現(xiàn)方式不太清楚填硕，今天打算仔細(xì)研究一下麦萤。
首先祭出官方提供的g2o類層次結(jié)構(gòu)圖鹿鳖。
這個(gè)圖需要分塊來(lái)看。
左上角HyperGraph提供了頂點(diǎn)和邊構(gòu)成的拓?fù)鋱D壮莹，它只關(guān)注圖的連接關(guān)系翅帜，不負(fù)責(zé)優(yōu)化相關(guān)的工作。其派生類OptimizableGraph為頂點(diǎn)和邊提供了可優(yōu)化的功能命满。再往下是OptimizableGraph的派生類SparseOptimizer涝滴，顯然，對(duì)于構(gòu)建的圖優(yōu)化問(wèn)題胶台，SparseOptimizer提供了稀疏求解的方案歼疮，這也是SLAM能夠達(dá)到實(shí)時(shí)性所依賴的關(guān)鍵技術(shù)。以上概作，是優(yōu)化器相關(guān)的部分腋妙。
再往下，優(yōu)化器包含了一個(gè)OptimizationAlgorithm讯榕，這是優(yōu)化算法的基類骤素，優(yōu)化器會(huì)調(diào)用該優(yōu)化算法實(shí)現(xiàn)優(yōu)化。右邊給出了優(yōu)化算法的具體實(shí)現(xiàn)愚屁，OptimizationWithHessian包含了一系列基于Hessian矩陣求解增量方程的優(yōu)化算法济竹，包括OptimizationAlgorithmGaussNewton、OptimizationAlgorithmLevenberg和OptimizationAlgorithmDogleg霎槐。不同的優(yōu)化算法給出了不同的梯度下降策略送浊，也就是說(shuō)，用不同的方式找出增量的方向和大小丘跌，但迭代求解的本質(zhì)是一樣的袭景。
再往右，優(yōu)化算法包含一個(gè)Solver闭树，也就是求解器耸棒。不論使用了什么優(yōu)化算法，每次迭代都需要求解一個(gè) H?x=g 的增量方程报辱，其中H是Hessian矩陣或其變體与殃，?x是待求的優(yōu)化變量的更新量。那么如何求解這個(gè)方程碍现，就是Solver的工作了幅疼。在g2o中，只提供了一個(gè)實(shí)現(xiàn)類BlockSolver<>昼接。所謂塊求解器爽篷，就是利用A矩陣的稀疏性，每個(gè)優(yōu)化變量和誤差項(xiàng)都體現(xiàn)為固定大小的矩陣塊慢睡，可以利用它的一些性質(zhì)加速計(jì)算狼忱。在塊求解器中膨疏，包含了一個(gè)SparseBlockMatrix<T>和LinearSolver，其實(shí)前者用來(lái)存放H矩陣的數(shù)據(jù)钻弄，后者用來(lái)指定具體的線性求解器佃却。之所以叫線性求解器，是因?yàn)樵隽糠匠淌且粋€(gè)線性方程窘俺。g2o提供的線性求解器有三個(gè)饲帅，分別是LinearSolverCSparse<>、LinearSolverCholmod<>和LinearSolverPCG<>瘤泪。它們之間的不同大概只是對(duì)矩陣求逆的方式不同灶泵，可能會(huì)有速度上的差異，但結(jié)果一定一致对途。
最后赦邻，右上角的一大部分是頂點(diǎn)和邊，這些比較容易理解实檀，也是我們編程中接觸得最多的惶洲，這里不再詳述。
分析完這個(gè)圖膳犹，基本上g2o的優(yōu)化流程也就差不多清晰了恬吕。但有幾個(gè)關(guān)鍵的環(huán)節(jié)剛才并沒(méi)有提到，而且我的理解也不敢保證沒(méi)有偏差须床，寫(xiě)在下面和大家一起交流铐料。
SLAM中有一個(gè)加速增量方程求解的方法，稱為邊緣化豺旬。邊緣化是說(shuō)钠惩，如果我們把待優(yōu)化的相機(jī)位姿放在H矩陣的左上角，把待優(yōu)化的路標(biāo)點(diǎn)放在H矩陣的右下角族阅，再把H矩陣分為四塊篓跛，就可以對(duì)H的矩陣塊進(jìn)行高斯消元，使得對(duì)相機(jī)位姿的求解不依賴于路標(biāo)點(diǎn)耘分。這種方法奏效的原因是因?yàn)橄鄼C(jī)矩陣相比于路標(biāo)點(diǎn)稀疏得多举塔，因此相機(jī)矩陣塊求逆更容易绑警。當(dāng)然求泰，具體的分析建議閱讀高翔的《視覺(jué)SLAM十四講》。這里我們更關(guān)注g2o中的實(shí)現(xiàn)计盒。邊緣化部分的操作在BlockSolver<>中渴频，塊求解器會(huì)把對(duì)增量方程的求解分為兩步，先求解相機(jī)位姿的增量北启，再求解路標(biāo)點(diǎn)的增量卜朗。當(dāng)然拔第，每次求解增量仍然是調(diào)用內(nèi)部的LinearSolver。思路很清晰场钉，但g2o在這里的實(shí)現(xiàn)卻有待商榷蚊俺，塊求解器中根據(jù)頂點(diǎn)是否被邊緣化決定該頂點(diǎn)是位姿頂點(diǎn)還是路標(biāo)點(diǎn)頂點(diǎn)，也就是說(shuō)逛万，它默認(rèn)所有位姿頂點(diǎn)都不被邊緣化泳猬，所有路標(biāo)點(diǎn)頂點(diǎn)都被邊緣化。假如你嘗試不邊緣化路標(biāo)點(diǎn)頂點(diǎn)宇植，或邊緣化位姿頂點(diǎn)得封，求解器都會(huì)報(bào)錯(cuò)，這就限制了我們優(yōu)化的靈活性指郁。在我看來(lái)忙上，這可能是g2o作者實(shí)現(xiàn)過(guò)程中的一個(gè)瑕疵。BlockSolver并沒(méi)有體現(xiàn)出其應(yīng)有的抽象闲坎，它應(yīng)該根據(jù)實(shí)際的頂點(diǎn)類型來(lái)決定如何實(shí)現(xiàn)邊緣化疫粥，而不是一股腦地認(rèn)為只有路標(biāo)點(diǎn)應(yīng)該被邊緣化。（注意箫柳，這里提到的邊緣化只用于加速增量方程求解手形，不同于滑動(dòng)窗口中的邊緣化。）
對(duì)于上面的問(wèn)題悯恍，其實(shí)并非不能解決库糠。如果我們不用固定大小的BlockSolver_6_3，而是用動(dòng)態(tài)大小的BlockSolverX涮毫，就不會(huì)出問(wèn)題瞬欧。因?yàn)榍罢吣J(rèn)維度為6的位姿頂點(diǎn)被邊緣化，維度為3的路標(biāo)點(diǎn)頂點(diǎn)不被邊緣化罢防，不符合維度要求的頂點(diǎn)會(huì)導(dǎo)致出錯(cuò)艘虎。而后者并不要求邊緣化的頂點(diǎn)維度為6，也不要求不邊緣化的頂點(diǎn)維度為3咒吐，允許同時(shí)存在維度為6和維度為3的頂點(diǎn)被邊緣化野建。但問(wèn)題解決并不意味著g2o的設(shè)計(jì)沒(méi)有問(wèn)題，BlockSolver中把頂點(diǎn)維度和是否邊緣化在語(yǔ)義層面綁定了起來(lái)恬叹，很容易造成誤解候生。舉個(gè)例子，如果我想邊緣化所有位姿頂點(diǎn)绽昼，不邊緣化路標(biāo)點(diǎn)頂點(diǎn)唯鸭，最簡(jiǎn)單的解決方案是使用BlockSolverX。但一般認(rèn)為硅确，固定矩陣大小可以把一部分運(yùn)行時(shí)時(shí)間轉(zhuǎn)移到編譯期目溉。所以我可能需要typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 6>> BlockSolver_3_6;明肮，相當(dāng)于把BlockSolverTraits的第一個(gè)模板參數(shù)_PoseDim設(shè)為3，第二個(gè)模板參數(shù)_LandmarkDim設(shè)為6缭付，也就是把路標(biāo)點(diǎn)當(dāng)成位姿柿估，把位姿當(dāng)成路標(biāo)點(diǎn)。雖然可以用陷猫，但實(shí)在太過(guò)別扭官份。
總體上看，我認(rèn)為g2o是一個(gè)結(jié)構(gòu)良好的圖優(yōu)化框架烙丛，它的類層次結(jié)構(gòu)提供了很高的可擴(kuò)展性舅巷。但遺憾的是，實(shí)際實(shí)現(xiàn)的功能并不多河咽，很多類的派生類都只有一個(gè)钠右，比如只有SparseOptimizer而沒(méi)有DenseOptimizer，只有OptimizationWithHessian而沒(méi)有OptimizationWithOthers忘蟹，只有BlockSolver<>而沒(méi)有PlainSolver飒房。以至于g2o只能用于求解視覺(jué)SLAM問(wèn)題，應(yīng)用范圍有些狹隘媚值。但畢竟g2o沒(méi)有企業(yè)在背后支持狠毯，不像Ceres有谷歌撐腰，搞不好以后做SLAM的標(biāo)配會(huì)變成Ceres吧褥芒，感覺(jué)有些可惜嚼松。
文末給出了一些詳細(xì)的參考資料，比本文更有價(jià)值锰扶，大家可以參考献酗。
參考資料
《視覺(jué)SLAM十四講》 高翔
g2o學(xué)習(xí)——g2o整體框架 無(wú)人的回憶
g2o學(xué)習(xí)——頂點(diǎn)和邊之外的solver 無(wú)人的回憶
A General Framework for Graph Optimization R Kümmerle
