浅入G2O

G2O

1.1 整体介绍

如程序中所示，编写一个图优化的程序需要从底层到顶层逐渐搭建，参照g2o官方框架图（上方），步骤可以分为6步：

1. 创建一个线性求解器LinearSolver。
2. 创建块求解器BlockSolver，并用上面定义的线性求解器初始化。
3. 创建总求解器solver，并从GN/LM/DogLeg中选一个作为迭代策略，再用上述块求解器BlockSolver初始化。
4. 创建图优化的核心：稀疏优化器（SparseOptimizer），同时setAlgorithm(solver)设置算法
5. 定义图的顶点和边，并添加到SparseOptimizer中。
6. 设置优化参数，开始执行优化。

添加相机参数

// parameter: camera intrinsics
    g2o::CameraParameters* camera = new g2o::CameraParameters (
        K.at<double> ( 0,0 ), Eigen::Vector2d ( K.at<double> ( 0,2 ), K.at<double> ( 1,2 ) ), 0
    );
    camera->setId ( 0 );
    optimizer.addParameter ( camera );

1.1.1 分步简介

1.1.1.1 前四步

前四步基本上是常规套路了，有点不一样的就是
（1）BlockSolverTraits<a,b>的维度
第一个维度代表：每个误差项优化变量维度
第二个维度代表：误差值维度

或者是 <a,b>代表你定义的边所对应的两个顶点的维度 来理解:
block里面只包含顶点维度
【参考文档】g2o中定义g2o::BlockSolverTraits<p,l>的时候，p和l要如何设置？

下面给两个例子便于理解：

• BlockSolverTraits<6,3>对世界坐标系中的点进行优化
  pose：R,t共6个维度
  landmark：（x,y,z）共3个维度

• BlockSolverTraits<6,2> 优化pnp
  pose：R,t共6个维度
  landmark：（x,y）共2个维度

（2）总求解器solver的选取（GN, LM, DogLeg 中选一个）

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 );   // 设置求解器

【代码】：来自《14讲》但是我是copySLAM从0到1——图优化g2o：从看懂代码到动手编写（长文）这一篇的的

1.1.1.2 定义图的顶点

图的顶点比较常用的有VertexSE3Expmap 李代数位姿

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

一般来说，必须实现的操作包括
（1）setEstimate 设置待优化位姿
（2）setId 设置Id号
（3）setFixed设置是否固定

//【1】 设置待优化位姿（这是粗略位姿）
vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));
//【2】 设置Id号
    vSE3->setId(0);//int类型
//【3】 设置是否固定
    vSE3->setFixed(false);

（4）同时需要通过optimizer.addVertex将定义好的定点放入图模型中

optimizer.addVertex(vSE3)

1.1.1.3 定义图的边

1.2 边

1.2.1 边的基础概念

【参考博客】SLAM——入门到放弃：g2o定义边

分为单元边，双元边和多元边

单元边：一条边只连接一个顶点

class EdgeSE3ProjectXYZOnlyPose: public g2o:: BaseUnaryEdge<2,vector2d, VertexSE3Expmap>## 标题

双元边：两元边理解为一条边连接两个顶点，下面以EdgeProjectXYZ2UV威力

class EdgeProjectXYZ2UV : public BaseBinaryEdge <2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>

除了输入参数外，定义边我们通常需要复写一些重要的成员函数，顶点里主要复写了顶点更新函数oplusImpl和顶点重置函数setToOriginImpl

1.2.2 常用的节点和边

1.2.2.1 VertexSE3Expmap 李代数位姿