g2o: 如何使用g2o的例子

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

g2o代码示例

1. simple_optimize

simple_optimize 所在的github 链接地址

首先, 看效果

数据集可以在OpenSLAM Vertigo 下载, 详见链接. 
包括 city10000, intel, manhattan, ring, ringCity, sphere2500.

先测试sphere2500.

运行前 
 
运行后 

测试manhattanOlson3500.

运行前 
 
运行后 

sphere2500 和manhattanOlson3500的维度不一样,故而给出两次测试.

其次, 看代码

此处的代码是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 -1;
    }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(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.save("../data/manhattanOlson3500_after.g2o");
    optimizer.clear();

    return 0;
}

• 代码中需要注意的地方, 

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
• 2

1. MAXITERATION 执行优化次数

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

CMakeLists.txt

cmake_minimum_required(VERSION 2.8)
project(simple_optimize)

#Enable support for C++11
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

最后, 介绍使用过程

为方便使用,也为自己存档, CSDN下载地址: 
g2o example simple_optimize

接着, 编译运行

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

• 查看结果

g2o_viewer ../data/sphere_after.g2o