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;
    }

    //优化过程中,第一个点固定,不做优化; 优化过程中,必须固定一个pose。
    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》


发布了57 篇原创文章 · 获赞 1241 · 访问量 138万+
展开阅读全文

没有更多推荐了,返回首页

©️2019 CSDN 皮肤主题: 游动-白 设计师: 上身试试

分享到微信朋友圈

×

扫一扫,手机浏览