沉默杀手
g2o的基础使用
|SLAM工具|
Word count: 715|Reading time: 3 min

以下为ubuntu18版本的g2o对应的代码，和16版不兼容。TEB的常见安装问题就是16版到18版不兼容

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
// 求解器一般有两种, TEB用的是 Levenberg
typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 3>> Block; // 每个误差项优化变量维度为3，误差值维度为1

Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();

Block* solver_ptr = new Block(std::unique_ptr<Block::LinearSolverType>(linearSolver) );

g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::unique_ptr<Block>(solver_ptr) );

g2o::SparseOptimizer optimizer;
// setAlgorithm()需要传递一个优化算法
optimizer.setAlgorithm(solver);
optimizer.setVerbose(true);
optimizer->initMultiThreading();

// ......
optimizer.optimize(maxIterations); 	//开始优化，设置迭代次数

  • static bool initMultiThreading();

Eigen3.1开始，要求we call an initialize函数，如果从不同线程调用Eigen
如果g2o基于OpenMP编译，而且Eigen版本高于3.1，则这个函数调用 Eigen::initParallel

LinearSolver

它用来求解线性方程 ，可以从PCG、CSparse、Choldmod、Dense、Eigen选择求解方法。

  • LinearSolverCSparse：使用CSparse法。继承自LinearSolverCCS

  • LinearSolverCholmod ：使用sparse cholesky分解法。继承自LinearSolverCCS

  • LinearSolverDense ：使用dense cholesky分解法。继承自LinearSolver

  • LinearSolverEigen： 依赖项只有eigen，使用eigen中的sparse Cholesky 求解，因此编译好后可以方便的在其他地方使用，性能和CSparse差不多。继承自LinearSolver

  • LinearSolverPCG ：使用preconditioned conjugate gradient 法，继承自LinearSolver

选哪个求解器很重要，有一次使用LinearSolverDense，结果260秒才迭代一次，换成LinearSolverCSparse，0.4秒就迭代一次

TEB是涉及到CSparse和Choldmod，默认使用前者

解析器solver的另一种创建方式

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
// 加载几组 solver
G2O_USE_OPTIMIZATION_LIBRARY(pcg)
G2O_USE_OPTIMIZATION_LIBRARY(cholmod)
G2O_USE_OPTIMIZATION_LIBRARY(csparse)
G2O_USE_OPTIMIZATION_LIBRARY(dense)
G2O_USE_OPTIMIZATION_LIBRARY(eigen)


g2o::OptimizationAlgorithmFactory *solver_factory = g2o::OptimizationAlgorithmFactory::instance();
// 记录所有可用的 solver
std::ofstream fileout("/home/user/test_nodes/solvers.txt");
solver_factory->listSolvers(fileout);
// or lm_var_cparse
g2o::OptimizationAlgorithmProperty solver_property;
g2o::OptimizationAlgorithm *solver = solver_factory->construct("lm_var", solver_property);
std::unique_ptr<g2o::SparseOptimizer> optimizer(new g2o::SparseOptimizer());
optimizer->setAlgorithm(solver);

construct函数比上面的LinearSolverDense系列更灵活，设置求解器更精准。

使用listSolvers输出所有可用solver到文件，这个结果是和使用了多少G2O_USE_OPTIMIZATION_LIBRARY有关的，上面的5个应该涵盖了所有求解器，结果非常多。比如不加载G2O_USE_OPTIMIZATION_LIBRARY(eigen)，那么lm_var不可用，会报错 SOLVER FACTORY WARNING: Unable to create solver lm_var

listSolvers会输出所有求解器名称和解释，比如

1
lm_var    Eigen 	Levenberg: Cholesky solver using Eigen's Sparse Cholesky methods (variable blocksize)

优化结果

optimizer.setVerbose(true)后才能看到优化过程

或者g2o::SparseOptimizer::chi2()，优化前后各调用一次，可得到优化前后误差变化

1
2
3
4
5
iteration= 0	 chi2= 6973711.783675	 time= 0.228985	 cumTime= 0.228985	 edges= 3995	 schur= 0	 lambda= 42.450053	 levenbergIter= 1
iteration= 1	 chi2= 477444.019837	 time= 0.219176	 cumTime= 0.448161	 edges= 3995	 schur= 0	 lambda= 14.150018	 levenbergIter= 1
								......
iteration= 20	 chi2= 10344.665262	 time= 0.200148	 cumTime= 4.63974	 edges= 3995	 schur= 0	 lambda= 1.634041	 levenbergIter= 1
iteration= 21	 chi2= 10344.665262	 time= 0.461369	 cumTime= 5.10111	 edges= 3995	 schur= 0	 lambda= 112290463347.367386	 levenbergIter= 8

time是每次的优化时间，cumTime是累计用时。可以看到chi2最后几次迭代不怎么变化了，说明iteration设置大了