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 链接地址

首先, 看效果

测试manhattanOlson3500.

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

其次, 看代码

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;

{
return -1;
}else
{
}

// 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处

2. optimizer.save()
3. VertexSE3* firstRobotPose 成VertexSE2* firstRobotPose

最后, 介绍使用过程

g2o example simple_optimize

cd simple_optimize
mkdir build
cmake ..
make
cd ../bin
./simple_optimize
• 查看结果
g2o_viewer ../data/sphere_after.g2o 

• 广告
• 抄袭
• 版权
• 政治
• 色情
• 无意义
• 其他

120