SlamBook14讲-g2o

g2o是一种非线性优化方法，用于计算最小二乘问题，因为感觉很重要，决定回过头再总结一下，明天再继续弄pnp部分。

参考了几篇好文章：

g2o入门(一)曲线拟合_111111111112454545的博客-CSDN博客_g2o教程

图优化中点指的是要优化的部分，边指的是误差项。

直接附上源码，和书中源码差距不大：

#include <iostream>
#include <opencv2/core/core.hpp>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <eigen3/Eigen/Core>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
class g2oVertex : 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( std::istream& in ) {}
    virtual bool write( std::ostream& out ) const {}
};

class g2oEdge : public g2o::BaseUnaryEdge<1,double, g2oVertex>{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    g2oEdge(double x) : BaseUnaryEdge(), _x(x) {}
    void computeError()
    {
        const g2oVertex* v = static_cast<const g2oVertex*> (_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( std::istream& in ) {}
    virtual bool write( std::ostream& out ) const {}
private:
    double _x;
};

int main(){
    //生成样本
    double a = 2.0, b = 3.0, c = 4.0;
    int N = 100;
    double w = 0.5;
    cv::RNG rand;
    std::vector<double> XData,YData;
    for(int i = 0; i < N; i++){
        double x = i/100.0;
        XData.push_back(x);
        YData.push_back(std::exp(a*x*x + b*x + c)+rand.gaussian(w));
    }
    //初始化
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;  // 每个误差项优化变量维度为3，误差值维度为1
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();
    Block* SolverPtr = new Block(std::unique_ptr<Block::LinearSolverType>(linearSolver));
    g2o::OptimizationAlgorithmLevenberg* Solver = new g2o::OptimizationAlgorithmLevenberg(std::unique_ptr<g2o::Solver>(SolverPtr));
    g2o::SparseOptimizer Optimazer;
    Optimazer.setAlgorithm(Solver);
    Optimazer.setVerbose(1);
    //增加顶点
    g2oVertex* v = new g2oVertex();
    v->setEstimate(Eigen::Vector3d(0,0,0));
    v->setId(0);
    Optimazer.addVertex(v);

    //增加单边
    for(int i = 0; i < N; i++){
        g2oEdge* edge = new g2oEdge(XData[i]);
        edge->setId(i);
        edge->setVertex(0,v);
        edge->setMeasurement(YData[i]);
        edge->setInformation(Eigen::Matrix<double,1,1>::Identity()*1/(w*w));
        Optimazer.addEdge(edge);
    }
    //开始优化
    Optimazer.initializeOptimization();
    Optimazer.optimize(100);
    Eigen::Vector3d abc = v->estimate();
    std::cout << abc.transpose() << std::endl;

}


    

有几点需要说明一下

首先是g2o中的各种数据结构，有些复杂，放一张图：

 

图片来源：深入理解图优化与g2o：g2o篇 - 半闲居士 - 博客园

我在写的过程中遇到的问题是，直接用了Optimazer.addVertex源码中写的OptimazableGraph::Vertex作为点的数据结果（毕竟代码就是这么写的），结果发现这个类中的函数为纯虚函数（末尾有=0），例如：

这意味着这个类为抽象类，抽象类是不能被实例化的。这就是为什么必须自己写一个类，再继承BaseVertex或者BaseUnaryEdge的原因。在类中需要重写几个函数，但我不知道自己写的话如何知道要重写这几个函数，看了很久也没有大佬讲解，不过也能看懂，只能暂时理解到这了。

其中setInformation是设置信息矩阵，其为协方差矩阵的逆。协方差矩阵一般设为同维数的对角阵。

剩下的就是set的过程了，没有太多可说的。