G2O非线性优化

上一篇文章介绍最小二乘法，本文介绍g2o实现最小二乘法。
g2o即General Graph Optimization，它是一个基于图优化的库。至于图优化理论，参照半闲居士的博客
接着上回曲线拟合问题，拟合 y=Asin(Bx)+Ccos(Dx) ，已知N组数据 (xi,yi),i=0,1,⋯N−1 ，待优化变量 V=[A,B,C,D] ，优化问题即为：

V=argmin(∑i=0N−1(y−Asin(BX)−Ccos(Dx))2)

整个图中只有一个顶点，即待优化变量V，连接只有一个顶点的边即一元边（Unary Edge），使用g2o主要有以下几个步骤：
1. 定义顶点和边的类型；
2. 选择优化算法；
3. 构建图；
4. 调用 g2o 进行优化，返回结果。

由于g2o中没有本例类型顶点和边，因此首先需要自己定义。节点和边定义的方式，都是继承BaseVertex和BaseEdge，可以参考优化相机位姿和3D点坐标常用几个类型VertexSE3Expmap、VertexSBAPointXYZ、EdgeSE3ProjectXYZ、EdgeSE3ProjectXYZOnlyPose定义代码，熟悉这种这种模板定义的方式。

// 曲线模型的顶点(要优化的元素)
// vertex dimension: 4
// vertex type: Vector4d
class CurveFittingVertex: public g2o::BaseVertex<4, Vector4d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    CurveFittingVertex();

    virtual void setToOriginImpl()  // 重置
    {
        _estimate << 0, 0, 0, 0;
    }

    virtual void oplusImpl(const double* update_);// 更新

    //输入输出可以不定义
    bool read(std::istream& is) {}
    bool write(std::ostream& os) const {}
};

// 顶点构造函数
CurveFittingVertex::CurveFittingVertex() : BaseVertex<4, Eigen::Vector4d>() 
{    
}

// 顶点更新函数
void CurveFittingVertex::oplusImpl(const double* update_)   
{
    Eigen::Map<const Vector4d> up(update_);
    _estimate += up;
}

顶点的定义主要需要定义_estimate的更新函数oplusImpl和setToOriginImpl。

---

// 曲线模型的边(要优化的误差)
// unary edge to optimize error
// error vector dimensition: 1
// measurement type: double
// Vertex type: CurveFittingVertex
class CurveFittingEdge: public g2o::BaseUnaryEdge<1, double, CurveFittingVertex>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    CurveFittingEdge();

    // 计算曲线模型误差
    void computeError();

    virtual void linearizeOplus();

    bool read(std::istream& is) {}
    bool write(std::ostream& os) const {}

public:
    double _x;
};

// 边的构造函数
CurveFittingEdge::CurveFittingEdge() : g2o::BaseUnaryEdge<1, double, CurveFittingVertex>()
{

}
// 误差函数
void CurveFittingEdge::computeError()
{
    const CurveFittingVertex* v = static_cast<const CurveFittingVertex*>( _vertices[0]);
    const Vector4d abcd = v->estimate();
    double A = abcd[0], B = abcd[1], C = abcd[2], D = abcd[3];
    _error(0,0) = _measurement - (A * sin(B*_x) + C * cos(D*_x));  // 误差函数：观测量减去估计量
}

// Jacobin
void CurveFittingEdge::linearizeOplus()
{
    CurveFittingVertex *vi = static_cast(_vertices[0]);
    Vector4d abcd = vi->estimate();
    double A = abcd[0], B = abcd[1], C = abcd[2], D = abcd[3];
    // 误差项对待优化变量的Jacobin
    _jacobianOplusXi(0,0) = -sin(B*_x);
    _jacobianOplusXi(0,1) = -A*_x*cos(B*_x);
    _jacobianOplusXi(0,2) = -cos(D*_x);
    _jacobianOplusXi(0,3) = C*_x*sin(D*_x);
}

误差函数 e=y−Asin(Bx)−Ccos(Dx) ,因此

∂e∂[ABCD]=[−sin(Bx)  −Axcos(Bx)  −cos(Dx)  Cxsin(Dx)]

边的定义主要需要定义误差函数和计算Jacobin。
当然边的Jacobin函数可以不定义，g2o会采用如上篇博客的数值求导方法。如果我们能够推导出雅可比矩阵的解析形式并告诉优化库，就可以避免数值求导中的诸多问题。

---

然后是图的构造和求解了

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

#include 
#include 

#include 
#include 

#include 

using namespace std;
using namespace Eigen;
int main()
{
    double a = 5.0, b = 1.0, c = 10.0, d = 2.0; // 真实参数值
    int N = 100;
    double w_sigma = 2.0;   // 噪声值Sigma
    cv::RNG rng;    // 随机数产生器OpenCV
    double abcd[4] = {0, 0, 0, 0};  // 参数的估计值abc

    vector<double> x_data, y_data;

    cout << "generate random data" << endl;

    for(int i = 0; i < N; i++)
    {
        //generate a random variable [-10 10]
        double x = rng.uniform(-10., 10.);
        double y = a * sin(b*x) + c * cos(d *x) + rng.gaussian(w_sigma);
        x_data.push_back(x);
        y_data.push_back(y);

        cout << x_data[i] << " , " << y_data[i] << endl;
    }

    // 构建图优化，先设定g2o
    // 矩阵块：每个误差项优化变量维度为4 ，误差值维度为1
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<4, 1> > Block;
    // 线性方程求解器：稠密的增量方程
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense();

    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::Vector4d(1.6, 1.4, 6.2, 1.7));
    v->setId(0);
    v->setFixed(false);
    optimizer.addVertex(v);

    // 往图中增加边
    for(int i = 0; i < N; i++)
    {
        CurveFittingEdge* edge = new CurveFittingEdge();
        edge->setId(i+1);
        edge->setVertex(0, v);      // 设置连接的顶点
        edge->setMeasurement( y_data[i] );      // 观测数值

        // 信息矩阵：协方差矩阵之逆
        edge->setInformation( Eigen::Matrix<double, 1, 1>::Identity() * 1 /(w_sigma* w_sigma) );

        edge->_x = x_data[i];

        optimizer.addEdge( edge );
    }

    // 执行优化
    cout << "strat 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_castdouble> > (t2 - t1);
    cout << "solve time cost = " << time_used.count() << " seconds." << endl;

    // 输出优化值
    Eigen::Vector4d abcd_estimate = v->estimate();
    cout << "estimated module: " <<  endl << abcd_estimate << endl;

    return 0;
}

---

另外g2o中设置核函数、边缘化等等后续再更新。

引用:
[1] 《g2o: A General Framework for Graph Optimization》
[2] 《slambook by gaoxiang》