视觉SLAM | g2o

g2o： General Graphic Optimization，是基于图优化的库
图优化（graph optimization),是一种将非线性优化和图论结合起来的理论。图优化里的图就是数据结构里的图，一个图由若干个顶点（vertex），以及连接这些顶点的边（edge）组成。
比如一个机器人在房屋里移动，它在某个时刻 t 的位姿（pose）就是一个顶点，这个也是待优化的变量。而位姿之间的关系就构成了一个边，比如时刻 t 和时刻 t+1 之间的相对位姿变换矩阵就是边，边通常表示误差项。
在SLAM里，图优化一般分解为两个任务：
1、构建图。机器人位姿作为顶点，位姿间关系作为边。
2、优化图。调整机器人的位姿（顶点）来尽量满足边的约束，使得误差最小。

基本框架

图1. g2o的基本框架

算法流程

高博在十四讲中g2o求解曲线参数的例子来说明

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

// 第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步：创建终极大boss 稀疏优化器（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);

参考文章

[1] :从零开始一起学习SLAM | 理解图优化，一步步带你看懂g2o代码
[2] : g2o学习笔记
[3] : graph slam tutorial : 从推导到应用1
[4] : 深入理解图优化与g2o：g2o篇

参考论文

[1] : 《SBA：A software package for generic sparse bundle adjustment》