graph slam tutorial : g2o 的使用

        g2o全称general graph optimization，是一个用来优化非线性误差函数的c++框架。如果阅读了前几篇graph slam tutorial的博客，再去读《g2o：a general framework for(hyper) graph optimization》这篇论文将变得轻松随意。读完论文以后，再看看github里面g2o example的例程就能上手g2o了。

        然而，看懂是一回事，自己写出来是另一回事，特别是如何将g2o用在自己的程序里面。因此，本文的主要目的是记录如何在自己程序里使用g2o，包括cmakelist编写，g2o函数的调用两方面。在往下读之前，希望你已经读完了g2o的那篇论文。

 

安装g2o

        github上有完整的教程，安装编译也很容易（g2o代码网站请点击）。

• 首先确保电脑上面已经有了cmake/eigen3/suitesparse/qt4/libqglviewer这些依赖项：

[plain]  view plain copy

1. sudo apt-get install libeigen3-dev libsuitesparse-dev libqt4-dev qt4-qmake libqglviewer-qt4-dev  

其中libqglviewer是编译g2o_viewer必须的。

• 接着把g2o代码下载到相应文件夹里（可以使用git clone命令或者自己手动到网站下载）
• 编译和安装

[plain]  view plain copy

1. mkdir build  
2. cd build  
3. cmake ../  
4. make  

编译完以后，在bin文件夹里就生成了各种可执行文件了，包括g2o_viewer,tutorial_slam2d等。为了把这个g2o当作一个外部库在自己程序中（像opencv一样）使用，make编译完以后，再用sudo make install 安装一下这个库。执行完命令，将看到 /usr/local/include文件夹里多了g2o这一项。注意，不install也行，但是自己程序里调用g2o时cmaklist的书写就不一样了，可以参看orb_slam。在下面我的程序中调用g2o，是基于make intall以后的。

在自己程序中使用g2o

       在该部分，我们调用g2o程序优化一个球，就是在论文中经常看到的那个例子，优化前后效果如下。首先自己创建一个g2o_test文件件用来存放我们将要编写的代码，在g2o_test文件里创建build,bin,lib这些cmake工程里常见的文件夹。测试g2o的数据和代码放在这个github里：https://github.com/HeYijia/GraphSLAM_tutorials_code/tree/master/g2o_test

        

先贴出源程序：

[cpp]  view plain copy

1. #include "g2o/core/sparse_optimizer.h"  
2. #include "g2o/core/block_solver.h"  
3. #include "g2o/core/factory.h"  
4. #include "g2o/core/optimization_algorithm_levenberg.h"  
5. #include "g2o/solvers/csparse/linear_solver_csparse.h"  
6.   
7. #include "g2o/types/slam3d/vertex_se3.h"  
8. //#include "g2o/types/slam3d/edge_se3.h"  
9. // 使用 宏函数 声明边和顶点类型，注意注释掉了上面两个头文件   
10. G2O_USE_TYPE_GROUP(slam3d);  
11. //G2O_USE_TYPE_GROUP(slam2d); //2d平面  
12.   
13. #include <iostream>  
14.   
15. using namespace std;  
16. using namespace g2o;  
17.   
18. #define MAXITERATION 50  
19. int main()  
20. {  
21.     cout<< "Hello g2o"<<endl;  
22.     // create the linear solver  
23.     BlockSolverX::LinearSolverType * linearSolver = new LinearSolverCSparse<BlockSolverX::PoseMatrixType>();  
24.   
25.     // create the block solver on the top of the linear solver  
26.     BlockSolverX* blockSolver = new BlockSolverX(linearSolver);  
27.       
28.     //create the algorithm to carry out the optimization  
29.     OptimizationAlgorithmLevenberg* optimizationAlgorithm = new OptimizationAlgorithmLevenberg(blockSolver);  
30.   
31. /*  //如果没用前面的宏函数，而是调用的是edge_se3和vertex_se3头文件 
32.     //想让程序能识别VertexSE3这些数据类型，就要显示的调用它们，如下 
33.     //如果只用了头文件，而没用显示调用，那么这些数据类型将不会link进来 
34.     //在下面的optimizer.load函数将不能识别这些数据类型 
35.     for(int f=0; f<10;++f) 
36.     { 
37.         VertexSE3* v = new VertexSE3; 
38.         v->setId(f++); 
39.     } 
40. */  
41.     // create the optimizer  
42.     SparseOptimizer optimizer;  
43.       
44.     if(!optimizer.load("../data/sphere_bignoise_vertex3.g2o"))  
45.     {  
46.         cout<<"Error loading graph"<<endl;  
47.         return -1;  
48.     }else  
49.     {  
50.         cout<<"Loaded "<<optimizer.vertices().size()<<" vertices"<<endl;  
51.         cout<<"Loaded "<<optimizer.edges().size()<<" edges"<<endl;  
52.     }  
53.   
54.     //优化过程中，第一个点固定，不做优化; 也可以不固定。  
55.     VertexSE3* firstRobotPose = dynamic_cast<VertexSE3*>(optimizer.vertex(0));  
56.     firstRobotPose->setFixed(true);  
57.   
58.     optimizer.setAlgorithm(optimizationAlgorithm);  
59.     optimizer.setVerbose(true);  
60.     optimizer.initializeOptimization();  
61.     cerr<<"Optimizing ..."<<endl;  
62.     optimizer.optimize(MAXITERATION);  
63.     cerr<<"done."<<endl;  
64.   
65.     optimizer.save("../data/sphere_after.g2o");  
66.     //optimizer.clear();  
67.   
68.     return 0;  
69. }  

       程序解读：

       程序思路很简单，初始化优化方法，接着创建optimizer优化器，并load() g2o数据文件，然后就是优化。是不是很简单。自己写slam程序时难的部分在数据文件g2o的创建，也就是顶点vertex和边edge的创建，这里直接使用的是已经创建好的数据文件，load一下就行了。在g2o的论文里，把怎么创建顶点和边的数据类型讲的很详细，如tutorial_slam2d那个程序。然而实际使用的时候，g2o已经帮我们定义好了各种类型的顶点和边，如用于3d slam的vertex_se3和edge_se3等，这些数据类型在g2o/types文件里可以找到，因此实际使用的时候只要调用就行了，不需要像论文里那么复杂。

       上面部分程序主要参考的g2o/example里面的simple_optimize,点击这里。在这个程序里，我们发现有一个宏函数在论文g2o的论文中没提到过，并且调用vertex_se3和edge_se3的头文件指令被注释掉了。

       这是怎么回事呢？即为什么使用G2O_USE_TYPE_GROUP？

       答：一开始我程序里面用#include “.../edge3_se3.h”和#include “.../vertex_se3.h”，以为这样程序就能识别这种类型的顶点和边了。结果运行程序的时候发现然并卵，optimizer.load()竟然不识别这些数据类型。然后就想手动调用这些数据类型，看看头文件是否找到了，于是就有了注释掉的VertexSE3 *v这一句程序，加了这句以后，再运行程序，load数据成功。这时开始纳闷了，于是翻看g2o example里simple_optimize这个程序，发现了这个宏函数。google一下，有了这么一段话：Furthermore, the macro G2O_REGISTER_TYPE_GROUP allows to declare a type group. This is necessary if we use the factory to construct the types and we have to enforce that our code is linked to a specific type group. Otherwise the linker may drop our library, since we do not explicitly use any symbol provided by the library containing our type. Declaring the usage of a specific type library and hence enforcing the linking is done by the macro called G2O_USE_TYPE_GROUP.于是豁然开朗。如果你要是在程序里显式的调用边或者顶点数据类型，就可以不用这个宏函数，而用头文件的形式就行了。由于，我们这里是load已经构建好的g2o文件，没有显式调用VertexSE3这些数据类型，所以就使用这个宏函数。注意，以后在自己的SLAM程序中，顶点和边一般都是临时构建的，一般是用头文件和显式调用的形式，可以不用这个宏函数。

      cmakelist的编写：

       在上面程序中我们使用include来调用g2o的头文件。但是，我们得告诉工程文件这些头文件存放在那里，怎么调用，比如在windows下vs中使用opencv时要进行各种设置一样。在linux下，得依靠cmake来完成这些工作。g2o作为一个外接程序库在自己程序里怎么使用呢？如果不熟悉cmake的话，可以点击这里（在工程中查找和使用其他程序库的方法），这里以及这里。

       有了cmake的知识以后，我们知道只要在cmakelist文件里写上find_package这条命令就能解决问题。并且要把编译好的g2o/cmake_modules文件里的cmake文件复制到自己的程序里，找到下载的g2o/cmake_modules文件夹，把所有的.cmake文件复制到你的程序目录的modules文件夹里，如g2o_test/cmake_modules.

       接下来就是编写cmakelist文件，如果你不熟悉cmake的写法，可以趁着这个机会自己练练，楼主也是一边看教程，一边照着rgbdslam、orb_slam的cmake写法自己捣鼓出来的。在这个过程中慢慢地就熟悉了cmake的命令和写法。cmakelist内容如下：

[plain]  view plain copy

1. cmake_minimum_required(VERSION 2.8)  
2. project(g2o_test)  
3.   
4. #Enable support for C++11  
5. SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")  
6.   
7. #设定二进制文件路径  
8. SET(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)  
9. #设定库文件编译路径  
10. SET(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)  
11. #设定.cmake文件存放路径  
12. #SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "{CMAKE_CURRENT_SOURCE_DIR}/cmake_modules")  
13. LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)  
14. #ADD_SUBDIRECTORY(${PROJECT_SOURCE_DIR}/src)  
15.   
16. # find g2o lib  
17. find_package(G2O REQUIRED)  
18. IF(G2O_FOUND)  
19.     include_directories(${G2O_INCLUDE_DIR})  
20.     message("G2O lib is found:"${G2O_INCLUDE_DIR})  
21. ENDIF(G2O_FOUND)  
22.   
23. find_package(Eigen3 REQUIRED)  
24. find_package(CSparse REQUIRED)  
25. #find_package(Cholmod REQUIRED)  
26. include_directories(${CSPARSE_INCLUDE_DIR})  
27. include_directories(${EIGEN3_INCLUDE_DIR})  
28. #include_directories(${CHOLMOD_INCLUDE_DIR})  
29.   
30. SET(G2O_LIBS g2o_cli g2o_ext_freeglut_minimal g2o_simulator g2o_solver_slam2d_linear g2o_types_icp g2o_types_slam2d g2o_core g2o_interface g2o_solver_csparse g2o_solver_structure_only g2o_types_sba g2o_types_slam3d g2o_csparse_extension g2o_opengl_helper g2o_solver_dense g2o_stuff g2o_types_sclam2d g2o_parser g2o_solver_pcg g2o_types_data g2o_types_sim3 cxsparse )  
31.   
32. ADD_EXECUTABLE(g2o_test main.cpp)  
33. target_link_libraries(g2o_test ${G2O_LIBS})  
34. #target_link_libraries(g2o_test csparse g2o_core g2o_solver_cholmod g2o_solver_csparse g2o_types_slam3d)  

       编写完以后，编译，在bin文件里生成了g2o_test可执行文件，切换到bin文件夹下 运行 ./g2o_test就可以看效果了。程序最后只是生成的sphere_after.g2o文件，请使用官方g2o/bin文件里的g2o_viewer程序来打开这个文件，效果如上图所示。

       在上面的程序中，优化开始前，我们固定了第一个顶点不进行优化。当然我们也可以不固定，可以自己对比效果。其他条件不变的情况下，固定第一个顶点比不固定效果要好，下图左为固定第一个顶点。

 

      知道了如何在自己的程序中使用g2o进行优化以后，解决了grpah-based slam的back-end问题，因此编写front-end的程序（点云匹配，闭环检测等）就能完成自己的rgbdslam程序。在下个教程中，将使用公共RGBD数据集来完成整个RGBDSLAM程序。

reference：

1. Rainer & Grisetti  《g2o: A General Framework for Graph Optimization》

2. Grisetti  《g2o：a general framework for(hyper) graph optimization》

转自：白巧克力亦唯心 http://blog.csdn.net/heyijia0327