g2o优化

g2o优化

1  概述

2  g2o使用细节

3  实现及效果

4  参考资料

---

接上一篇RANSAC

1 概述

       就像g2o文献[1]中说的一样，它的目的是提供一个能够容易应用到各种问题的一个简单、易用的图优化库。它使用图的形式来描述和执行非线性最小二乘问题的优化。所以整个框架大概分为两部分：一部分是用图来描述，另一部分是优化。图一描述了这个结构。

                                                                         图一 g2o框架

       由上图可以看出整个优化问题可以表述成是一个稀疏优化器（SpareOptimizer）。它由图和优化方法组成。

       图由顶点和边组成。具体到一个优化问题时，顶点表示待优化变量，它继承自Base Vertex，在继承时我们需要给定它的变量维度D和变量类型T;边表示当前模型和观测值之间的误差，它分为一元边、二元边和超边，分别连接一个变量、两个变量和多个变量，在继承时需要提供误差的维度D、类型E，如果不是超边的话还需要提供它所连接的顶点类型。

       优化方法可以分为下降策略和线性方程组求解器。其中下降策略可以选择高斯牛顿、LM和dogleg。这三种方法对变量（顶点）的更新策略不同，有时适用的条件也不同。而Solver主要是求解增量方程，其中SparseBlockMatrix主要是构建增量方程，LinearSolver是数值求解增量方程使用的方法。

       对于一个非线性最小二乘问题（线性最小二乘问题比较简单，直接对误差函数求导，令导函数为零即可求解），由于误差函数求导并另导数为零后转化为一个非线性方程组，不好求解，所以要使用迭代的方法来解决，这时候就可以使用g2o了。

2  g2o使用细节

我们还是按照上面的结构分两个部分来介绍如何使用g2o。

构建图

       要构建图我们首先要根据自己的问题来确定顶点和边。

       在上一篇文章中我们使用RANSAC估计出了一个直线模型，这里我们在它模型基础上进行优化。这个问题现在变成如何调整模型参数使得所有内点到新模型的距离和最短。

       设直线方程为,内点为,那么问题可以描述成。这里我们要优化的是直线模型，它由两个参数组成a和c，所以顶点的维度为2,类型是double型的二维向量。对于每一个内点来说，误差是它到直线的距离，所以边的维度为1,类型为double，又因为它只和直线模型有关，所以是一元边，需要给出顶点的类型即double类型的二维向量。

在继承的过程中我们还要重写纯虚函数（具体的可以看后面的实现）

在顶点的定义里面要重写：

virtual void setToOriginImpl() //重置估计值
virtual void oplusImpl( const double* update )//更新估计值
virtual bool read( std::istream& in )//读函数
virtual bool write( std::ostream& out )//写函数

在边的定义里要重写：

void computeError()//计算误差
virtual  void linearizeOplus()//计算雅可比矩阵（可选）
virtual bool read( std::istream& in )//读函数
virtual bool write( std::ostream& out )//写函数

这些定义完成后构建图所需要的顶点和边就准备好了，下面是优化算法。

优化算法

       对于优化算法，由图一可以看出它是有包含关系的，所以在构建它的时候可以按照结构，从后往前逐次构建。

using namespace g2o;
//估计这两个参数都是优化变量维度