0 參考鏈接
[從零開始一起學習SLAM | 理解圖優化，一步步帶你看懂g2o代碼]
本文參考上面兩個鏈接，想要了解詳細內容，參考上面兩個
1 基本流程梳理
圖的核心
SparseOptimizer是整個圖的核心， is-a 是實心箭頭，表示這個SparseOptimizer它是一個Optimizable Graph，從而也是一個超圖（HyperGraph）
定點和邊
has-many 表示這個超圖（HyperGraph）包含了許多頂點（HyperGraph::Vertex）和邊（HyperGraph::Edge）。而這些頂點繼承自 Base Vertex，也就是OptimizableGraph::Vertex，而邊可以繼承自 BaseUnaryEdge（單邊）, BaseBinaryEdge（雙邊）或BaseMultiEdge（多邊），它們都叫做OptimizableGraph::Edge
is-a:相當於一個等號
has-a:表示包含一個，這個包含的的屬於必備的組件
has-many:和has-a差不多，至少要有一個
配置SparseOptimizer的優化算法和求解器
核心SparseOptimizer 包含一個優化算法（OptimizationAlgorithm）的對象。OptimizationAlgorithm是通過OptimizationWithHessian 來實現的。其中迭代策略可以從Gauss-Newton（高斯牛頓法，簡稱GN）, Levernberg-Marquardt（簡稱LM法）, Powell’s dogleg 三者中間選擇一個（我們常用的是GN和LM）
求解流程
OptimizationWithHessian 內部包含一個求解器（Solver），這個Solver實際是由一個BlockSolver組成的。這個BlockSolver有兩個部分，一個是SparseBlockMatrix ，用於計算稀疏的雅可比和Hessian矩陣；一個是線性方程的求解器（LinearSolver），它用於計算迭代過程中最關鍵的一步HΔx=−b，LinearSolver有幾種方法可以選擇：PCG, CSparse, Choldmod
2 代碼文件
$ tree -L 3 -d
.
├── bin
├── build
│ ├── CMakeFiles
│ │ ├── 3.9.1
│ │ └── CMakeTmp
│ ├── EXTERNAL
│ │ ├── CMakeFiles
│ │ ├── csparse
│ │ └── freeglut
│ └── g2o
│ ├── apps
│ ├── CMakeFiles
│ ├── core
│ ├── examples
│ ├── solvers
│ ├── stuff
│ └── types
├── cmake_modules
├── doc
│ ├── doxygen
│ └── pics
├── EXTERNAL
│ ├── ceres
│ ├── csparse
│ └── freeglut
├── g2o
│ ├── apps
│ │ ├── g2o_cli
│ │ ├── g2o_hierarchical
│ │ ├── g2o_simulator
│ │ ├── g2o_viewer
│ │ └── linked_binaries
│ ├── core
│ ├── examples
│ │ ├── ba
│ │ ├── ba_anchored_inverse_depth
│ │ ├── bal
│ │ ├── calibration_odom_laser
│ │ ├── data_convert
│ │ ├── data_fitting
│ │ ├── g2o_unfold
│ │ ├── icp
│ │ ├── interactive_slam
│ │ ├── line_slam
│ │ ├── plane_slam
│ │ ├── sba
│ │ ├── simple_optimize
│ │ ├── slam2d
│ │ ├── sphere
│ │ ├── target
│ │ └── tutorial_slam2d
│ ├── solvers
│ │ ├── cholmod
│ │ ├── csparse
│ │ ├── dense
│ │ ├── eigen
│ │ ├── pcg
│ │ ├── slam2d_linear
│ │ └── structure_only
│ ├── stuff
│ └── types
│ ├── data
│ ├── deprecated
│ ├── icp
│ ├── sba
│ ├── sclam2d
│ ├── sim3
│ ├── slam2d
│ ├── slam2d_addons
│ ├── slam3d
│ └── slam3d_addons
├── lib
└── script
g2o項目中含有若干文件夾。刨開那些gitignore之類的零碎文件，主要有以下幾個：
- EXTERNAL 三方庫，有ceres, csparse, freeglut，可以選擇性地編譯；
- cmake_modules 給cmake用來尋找庫的文件。我們用g2o時也會用它裏頭的東西，例如FindG2O.cmake
- doc 文檔。包括g2o自帶的說明書（難度挺大的一個說明文檔）。
- g2o 最重要的源代碼都在這裏！
- script 在android等其他系統編譯用的腳本
3 代碼使用舉例
typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block; // 每個誤差項優化變量維度爲3，誤差值維度爲1
// 第1步：創建一個線性求解器LinearSolver
Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();
// 第2步：創建BlockSolver。並用上面定義的線性求解器初始化
Block* solver_ptr = new Block( linearSolver );
// 第3步：創建總求解器solver。並從GN, LM, DogLeg 中選一個，再用上述塊求解器BlockSolver初始化
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
// 第4步：創建終極大boss 稀疏優化器（SparseOptimizer）
g2o::SparseOptimizer optimizer; // 圖模型
optimizer.setAlgorithm( solver ); // 設置求解器
optimizer.setVerbose( true ); // 打開調試輸出
// 第5步：定義圖的頂點和邊。並添加到SparseOptimizer中
CurveFittingVertex* v = new CurveFittingVertex(); //往圖中增加頂點
v->setEstimate( Eigen::Vector3d(0,0,0) );
v->setId(0);
optimizer.addVertex( v );
for ( int i=0; i<N; i++ ) // 往圖中增加邊
{
CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );
edge->setId(i);
edge->setVertex( 0, v ); // 設置連接的頂點
edge->setMeasurement( y_data[i] ); // 觀測數值
edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); // 信息矩陣：協方差矩陣之逆
optimizer.addEdge( edge );
}
// 第6步：設置優化參數，開始執行優化
optimizer.initializeOptimization();
optimizer.optimize(100);