g2o的原理

g2o图优化讲解

g2o(General Graphic Optimization)是一个通用图优化算法库。由于目前主流的SLAM研究基本都是基于优化的，因此十分有必要掌握g2o方法。

特别需要记住的是：

• 图优化中的点是相机位姿，也就是优化变量（状态变量）
• 图优化中的边是指位姿之间的变换关系，通常表示误差项

下图为官方文档中经典的g2o框架：[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传

对这个结构框图做一个简单介绍（注意图中三种箭头的含义（右上角注解））：

（1）整个g2o框架可以分为上下两部分，两部分中间的连接点：SparseOpyimizer 就是整个g2o的核心部分。

（2）往上看，SparseOpyimizer其实是一个Optimizable Graph，从而也是一个超图（HyperGraph）。

（3）超图有很多顶点和边。顶点继承自 Base Vertex，也即OptimizableGraph::Vertex；而边可以继承自 BaseUnaryEdge（单边）, BaseBinaryEdge（双边）或BaseMultiEdge（多边），它们都叫做OptimizableGraph::Edge。

（4）往下看，SparseOptimizer包含一个优化算法部分OptimizationAlgorithm，它是通过OptimizationWithHessian 来实现的。其中迭代策略可以从Gauss-Newton（高斯牛顿法，简称GN）、 Levernberg-Marquardt（简称LM法），Powell’s dogleg 三者中间选择一个（常用的是GN和LM）。

（5）对优化算法部分进行求解的是求解器solver，它实际由BlockSolver组成。BlockSolver由两部分组成：一个是SparseBlockMatrix，它由于求解稀疏矩阵(雅克比和海塞)；另一个部分是LinearSolver，它用来求解线性方程得到待求增量，因此这一部分是非常重要的，它可以从PCG/CSparse/Choldmod选择求解方法。

我们再来看一下框架的搭建步骤，以高博在SLAM十四讲中使用g2o求解曲线参数为例（注意注释）:

typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;  // 每个误差项优化变量维度为3，误差值维度为1


/*************** 第1步：创建一个线性求解器LinearSolver*************************/
Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); 


/*************** 第2步：创建BlockSolver。并用上面定义的线性求解器初始化**********/
Block* solver_ptr = new Block( linearSolver );      


/*************** 第3步：创建总求解器solver。并从GN, LM, DogLeg 中选一个，再用上述块求解器BlockSolver初始化****/
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );


/*************** 第4步：创建图优化的核心：稀疏优化器（SparseOptimizer）**********/
g2o::SparseOptimizer optimizer;     // 图模型

optimizer.setAlgorithm( solver );   // 设置求解器

optimizer.setVerbose( true );       // 打开调试输出


/*************** 第5步：定义图的顶点和边。并添加到SparseOptimizer中**********/
CurveFittingVertex* v = new CurveFittingVertex(); //往图中增加顶点
v->setEstimate( Eigen::Vector3d(0,0,0) );
v->setId(0);
optimizer.addVertex( v );
for ( int i=0; i<N; i++ )    // 往图中增加边
{
  CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );
  edge->setId(i);
  edge->setVertex( 0, v );                // 设置连接的顶点
  edge->setMeasurement( y_data[i] );      // 观测数值
  edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); // 信息矩阵：协方差矩阵之逆
  optimizer.addEdge( edge );
}


/*************** 第6步：设置优化参数，开始执行优化**********/
optimizer.initializeOptimization();
optimizer.optimize(100);    //设置迭代次数

如程序中所示，编写一个图