图优化与g2o通用图优化库入门讲解

图优化与g2o通用图优化库入门

图优化是指把一个常规的优化问题，以图（Graph）的形式来表述，图是由顶点（Vertex）和边（Edge）组成的结构，边也可以连接一个顶点（Unary Edge，一元边）、两个顶点（Binary Edge，二元边）或多个顶点（Hyper Edge，多元边）。当一个图中存在连接两个以上顶点的边时，称这个图为超图（Hyper Graph）。而SLAM问题就可以表示成一个超图，

图优化求解，其实是求解线性方程𝐻Δ𝑥=−𝑏，主要是求解一阶导雅可比矩阵J和二阶导海塞矩阵H,其流程如下:

    1.选择你想要的图里的节点与边的类型，确定它们的参数化形式；

    2.往图里加入实际的节点和边；

    3.选择初值，开始迭代；

    4.每一步迭代中，计算对应于当前估计值的雅可比矩阵和海塞矩阵；

    5.求解稀疏线性方程𝐻Δ𝑥=−𝑏，得到梯度方向；

    6.继续用GN或LM进行迭代。如果迭代结束，返回优化值。

 

实际上，g2o能帮你做好第3-6步，你要做的只是前两步而已。下节我们就来尝试这件事

 

g2O 叫通用图优化库，它带有各种各样的求解器，顶点和边的类型也多种多样，只要一个优化问题可以用图表示都可以用g2o 去求解它，常见的比如bundle adjustment(捆集调整)，ICP，数据拟合等

 

g2o最基本的类结构如下：

先看上半部分。SparseOptimizer 是我们最终要维护的东东。它是一个Optimizable Graph，从而也是一个Hyper Graph。一个 SparseOptimizer 含有很多个顶点 （都继承自 Base Vertex）和很多个边（继承自 BaseUnaryEdge, BaseBinaryEdge或BaseMultiEdge）。这些 Base Vertex 和 Base Edge 都是抽象的基类，而实际用的顶点和边，都是它们的派生类。我们用 SparseOptimizer.addVertex 和 SparseOptimizer.addEdge 向一个图中添加顶点和边，最后调用 SparseOptimizer.optimize 完成优化。

　　在优化之前，需要指定我们用的求解器和迭代算法。从图中下半部分可以看到，一个 SparseOptimizer 拥有一个 Optimization Algorithm，继承自Gauss-Newton, Levernberg-Marquardt, Powell's dogleg 三者之一（我们常用的是GN或LM）。同时，这个 Optimization Algorithm 拥有一个Solver，它含有两个部分。一个是 SparseBlockMatrix ，用于计算稀疏的雅可比和海塞； 一个是用于计算迭代过程中最关键的一步

𝐻Δ𝑥=−𝑏

这就需要一个线性方程的求解器。而这个求解器，可以从 PCG, CSparse, Choldmod 三者选一。

　　综上所述，在g2o中选择优化方法一共需要三个步骤：

选择一个线性方程求解器，从 PCG, CSparse, Choldmod中选，实际则来自 g2o/solvers 文件夹中定义的东东。

选择一个 BlockSolver 。

选择一个迭代策略，从GN, LM, Doglog中选

使用例子如下bundle adjustment:

// 构造g2o中的图     

// 先构造求解器    

g2o::SparseOptimizer    optimizer; 

 // 使用Cholmod中的线性方程求解器    

g2o::BlockSolver_6_3::LinearSolverType* linearSolver = new   g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType> (); 

// 6*3 的参数 

 g2o::BlockSolver_6_3* block_solver = new g2o::BlockSolver_6_3( linearSolver ); 

// L-M 下降  

g2o::OptimizationAlgorithmLevenberg* algorithm = new  g2o::OptimizationAlgorithmLevenberg( block_solver );    optimizer.setAlgorithm( algorithm ); 

optimizer.setVerbose( false );

// 添加节点,两个位姿节点(代码只抽取一小部分，做说明)

g2o::VertexSE3Expmap* v = new g2o::VertexSE3Expmap();

optimizer.addVertex( v );

// 添加特征点的节点

g2o::VertexSBAPointXYZ* v = new g2o::VertexSBAPointXYZ();

optimizer.addVertex( v );

//添加多条边

vector<g2o::EdgeProjectXYZ2UV*> edges;

g2o::EdgeProjectXYZ2UV*  edge = new g2o::EdgeProjectXYZ2UV();

edge->setVertex( 0, dynamic_cast<g2o::VertexSBAPointXYZ*>   (optimizer.vertex(i+2)) );

edge->setVertex( 1, dynamic_cast<g2o::VertexSE3Expmap*>     (optimizer.vertex(0)) );

edge->setMeasurement( Eigen::Vector2d(pts1[i].x, pts1[i].y ) );

edge->setInformation( Eigen::Matrix2d::Identity() );

edge->setParameterId(0, 0);

edge->setRobustKernel( new g2o::RobustKernelHuber() );

optimizer.addEdge( edge );

edges.push_back(edge);


cout<<"开始优化"<<endl;

optimizer.setVerbose(true);

optimizer.initializeOptimization();

optimizer.optimize(10);

cout<<"优化完毕"<<endl;