G2O库入门

图优化本质上仍然是非线性优化。只不过利用图的方式表现出来，使问题可视化，然后可以根据可视化的结果来更好的调整优化过程。

图 是一种数据结构。在图优化中，用顶点 (vertex) 表示优化变量，用边 (edge) 表示误差项。于是，对于任意一个上述形式的非线性最小二乘问题，都可以构建一个与之对应的图。

G2O库的使用主要可以分为以下几个步骤：

1. 定义顶点
2. 定义边
3. 配置BlockSolver(LinerSolverType)
4. 配置OptimizationAlgorithm
5. 配置Optimizer
6. 添加顶点
7. 添加边
8. 启动优化

这个是g2o自带文档给的一个图。可以看到一个图问题，由图的顶点和边组成，先看顶点（Vertex），主要来自继承BaseVertex<D,T>这个类，而边（Edge）有三种边，单元边（BaseUnaryEdge<D,E,VertexXi>），二元变（BaseBinaryEdge<D,E,VertexXi,VertexXj>），多元变（BaseMultiEdge<D,E>），元可以理解为顶点。

然后开始配置求解器，类似之前的ceres库。线性求解器LinearSolver有三种求解方法（LinearSolverPCG<>,LinearSolverCSparse<>,LinearSolverCholmod<>）,迭代方法有Gauss-Newton,Levenberg-Marquart,Pollwell's dogleg三种。

 

 

 

 

最最让人无奈的可能就是顶点、边的定义和添加。

先从简单的顶点定义开始

定点定义

下面的代码来自slam十四讲的ch6的代码。不过我尽可能模板化。

顶点类的定义

// 曲线模型的顶点，模板参数：优化变量维度和数据类型
class Name_of_Vertex: public g2o::BaseVertex< 3, Eigen::Vector3d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    virtual void setToOriginImpl() // 重置
    {
        _estimate << 0,0,0;
    }
    
    virtual void oplusImpl( const double* update ) // 更新
    {
        _estimate += Eigen::Vector3d(update);
        //Xk+1=Xk+△X
    }
    // 存盘和读盘：留空
    virtual bool read( istream& i