首先，致謝半閒居士（http://www.tuicool.com/articles/QZVjuu）。
RGBD-SLAM用的傳感器是kinect，可以得到深度信息與圖像信息。我在實驗中使用的是激光掃描儀，只能得到深度信息，不過原理是一致的。
1. 得到三維點雲圖；
2. 對三維點雲圖進行兩兩匹配；
3. 進行圖形優化；
4. 紋理貼圖。
本文重點講述第三點：圖形優化問題（g2o）。
要了解g2o，可以參考以下一些文章：
Hierarchical Optimization on Manifolds for Online 2D and 3D Mapping;(2010)
g2o: A General Framework for Graph Optimization;(2011)
Experimental Analysis of Dynamic Covariance Scaling for Robust Map Optimization Under Bad Initial Estimates;(2014)
以上3篇文章，第二篇是基礎（爲什麼第二篇文章比第一篇文章發表的晚，這個我也弄不明白，這是個bug！！），第二篇文章主要就是講了如何建模，從而引入優化問題，之後呢，作者對該非線性問題進行了線性化，從而可以解；但是，以上是基於歐式空間的假設。進而，作者對其進行了擴展，擴展到了普遍意義上的優化問題。第一篇文章講了g2o的一種應用方法，即將整個圖形進行了分層處理。第三篇文章講到：如果初始位置估計誤差很大，則用原始g2o可能得不到自己想要的結果，於是，在優化問題中引入了一個變量參數。
g2o的基礎是：頂點（vertex）和邊（edge）。頂點即爲機器人的位置估計，邊即爲兩個位置之間的約束，簡而言之，就是兩個位置的關係（比如，在我的實驗中，就是機器人兩個位置之間的旋轉平移矩陣）。理解g2o的應用，最好的例子就是g2o目錄下的例子：tutorial_slam2d。
g2o最好在檢測到閉環之後再用，這樣全局優化的效果最好，比如：一個閉環有20個poses，我們可以得到相鄰poses的約束20個，此時，我們可以在g2o對象中加入20個頂點，以及20個邊，之後，我們可以得到優化之後的poses，它會將第一個pose和最後一個pose大概捏到一塊，這個的決定因素是第一幀和最後一幀的約束，這個矩陣的模越小，它們將會捱得更近。當然了，如果優化效果並不是很好的情況下，我們可以加入更多的邊，更多的約束。注意：加入的約束儘量要正確，不要引入誤差，否則，效果將適得其反。
這是我用matlab畫出的poses圖，藍色線是g2o前的，紅的是g2o之後的，起點是[1,1,1]。從圖中可以看出，最後一個pose是[-1508,274.5，-2213],和第一個pose差別很大，但是經過g2o優化之後，他們兩個幾乎到一塊兒去了，並且對其他poses進行了修正。