g2o的使用

相关文献

1.论文 Grisetti, Giorgio, et al. “A tutorial on graph-based SLAM.” IEEE Intelligent Transportation Systems Magazine 2.4 (2010): 31-43. pdf
2.论文 Kümmerle, Rainer, et al. “g2o: A general framework for graph optimization.” Robotics and Automation (ICRA), 2011 IEEE International Conference on. IEEE, 2011. pdf
3.课件 Least Squares and SLAM Pose-SLAM pdf1 pdf2

1.g2o的代码示例

simple_optimize

地址http://download.csdn.net/detail/ziqian0512/9851689

数据集可以在这里下载

http://www.openslam.org/vertigo或者更进一步https://svn.openslam.org/data/svn/vertigo/trunk/datasets/

包括 city10000, intel, manhattan, ring, ringCity, sphere2500.

先测试sphere2500.

运行前

运行后

simple_optimize.cpp

#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/slam2d/vertex_se2.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); 

#include <iostream>

using namespace std;
using namespace g2o;

#define MAXITERATION 10
int main()
{
    // 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);

    // create the optimizer
    SparseOptimizer optimizer;

    if(!optimizer.load("../data/sphere_bignoise_vertex3.g2o"))
    // if(!optimizer.load("../data/manhattanOlson3500.g2o"))
    {
        cout<<"Error loading graph"<<endl;
        return -;
    }else
    {
        cout<<"Loaded "<<optimizer.vertices().size()<<" vertices"<<endl;
        cout<<"Loaded "<<optimizer.edges().size()<<" edges"<<endl;
    }

    // VertexSE2* firstRobotPose = dynamic_cast<VertexSE2*>(optimizer.vertex(0));
    VertexSE3* firstRobotPose = dynamic_cast<VertexSE3*>(optimizer.vertex());
    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.save("../data/manhattanOlson3500_after.g2o");
    optimizer.clear();

    return ;
}

代码中需要注意的地方,
1. G2O_USE_TYPE_GROUP 保证能够进graph的load 和save, 进行类型注册, 也就是optimizer.load
和optimizer.save 在执行时有相对的类型对应. slam3d 和slam2d是对应不同的顶点和边的类型.
这里都registration

G2O_USE_TYPE_GROUP(slam3d);
G2O_USE_TYPE_GROUP(slam2d); 

1. MAXITERATION 执行优化次数

2. VertexSE3* firstRobotPose
   此处本来是设定不优化的固定点的,但是考虑不同的维度的顶点需要采用不同的类型设置固定点,所以在此提出.

CMakeLists.txt

cmake_minimum_required(VERSION 2.8)
project(simple_optimize)

#Enable support for C++
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")

#set binary path
SET(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
#set lib path
SET(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
#set .cmake path
#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(simple_optimize simple_optimize.cpp)
target_link_libraries(simple_optimize ${G2O_LIBS})

而对于manhattanOlson3500的测试代码, 三维到二维仅仅需要修改3处
1. optimizer.load()
2. optimizer.save()
3. VertexSE3* firstRobotPose 成VertexSE2* firstRobotPose

使用过程如下

cd simple_optimize
mkdir build
cmake ..
make
cd ../bin
./simple_optimize

查看结果

g2o_viewer ../data/sphere_after.g2o 

这篇博客也介绍了如何使用g2o

http://blog.csdn.net/ganbaoni9yang/article/details/50460059