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这些依赖项：
sudo apt-get install libeigen3-dev libsuitesparse-dev libqt4-dev qt4-qmake libqglviewer-qt4-dev其中libqglviewer是编译g2o_viewer必须的。
- 接着把g2o代码下载到相应文件夹里（可以使用git clone命令或者自己手动到网站下载）
- 编译和安装
mkdir build cd build cmake ../ 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工程里常见的文件夹。lz测试g2o的数据和代码放在我的github里：https://github.com/HeYijia/GraphSLAM_tutorials_code/tree/master/g2o_test
先贴出源程序：
#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/factory.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/solvers/csparse/linear_solver_csparse.h"
#include "g2o/types/slam3d/vertex_se3.h"
//#include "g2o/types/slam3d/edge_se3.h"
// 使用 宏函数 声明边和顶点类型，注意注释掉了上面两个头文件
G2O_USE_TYPE_GROUP(slam3d);
//G2O_USE_TYPE_GROUP(slam2d); //2d平面
#include <iostream>
using namespace std;
using namespace g2o;
#define MAXITERATION 50
int main()
{
cout<< "Hello g2o"<<endl;
// create the linear solver
BlockSolverX::LinearSolverType * linearSolver = new LinearSolverCSparse<BlockSolverX::PoseMatrixType>();
// create the block solver on the top of the linear solver
BlockSolverX* blockSolver = new BlockSolverX(linearSolver);
//create the algorithm to carry out the optimization
OptimizationAlgorithmLevenberg* optimizationAlgorithm = new OptimizationAlgorithmLevenberg(blockSolver);
/* //如果没用前面的宏函数，而是调用的是edge_se3和vertex_se3头文件
//想让程序能识别VertexSE3这些数据类型，就要显示的调用它们，如下
//如果只用了头文件，而没用显示调用，那么这些数据类型将不会link进来
//在下面的optimizer.load函数将不能识别这些数据类型
for(int f=0; f<10;++f)
{
VertexSE3* v = new VertexSE3;
v->setId(f++);
}
*/
// create the optimizer
SparseOptimizer optimizer;
if(!optimizer.load("../data/sphere_bignoise_vertex3.g2o"))
{
cout<<"Error loading graph"<<endl;
return -1;
}else
{
cout<<"Loaded "<<optimizer.vertices().size()<<" vertices"<<endl;
cout<<"Loaded "<<optimizer.edges().size()<<" edges"<<endl;
}
//优化过程中，第一个点固定，不做优化; 也可以不固定。
VertexSE3* firstRobotPose = dynamic_cast<VertexSE3*>(optimizer.vertex(0));
firstRobotPose->setFixed(true);
optimizer.setAlgorithm(optimizationAlgorithm);
optimizer.setVerbose(true);
optimizer.initializeOptimization();
cerr<<"Optimizing ..."<<endl;
optimizer.optimize(MAXITERATION);
cerr<<"done."<<endl;
optimizer.save("../data/sphere_after.g2o");
//optimizer.clear();
return 0;
}
程序解读：
程序思路很简单，初始化优化方法，接着创建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内容如下：
cmake_minimum_required(VERSION 2.8)
project(g2o_test)
#Enable support for C++11
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
#设定二进制文件路径
SET(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
#设定库文件编译路径
SET(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
#设定.cmake文件存放路径
#SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "{CMAKE_CURRENT_SOURCE_DIR}/cmake_modules")
LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)
#ADD_SUBDIRECTORY(${PROJECT_SOURCE_DIR}/src)
# find g2o lib
find_package(G2O REQUIRED)
IF(G2O_FOUND)
include_directories(${G2O_INCLUDE_DIR})
message("G2O lib is found:"${G2O_INCLUDE_DIR})
ENDIF(G2O_FOUND)
find_package(Eigen3 REQUIRED)
find_package(CSparse REQUIRED)
#find_package(Cholmod REQUIRED)
include_directories(${CSPARSE_INCLUDE_DIR})
include_directories(${EIGEN3_INCLUDE_DIR})
#include_directories(${CHOLMOD_INCLUDE_DIR})
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 )
ADD_EXECUTABLE(g2o_test main.cpp)
target_link_libraries(g2o_test ${G2O_LIBS})
#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程序。
（转载请注明作者和出处：http://blog.csdn.net/heyijia0327 未经允许请勿用于商业用途）
reference：
1. Rainer & Grisetti 《g2o: A General Framework for Graph Optimization》
2. Grisetti 《g2o：a general framework for(hyper) graph optimization》