g2o曲线拟合

先声明，用g2o来做曲线拟合，有点大材小用。g2o本身是一个通用图优化框架，可以解决本质上为非线性优化的图优化问题。

本文先介绍图优化的概念，再介绍如何将曲线拟合转化为图优化问题，最后介绍如何使用g2o解决这个问题。

一、什么是图优化？

图优化是SLAM领域针对非线性优化的一种特殊解法。这里假设我们对一般的优化问题有一定了解，比如，知道优化问题的三个要素：目标函数、优化变量和优化约束。简单说来，优化问题就是在给定约束条件下，求使目标函数最小的优化变量的值。

对应本文的曲线拟合问题，目标函数就是真实点与拟合曲线上的点的误差，优化变量就是拟合曲线的参数，约束暂时没有。如果阅读过本文集的上一篇文章《Ceres曲线拟合》，应该可以体会到Ceres是如何解决这个非线性优化问题的。而图优化则提供了一种更为新颖的解决方案。

图优化将优化变量表示为顶点，将目标函数（或称为误差项）表示为边，于是，针对每一个非线性优化问题，都可以构造一个图。这种表示方法与一般的非线性优化方法（比如最小二乘）相比，可以更细粒度地控制优化变量之间的关联，只有相关的顶点间才会建立边的连接，从而降低整个优化问题的复杂度。具体的形式下文将详细说明。

二、曲线拟合的图优化表示

对于曲线拟合问题来说，只有一组优化变量a、b和c，因此它们可以作为一个顶点，而目标函数或者说误差项，就是实际的y值与通过abc计算出来的y值之差，每一个误差项可以对abc进行一次修正，因此边的起点和终点都是同一个顶点abc。画出来是这个样子：

接下来，就可以使用g2o求解这个图优化问题了。

三、g2o曲线拟合

代码下载地址：https://github.com/jingedawang/G2OCurveFitting

按照惯例，代码量不大而且思路清晰的情况下就直接贴出来好了。

#include <iostream>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <cmath>
#include <chrono>
using namespace std;

// 曲线模型的顶点，模板参数：优化变量维度和数据类型
class CurveFittingVertex: 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);
    }
    // 存盘和读盘：留空
    virtual bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}
};

// 误差模型 模板参数：观测值维度，类型，连接顶点类型
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}
    // 计算曲线模型误差
    void computeError()
    {
        const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]);
        const Eigen::Vector3d abc = v->estimate();
        _error(0,0) = _measurement - std::exp( abc(0,0)*_x*_x + abc(1,0)*_x + abc(2,0) ) ;
    }
    virtual bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}
public:
    double _x;  // x 值， y 值为 _measurement
};

int main( int argc, char** argv )
{
    double a=1.0, b=2.0, c=1.0;         // 真实参数值
    int N=100;                          // 数据点
    double w_sigma=1.0;                 // 噪声Sigma值
    cv::RNG rng;                        // OpenCV随机数产生器
    double abc[3] = {0,0,0};            // abc参数的估计值

    vector<double> x_data, y_data;      // 数据

    cout<<"generating data: "<<endl;
    for ( int i=0; i<N; i++ )
    {
        double x = i/100.0;
        x_data.push_back ( x );
        y_data.push_back (
                exp ( a*x*x + b*x + c ) + rng.gaussian ( w_sigma )
        );
        cout<<x_data[i]<<" "<<y_data[i]<<endl;
    }

    // 构建图优化，先设定g2o
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;  // 每个误差项优化变量维度为3，误差值维度为1
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 线性方程求解器
    Block* solver_ptr = new Block( linearSolver );      // 矩阵块求解器
    // 梯度下降方法，从GN, LM, DogLeg 中选
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
    // g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );
    // g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( solver_ptr );
    g2o::SparseOptimizer optimizer;     // 图模型
    optimizer.setAlgorithm( solver );   // 设置求解器
    optimizer.setVerbose( true );       // 打开调试输出

    // 往图中增加顶点
    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 );
    }

    // 执行优化
    cout<<"start optimization"<<endl;
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    optimizer.initializeOptimization();
    optimizer.optimize(100);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
    cout<<"solve time cost = "<<time_used.count()<<" seconds. "<<endl;

    // 输出优化值
    Eigen::Vector3d abc_estimate = v->estimate();
    cout<<"estimated model: "<<abc_estimate.transpose()<<endl;

    return 0;
}

可以看出，g2o是个更加模块化的框架，用户声明的顶点和边的类型都需要通过继承基础类来实现。代码中，顶点继承自BaseVertex模板类，需要重写oplusImpl方法以实现优化变量的更新操作，此处只需要简单的加法就可以了。边继承自BaseUnaryEdge模板类，代表这是一个一元边，即边的起点和终点重合，需要重写computeError方法以实现误差项的计算。除此之外，其它的设置操作本质上与Ceres并无区别，只不过g2o全部采用数值求导方法，而不采用自动求导。

该程序在0.0107522s内运行完毕，迭代18次。优化结果为：

estimated model: 0.890912 2.1719 0.943629

与Ceres的结果很接近，但用时长了不少，大概是因为Ceres是个轻量级框架，而g2o太过沉重，用来解决这类小问题反而得不偿失了。

四、参考资料

《视觉SLAM十四讲》第6讲 非线性优化 高翔
深入理解图优化与g2o：图优化篇 高翔
深入理解图优化与g2o：g2o篇 高翔