轉載自：http://blog.csdn.net/u012700322/article/details/52857244

原文發表於IEEE InternationalConference on Robotics and Automation (ICRA), Shanghai, China,May 2011

http://www.cnblogs.com/gaoxiang12/p/5244828.html      深入理解圖優化與g2o：圖優化篇

http://blog.csdn.NET/heyijia0327/article/details/50446140   李群、李代數在計算機視覺中的應用

建議結合本g2o論文和上面兩篇文章看，以緩解理解壓力。

摘要：

機器人和計算機視覺的很多問題，包括SLAM和BA，都可以歸結為採用最小二乘法優化影象表示的非線性誤差函式。本文描述了這些問題的結構，提出了g2o框架，一個開源C++框架，用於優化基於影象非線性誤差函式。我們的系統容易擴充套件，新的方法可以直接新增到程式碼裡面。目前提供了幾種SLAM和BA方案。本文在真實環境下和模擬的資料集上對演算法做了評估。結果表明g2o效能優良。

一  簡介

機器人和計算機視覺都有一個普遍性的問題，圖優化的線性誤差函式的最小化。比較典型的例子是SLAM和BA。解決這些問題的方法都是引數的配置或宣告變數儘可能地解釋清楚高斯噪聲對測量的影響。比如，視覺SLAM的變數可能是機器人在環境中的位置或者路標在地圖中的位置，可以通過機器人的感測器獲得。因此可以通過兩個變數的相對位置進行測量，比如里程計可以測量兩個連續的位姿距離。類似的，在BA或基於路標的SLAM中，3D雲點或路標可以通過觀察到的雲點在世界座標系的位置和感測器的位置進行測量。

這些問題都可以通過圖來表示。圖的每一個節點（Node）都表示一個可以優化的變數，兩個變數之間的每一個邊緣（Edge）都代表了和它相連的兩個成對的節點。在很多文獻中都提到了這類問題。基本上都採用了標準的方法提供一個可接受的結果解決這樣的應用問題，比如 Gauss-Newton, Levenberg-Marquardt (LM), Gauss-Seidel relaxation，或variants of gradient descent。 但如果要獲得更好的效能的話就需要專業知識並且持續努力。

本文描述了優化非線性最小二乘法問題的一般框架。我們稱為g2o（general graph optimization）。圖1是用g2o作為後端優化解決的各種問題。其效能與目前的最新的演算法相媲美，可以處理非線性度量的一般形式。我們通過以下演算法提高執行效率：

• 採用圖的稀疏連線方式；
• 採用特殊的圖結構處理上面提到的問題；
• 用一種新的方法處理離散線性系統；
• 有效利用現代處理器的指令系統SIMD優化快取的使用。

除了高效之外，g2o也很通用，擴充套件性也很好：

2D SLAM演算法的C++程式碼少於30行。使用者只需要指定誤差函式和引數就可以了。

我們將g2o應用於不同型別的最小二乘法優化問題中，用不同的演算法解決問題比較了它們的效能。我們在真實環境中和模擬的資料集上進行了評估，在所有的試驗中g2o效能與目前最新的演算法相當，在幾個例子中效能優於它們。

本文首先討論了SLAM和BA的相關工作。在第3章中，我們詳細描述了一個嵌入式的圖優化問題，討論了Gauss-Newton或LM的非線性最小二乘法。第4章中，討論了程式的特點。最後，第5章，做實驗評估了g2o和目前最新演算法的比較。

二  相關工作

圖優化理論在機器人和機器視覺領域都有大量研究。論文19做出了開創性的工作，他們通過線性迭代進行圖優化。當時，圖優化被認為實時效能很差，但最近的出現的直接線性方法，如論文4，圖優化被用於基於圖的SLAM，取得了重大進展。例如，論文12採用了一個寬鬆的方式構建地圖。論文6，採用高斯-塞德爾鬆散迭代法最小化誤差（詳見《數值分析》）。論文8介紹的多層鬆散迭代法，是高斯-塞德爾鬆散迭代法的一個變形，在不同的解析度下使用。最近，論文22提出了一個梯度下降方法優化位置圖。後來，論文4擴充套件了這種方法，他應用基於樹的引數模型來增加融合速度。兩種方法對初識的估計都具有魯棒性，也很容易實現。然而，他們假設方差近似球形，因此在位姿圖優化上會有困難，方差在特定值上為零度空間或有很大的差別。

圖優化被認為是一個非線性最小二乘法問題（見《計算方法》），他通過構建一個當前狀態的線性模型，進行迭代來解決問題。我們用於解決線性系統的方法是預處理共軛梯度方法 PCG，論文17和論文20也採用這種高效方法介紹大場景下鬆散位姿約束系統的問題。g2o具有較高效率，它包含了一個稀疏預處理共軛梯度方法 PCG，如論文13，採用block-Jacobi pre-conditioner（見《區域分解演算法-演算法與理論》）。

最近，論文5提出SAM開方系統，他們採用論文4的稀疏直接線性方法。論文14介紹了iSAM方法，可以更新非線性最小二乘問題的線性矩陣來解決這個問題。論文16通過探索線性系統的典型稀疏結構來構建線性矩陣。然而，這些方法都受制於就2D位姿圖。在g2o裡面，我們採用了類似的方法。該系統可以應用於各種變形問題的SLAM和捆集調整優化問題，例如：基於路標的2D SLAM，單目相機捆集調整，立體視覺捆集調整。然而，通過對比實驗，我們發現g2o的效能具有明顯優勢。

在計算機視覺中，論文27的稀疏捆集調整，是一個非線性最小二乘法，它採用地圖雲點和相機位姿的雅可比矩陣的稀疏性。就在最近，又出現了新的方法，論文15，論文13，提出了與稀疏線性解決方法類似的方法，如第3章D節，通過舒爾分解降維演算法有效計算大的系統（約1億個稀疏矩陣單元）。也有一些新的系統採用非線性共軛梯度法，而不是線性方法，如論文1和論文2；它的收斂速度會更慢一些，但可以用於超大資料集（約10億矩陣單元）。本文對比了g2o和論文15中的SSBA系統，它被認為是目前效能最好最新的系統。

三  最小二乘法的非線性圖優化

很多機器人和計算機視覺問題，都可以通過尋找函式的最小值問題來解決：

這裡，X=(X1T, … , XnT)T表示引數向量，每個Xi表示一個引數組，Zij和Ωij分別表示引數Xi和Xj相關的平均和係數矩陣，e(Xi, Xj, Zij)是誤差函式，表示引數Xi和Xj對約束Zij的滿足程度。當Xi和Xj完全滿足約束時，它的值是0。

為了簡化公式，我們將誤差函式分解為： 

每個誤差函式，引數塊，都佔用不同的儲存空間。這個問題可以通過圖的形式來表示。圖的節點i表示引數塊Xi，節點i和j之間的邊表示兩個引數塊Xi和Xj的約束關係。圖2 顯示了圖和目標函式的對應關係。

A.  最小二乘法優化

這裡Jij是eij(X)的雅可比矩陣，通過`X和 進行計算。代入公式5，結合公式1，我們可以得到：

根據區域性估計（區域性收斂，全域性不一定收斂），我們重寫公式1中的函式F（X）：

公式13中的二次項由公式12推導，設

通過解線性方程14取得最小的⊿X

H是方程的係數矩陣。可以用⊿X*作為初始估計來求解

高斯-牛頓演算法的迭代線性方程13，方法是方程14，等式15為迭代步驟。每次迭代，之前的結果作為一個線性點輸入，知道給定判別結束。（詳見《計算方法》）

LM演算法引入了阻尼引數來控制收斂，不同於公式14，LM的迭代算式：

λ是阻尼因子：λ越大，⊿X越小。這對控制非線性系統的步長非常有用。LM演算法是動態控制阻尼因子的。每次迭代都會控制誤差。如果新的誤差低於之前的誤差，λ就會變小。否則，λ就會變大。

B. 引數法

上面描述的計算過程的通常方法是最小化變數方程。設引數空間X是歐幾里得空間，但SLAM和BA無法使用。為了將狀態變數擴充套件到非歐空間，通常的方法是對於引數Xi的增量⊿Xi的用不同表示方法。

比如，在SLAM問題中，引數Xi由平移變數ti和旋轉變數αi表示。平移變數ti是一個歐式空間，旋轉變數αi擴充套件至2D或3D旋轉空間SO（2）或SO（3）。為了避免奇異化，向量空間通過旋轉矩陣或四元數用過引數化方法來表示。直接使用公式15做過引數化表示會破壞這個約束。為了克服這個問題，可以使用最小表示旋轉（如3D空間的尤拉角）。但這受制於奇異矩陣。

另一種方法是計算新的誤差函式，⊿Xi根據`Xi變動。⊿Xi用一個最小值表示旋轉，Xi採用過引數化方法。通常⊿Xi都比較小，但不是奇異點。變數Xi*優化後通過非線性運算獲得：

比如，在3D SLAM中，⊿Xi用平移向量和歸一化四元數表示。位姿Xi用平移向量和完全四元數。田操作符表示Xi的增量⊿Xi，使用相對運動的座標複合運算子進行運算Å（具體請查詢論文25，“Estimating uncertain spatial realtionships in robotics”），需要先將增量轉換成與位姿的相同變數：

這樣就可以定義一個新的誤差函式：

`X 擴充套件到過引數化向量空間（過引數化擬合）。

雅可比矩陣Jij變為：

增量 通過初始估計`X的歐幾里得空間計算，通過操作符田來計算重影射到原始空間。

g2o框架可以非常容易地定義不同向量空間的增量和狀態變數，可以直接支援任意引數化。儘管使用了引數化方法，海塞矩陣H仍然保留。

C.線性化系統架構

根據等式13，矩陣H和向量b可以通過以下方式計算。設 , ，那麼：

這種結構是由誤差函式的雅可比矩陣構成。誤差函式取決於2個頂點的取值，等式5的雅可比矩陣，可表示為：

Aij和Bij是誤差函式中⊿Xi和⊿Xj的偏移量。

等式9可以得到矩陣Hij：

為了簡化，我們去掉0元素。大家可能會注意到矩陣H是圖的相鄰矩陣。所以，有非零分塊是途中的邊。這是典型的稀疏H矩陣。在g2o中，我們使用了H的特性，採用最新的方法解決等式14的線性系統。

D.  系統中的特殊結構

有的問題，比如捆集調整，H矩陣具有更多的特徵。我們採用這些特徵結構對效能進行優化。在BA中，有2種變數，相機位姿p和相機觀察到的路標位姿l。重新排列等式14中的變數，相機位姿帶有下標，那麼等式就變為：

通過H矩陣的舒爾補降維：

Hll是分塊對角矩陣，計算Hll-1就比較容易。等式25根據相機位姿增量⊿Xp*來計算，我們可以計算下面的式子：

⊿X1*是調整世界特徵的。通常，世界座標特徵的數量比相機位姿多，那麼等式25比等式14的計算更快，儘管等式左邊的矩陣也要化很多時間計算。

四實施

我們的C++程式碼實現儘可能地快，但也非常通用。為了達成這一目標，我們採用對圖的頂點和邊採用抽象基類。兩個類都提供了虛擬函式，內部操作使用了局部引數提高效率。我們使用Eigen工具包，用SSE指令進行優化，可以獲得更高的效能。

圖3是我們的系統設計。灰色框部分是用於優化問題。採用所提供的基類，派生出新的節點定義田操作符操作增量。連線兩個頂點Xi，Xj的邊需要一個誤差函式eij(.)。雅可比矩陣Jij就需要具體數字，為了提高計算效率，使用者可以通過重寫虛擬類函式指定Jij。所以，解決優化問題或比較不同的過引數化問題，只需要寫很少的程式碼就可以了。

H的計算採用大小不固定的矩陣。如果變數維度，比如Xi的維度，已知，我們的框架也可以採用固定維度的矩陣計算。如果已知矩陣維度，可以使用編譯時優化，執行矩陣乘法。

特別注意，等式25的舒爾降維方法進行矩陣乘法。圖的鬆散結構只能乘以非零的矩陣，產生Hpp。另外，我們計算矩陣的分塊結構，相比矩陣數乘，在快取中執行矩陣乘法。

我們的框架對被嵌入的線性方案是不可知的，所以，我們可以用合適的方法解決不同的問題。實驗中主要有兩種方法。

五實驗

本章，我們用真實資料和合成的資料集做實驗來比較g2o和其他優化方法。圖4是3D位姿圖資料集，圖5是捆集調整資料集和Keble College位姿圖，它採用7自由度相似約束執行對尺度變換敏感的SLAM，圖6是2D資料集。變數的資料和約束如表1所示。所有的實驗都在單核Intel i7-930,2.8GHz處理器上執行。

我們將g2o和 ,SPA,sSBA,RobotVision進行對比。這些方法只針對一種優化問題的子集，而g2o針對所有問題，也很容易擴充套件到新的問題。

A.     Runtime比較

我們對比每種方案迭代一次的時間。每種方案都用全優化問題測試，測試10次迭代，計算每次迭代的平均時間。這個實驗中，g2o採用喬里斯基分解處理線性系統CHOLMOD，我們拿它來做實驗對比。因此，處理線性系統的時間類似，它們之間的差別反映的是構建線性系統的效率。結果如圖7所示。

我們的系統g2o在所有的2維和3維資料集上比 速度快。

參考文獻

[1] S. Agarwal, N. Snavely, S. M. Seitz, and R. Szeliski, “Bundle adjustment in the large,” in Proc. of the European Conf. on Computer Vision (ECCV), 2010.

[2] M. Byrod and K. Astroem, “Conjgate gradient bundle adjustment,” in Proc. of the European Conf. on Computer Vision (ECCV), 2010.

[3] Y. Chen, T. A. Davis, W. W. Hager, and S. Rajamanickam, “Algorithm 887: Cholmod, supernodal sparse cholesky factorization and update/downdate,” ACM Trans. Math. Softw., vol. 35, no. 3, pp. 1–14, 2008.

[4] T. A. Davis, Direct Methods for Sparse Linear Systems. SIAM, 2006, part of the SIAM Book Series on the Fundamentals of Algorithms.

[5] F. Dellaert and M. Kaess, “Square Root SAM: Simultaneous localization and mapping via square root information smoothing,” Int. Journal of Robotics Research, vol. 25, no. 12, pp. 1181–1204, Dec 2006.

[6] T. Duckett, S. Marsland, and J. Shapiro, “Fast, on-line learning of globally consistent maps,” Autonomous Robots, vol. 12, no. 3, pp. 287 – 300, 2002.

[7] U. Frese, “A proof for the approximate sparsity of SLAM information matrices,” in Proc. of the IEEE Int. Conf. on Robotics & Automation (ICRA), 2005.

[8] U. Frese, P. Larsson, and T. Duckett, “A multilevel relaxation algorithm for simultaneous localisation and mapping,” IEEE Transactions on Robotics, vol. 21, no. 2, pp. 1–12, 2005.

[9] G. Grisetti, R. K¨ummerle, C. Stachniss, U. Frese, and C. Hertzberg, “Hierarchical optimization on manifolds for online 2d and 3d mapping,” in Proc. of the IEEE Int. Conf. on Robotics & Automation (ICRA), 2010.

[10] G. Grisetti, C. Stachniss, and W. Burgard, “Non-linear constraint network optimization for efficient map learning,” IEEE Trans. on Intelligent Transportation Systems, 2009.

[11] G. Guennebaud, B. Jacob, et al., “Eigen v3,” http://eigen.tuxfamily.org, 2010.

[12] A. Howard, M. Matari´c, and G. Sukhatme, “Relaxation on a mesh: a formalism for generalized localization,” in Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS), 2001.

[13] Y. Jeong, D. Nister, D. Steedly, R. Szeliski, and I. Kweon, “Pushing the envelope of modern methods for bundle adjustment,” in Proc. of the IEEE Conf. on Comp. Vision and Pattern Recognition (CVPR), 2010.

[14] M. Kaess, A. Ranganathan, and F. Dellaert, “iSAM: Incremental smoothing and mapping,” IEEE Trans. on Robotics, vol. 24, no. 6, pp. 1365–1378, Dec 2008.

[15] K. Konolige, “Sparse sparse bundle adjustment,” in Proc. of the British Machine Vision Conference (BMVC), 2010.

[16] K. Konolige, G. Grisetti, R. K¨ummerle, W. Burgard, B. Limketkai, and R. Vincent, “Sparse pose adjustment for 2d mapping,” in Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS), 2010.

[17] K. Konolige, “Large-scale map-making,” in Proc. of the National Conference on Artificial Intelligence (AAAI), 2004.

[18] M. A. Lourakis and A. Argyros, “SBA: A Software Package for Generic Sparse Bundle Adjustment,” ACM Trans. Math. Software, vol. 36, no. 1, pp. 1–30, 2009.

[19] F. Lu and E. Milios, “Globally consistent range scan alignment for environment mapping,” Autonomous Robots, vol. 4, pp. 333–349, 1997.

[20] M. Montemerlo and S. Thrun, “Large-scale robotic 3-d mapping of urban structures,” in Proc. of the Int. Symposium on Experimental Robotics (ISER), 2004.

[21] K. Ni and F. Dellaert, “Multi-level submap based SLAM using nested dissection,” in Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS), 2010.

[22] E. Olson, J. Leonard, and S. Teller, “Fast iterative optimization of pose graphs with poor initial estimates,” in Proc. of the IEEE Int. Conf. on Robotics & Automation (ICRA), 2006, pp. 2262–2269.

[23] W. Press, S. Teukolsky, W. Vetterling, and B. Flannery, Numerical Recipes, 2nd Edition. Cambridge Univ. Press, 1992.

[24] M. Smith, I. Baldwin, W. Churchill, R. Paul, and P. Newman, “The new college vision and laser data set,” Int. Journal of Robotics Research, vol. 28, no. 5, pp. 595–599, May 2009.

[25] R. Smith, M. Self, and P. Cheeseman, “Estimating uncertain spatial realtionships in robotics,” in Autonomous Robot Vehicles, I. Cox and G. Wilfong, Eds. Springer Verlag, 1990, pp. 167–193.

[26] H. Strasdat, J. M. M. Montiel, and A. Davison, “Scale drift-aware large scale monocular SLAM,” in Proc. of Robotics: Science and Systems (RSS), 2010.

[27] B. Triggs, P. F. McLauchlan, R. I. Hartley, and A. W. Fitzibbon, “Bundle adjustment - a modern synthesis,” in Vision Algorithms: Theory and Practice, ser. LNCS. Springer Verlag, 2000, pp. 298–375.