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 
阅读更多
想对作者说点什么?

博主推荐

换一批

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