《视觉SLAM十四讲精品总结》4：非线性优化g2o(BA基础)

一、g2o图优化（General Graph Optimization）

g2o最基本的类结构是怎么样的呢？我们如何来表达一个Graph，选择求解器呢？我们祭出一张图：

先看上半部分。SparseOptimizer 是我们最终要维护的东东。它是一个Optimizable Graph，从而也是一个Hyper Graph。一个 SparseOptimizer 含有很多个顶点 （都继承自 Base Vertex）和很多个边（继承自 BaseUnaryEdge, BaseBinaryEdge或BaseMultiEdge）。这些 Base Vertex 和 Base Edge 都是抽象的基类，而实际用的顶点和边，都是它们的派生类。我们用 SparseOptimizer.addVertex 和 SparseOptimizer.addEdge 向一个图中添加顶点和边，最后调用 SparseOptimizer.optimize 完成优化。

在优化之前，需要指定我们用的求解器和迭代算法。从图中下半部分可以看到，一个 SparseOptimizer 拥有一个 Optimization Algorithm，继承自Gauss-Newton, Levernberg-Marquardt, Powell's dogleg 三者之一（我们常用的是GN或LM）。同时，这个 Optimization Algorithm 拥有一个Solver，它含有两个部分。一个是 SparseBlockMatrix ，用于计算稀疏的雅可比和海塞； 一个是用于计算迭代过程中最关键的一步

HΔx=−b

这就需要一个线性方程的求解器。而这个求解器，可以从 PCG, CSparse, Choldmod 三者选一。

综上所述，在g2o中选择优化方法一共需要三个步骤：

    选择一个线性方程求解器，从 PCG, CSparse, Choldmod中选，实际则来自 g2o/solvers 文件夹中定义的东东。
    选择一个迭代策略，从GN, LM, Doglog中选。

 

总结一下做图优化的流程。

    选择你想要的图里的节点与边的类型，确定它们的参数化形式；
    往图里加入实际的节点和边；
    选择初值，开始迭代；
    每一步迭代中，计算对应于当前估计值的雅可比矩阵和海塞矩阵；
    求解稀疏线性方程HkΔx=−bk，得到梯度方向；
    继续用GN或LM进行迭代。如果迭代结束，返回优化值。

 
二、代码解析

g2o中派生出的图优化顶点和边两个派生类

顶点CurvFittingVertex

    //继承的g2o::BaseVertex基类   <优化变量维度,数据类型>
    class CurveFittingVertex :public g2o::BaseVertex < 3, Eigen::Vector3d >
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
            //1. 重置
        virtual void setToOriginImpl()
        {
            _estimare << 0, 0, 0;
        }
        //2. 更新X(k+1)=X(k)+x;
        virtual void oplusImpl(const double* update)
        {
            _estimate += Eigen::Vector3d(update);
        }
        //存盘和读盘：留空
        virtual bool read(istream& in){}
        virtual bool write(ostream& out){}
    };

派生类中的虚函数

1、顶点的更新oplusImpl

2、顶点的重置函数setToOriginImpl

3、边的误差计算函数computeError

边CurveFittingEdge

    //继承的g2o::BaseUnaryEdge基类 <观测值维度，类型，连接顶点类型>
    class CurveFittingEdge :public g2o::BaseUnaryEdge < 1, double, CurveFittingVertex >
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
     
        CurveFittingEdge(double x) :BaseUnaryEdge(), _x(x){}//构造函数
        double _x, _measurement;
        //1. 计算误差
        void computeError()
        {
                    //初始化顶点v
            const CurveFittingVertex* v = static_cast(_vertices[0]);
                    //abc是边所连接顶点的当前估计值
            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){}
     
    };

主框架函数

    //主函数
    int main()
    {
        double a = 1, b = 2, c = 3;//abc参数真实值
        int N = 100;
        double sigma = 1;
        cv::RNG rng;？？？
        double abc[3] = { 0, 0, 0 };//abc参数估计值
     
        //1. 产生顶点和观测值共100个数据点
        vector xdata, ydata;
     
        for (int i = 0; i < N; i++)
        {
            double x = i / 100;
            xdata.push_back(x);
            ydata.push_back(exp(a*x*x+b*x+c)+rng.gaussian(sigma);
        }
        //2. 构建图优化，设定线性方程求解器Blocksolver和迭代算法.
        typedef g2o::BlockSolver> Block;
        Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense();
        Block* solver_ptr = new Block(linearSolver);
     
        g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
        //图模型，并设置求解器
        g2o::SparseOptimizer optimizer;
        optimizer.setAlgorithm(solver);
        //3. 图中增加顶点
        CurveFittingVertex* v = new CurveFittingVertex();
        v->setEstimate(Eigen::Vector3d(0, 0, 0));
        v->setId(0);
        optimizer.addVertex(v);
        //4. 图中增加边
        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::Identity() * 1 / (sigma*sigma));//协方差矩阵逆
            optimizer.addEdge(edge);
        }
        //5. 执行优化
        optimizer.initializeOptimization();
        optimizer.optimize(100);
        Eigen::Vector3d abc_estimate = v->estimate();
     
    }

 

    注解1：cv产生随机数RNG

    cv::RNG rng
    rng.gaussian(sigma);

    注解2：g2o中派生出CureFittingVertex和CureFittingEdge.