g2o学习笔记

g2o整体框架

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

1. 选择一个线性方程求解器，从PCG、CSparse、Choldmod三个选一
2. 选择一个BlockSolver
3. 选择一个迭代器，从GN、LM、Doglog中选

自定义顶点（Vertex）和边（Edge）

• 在整体框架图中，可以看到不管是顶点还是变，都可以说是继承子basexxx这个类的，因此我们在自定义的时候，也可以仿照着继承这两个类，当然也可以继承g20中较为“成熟”类，不管怎样，都要重写下述的四个函数

• 自定义顶点函数：

    virtual bool read(std::istream& is);  
    virtual bool write(std::ostream& os) const;  
    virtual void oplusImpl(const number_t* update);
    virtual void setToOriginImpl();  
  

其中read和write函数可以不进行覆写，仅仅声明一下就可以了，setToOriginImpl设定为优化变量的初始值，oplusImpl比较重要，我们根据增量方程计算出增量后，就通过这个函数对估计值进行调整。

• 自定义边：

    virtual bool read(std::istream& is);  
    virtual bool write(std::ostream& os) const;  
    virtual void computeError();  
    virtual void linearizeOplus();  
  

其中read和write函数同上，computeError函数是使用当前顶点的值计算的测量值与真实值之间的误差，linearizeOplus函数是在当前顶点的值下，该误差对优化变量的偏导数，即jacobian矩阵

- 注意：如果没有定义边的linearizeOplus函数，就会调用数值求导，运算比较慢

优化算法

• 主要包括三个部分，线性求解器、块求解器、优化的算法。在程序中，这部分通常位于g2o算法的开头配置部分

• 在求增量方程时，需要对H矩阵求逆，但当H的维度很大时，我们就需要一些特殊的方法对矩阵进行求逆，g2o中对稠密矩阵有Dense方法，对稀疏矩阵有PCG、CSparse、Cholmod方法

• 块求解器是包含线性求解器的存在，它会构建好线性求解器所需要的矩阵块（即H和b），之后给线性求解器让它进行运算，边的jacobian也在此时使用上

• 优化算法，主要表现在最终的H矩阵构造不同，主要有三种方法：GN、LM、PSD

SLAM使用g2o过程

• 先定义你要用到的节点、边的类型

• 定义一个SparseOptimizer，设置好优化算法

• 将优化变量设为节点、误差设为边，加入到optimizer中，每条边还需要设定一个信息矩阵作为权重的度量

• 初始化SparseOptimizer，设置好迭代次数，就可以进行迭代优化，最终输出节点的预测值

注：信息矩阵等于协方差矩阵的逆，可以理解为权重，协方差越大，说明不确定度越大，那么在优化的时候就要给予较小的权重，不至于误差较大的顶点或者边带偏了整个优化