相关文献
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 -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);
- MAXITERATION 执行优化次数
- 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
使用过程如下
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