使用g2o进行图优化

使用g2o进行图优化,第1张

G2O(General Graphic Optimization)

是一个用来优化非线性误差函数的c++开源库.

安装过程参考官网即可.

1 g2o框架

 由以下5个部分构成:

(1)SparseOptimizer是整个图的核心,找到它向上走,它是一个Optimizable Graph 也就是一个超图HyperGraph,所以在使用时需要添加顶点和边.
(2)顶点(HyperGraph::Vertex)和边(HyperGraph::Edge),顶点继承自 Base Vertex,边可以继承自BaseUnaryEdge(单边), BaseBinaryEdge(双边)或BaseMultiEdge(多边).
(3)整个框架的上部分就说的顶点和边(图的结构),下部分就是优化算法.SparseOptimizer包含一个优化算法OptimizationAlgorithm的对象,通过OptimizationWithHessian 来实现的.迭代策略可以从Gauss-Newton(高斯牛顿法,简称GN), Levernberg-Marquardt(简称LM法), Powell’s dogleg 三者中间选择一个.
(4)OptimizationWithHessian 内部包含一个求解器Solver,Solver实际是由一个BlockSolver组成的.
(5)BlockSolver有两个部分,一个是SparseBlockMatrix ,用于计算稀疏的雅可比和Hessian矩阵;一个是线性方程的求解器LinearSolver,它用于计算迭代过程中最关键的一步HΔx=−b,LinearSolver有几种方法可以选择:PCG, CSparse, Choldmod.

2 g2o优化流程

由于定义时需要初始化,所以应该从底向上初始化.

(1)创建一个线性求解器LinearSolver
(2)创建BlockSolver。


并用上面定义的线性求解器初始化
(3)创建总求解器solver。


并从GN, LM, DogLeg 中选一个,再用上述块求解器BlockSolver初始化
(4)创建SparseOptimizer 稀疏优化器
(5)定义图的顶点和边。


并添加到SparseOptimizer中
(6)设置优化参数,开始执行优化

3 二维数据优化例子
    typedef g2o::BlockSolver> Block; // 每个误差项优化变量维度为3,误差值维度为3

    /*创建线性求解器,solvers文件夹中*/
    // Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense();
    // Block::LinearSolverType* linearSolver = new g2o::LinearSolverEigen();
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse();

    /*创建BlockSolver,在/core/block_solver.h中,可定义数据维度*/
    Block* solver_ptr = new Block(linearSolver);

    /*创建总求解器solver。


并从GN, LM, DogLeg 中选一个*/ // g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr ); // g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( solver_ptr ); /*创建SparseOptimizer 稀疏优化器, 设置求解方法*/ g2o::SparseOptimizer optimizer; optimizer.setAlgorithm(solver); /*添加顶点SE2*/ for (size_t i = 0; i < Vertexs.size(); i++) { VertexSE2* v = new VertexSE2(); v->setEstimate(Vertexs[i]); v->setId(i); if (i == 0) { v->setFixed(true); } optimizer.addVertex(v); } /*添加边*/ for (size_t i = 0; i < Edges.size(); i++) { EdgeSE2* edge = new EdgeSE2(); Edge tmpEdge = Edges[i]; edge->setId(i); edge->setVertex(0, optimizer.vertices()[tmpEdge.xi]); edge->setVertex(1, optimizer.vertices()[tmpEdge.xj]); edge->setMeasurement(tmpEdge.measurement); edge->setInformation(tmpEdge.infoMatrix); optimizer.addEdge(edge); } /*设置优化参数 并求解*/ optimizer.setVerbose(true); optimizer.initializeOptimization(); SparseOptimizerTerminateAction* terminateAction = new SparseOptimizerTerminateAction; terminateAction->setGainThreshold(1e-4); optimizer.addPostIterationAction(terminateAction); optimizer.optimize(100); /*取出求解结果*/ for (size_t i = 0; i < Vertexs.size(); i++) { VertexSE2* v = static_cast(optimizer.vertices()[i]); Vertexs[i] = v->estimate().toVector(); }

欢迎分享,转载请注明来源:内存溢出

原文地址: http://outofmemory.cn/langs/634474.html

(0)
打赏 微信扫一扫 微信扫一扫 支付宝扫一扫 支付宝扫一扫
上一篇 2022-04-16
下一篇 2022-04-16

发表评论

登录后才能评论

评论列表(0条)

保存