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.
由于定义时需要初始化,所以应该从底向上初始化.
(1)创建一个线性求解器LinearSolver
(2)创建BlockSolver。
并用上面定义的线性求解器初始化
(3)创建总求解器solver。
并从GN, LM, DogLeg 中选一个,再用上述块求解器BlockSolver初始化
(4)创建SparseOptimizer 稀疏优化器
(5)定义图的顶点和边。
并添加到SparseOptimizer中
(6)设置优化参数,开始执行优化
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();
}
欢迎分享,转载请注明来源:内存溢出
评论列表(0条)