g2o優化

接上一篇RANSAC

1 概述

就像g2o文獻[1]中說的一樣，它的目的是提供一個能夠容易應用到各種問題的一個簡單、易用的圖優化庫。它使用圖的形式來描述和執行非線性最小二乘問題的優化。所以整個框架大概分爲兩部分：一部分是用圖來描述，另一部分是優化。圖一描述了這個結構。

圖一 g2o框架

由上圖可以看出整個優化問題可以表述成是一個稀疏優化器（SpareOptimizer）。它由圖和優化方法組成。

圖由頂點和邊組成。具體到一個優化問題時，頂點表示待優化變量，它繼承自Base Vertex，在繼承時我們需要給定它的變量維度D和變量類型T;邊表示當前模型和觀測值之間的誤差，它分爲一元邊、二元邊和超邊，分別連接一個變量、兩個變量和多個變量，在繼承時需要提供誤差的維度D、類型E，如果不是超邊的話還需要提供它所連接的頂點類型。

優化方法可以分爲下降策略和線性方程組求解器。其中下降策略可以選擇高斯牛頓、LM和dogleg。這三種方法對變量（頂點）的更新策略不同，有時適用的條件也不同。而Solver主要是求解增量方程，其中SparseBlockMatrix主要是構建增量方程，LinearSolver是數值求解增量方程使用的方法。

對於一個非線性最小二乘問題（線性最小二乘問題比較簡單，直接對誤差函數求導，令導函數爲零即可求解），由於誤差函數求導並另導數爲零後轉化爲一個非線性方程組，不好求解，所以要使用迭代的方法來解決，這時候就可以使用g2o了。

2 g2o使用細節

我們還是按照上面的結構分兩個部分來介紹如何使用g2o。

構建圖

要構建圖我們首先要根據自己的問題來確定頂點和邊。

在上一篇文章中我們使用RANSAC估計出了一個直線模型，這裏我們在它模型基礎上進行優化。這個問題現在變成如何調整模型參數使得所有內點到新模型的距離和最短。

設直線方程爲,內點爲,那麼問題可以描述成 $min\sum_{i=1}^n \frac{\left | {a*x_i-y_i+c} \right | }{a^{2}+1}$ 。這裏我們要優化的是直線模型，它由兩個參數組成a和c，所以頂點的維度爲2,類型是double型的二維向量。對於每一個內點來說，誤差是它到直線的距離，所以邊的維度爲1,類型爲double，又因爲它只和直線模型有關，所以是一元邊，需要給出頂點的類型即double類型的二維向量。

在繼承的過程中我們還要重寫純虛函數（具體的可以看後面的實現）

在頂點的定義裏面要重寫：

virtual void setToOriginImpl() //重置估計值
virtual void oplusImpl( const double* update )//更新估計值
virtual bool read( std::istream& in )//讀函數
virtual bool write( std::ostream& out )//寫函數

在邊的定義裏要重寫：

void computeError()//計算誤差
virtual  void linearizeOplus()//計算雅可比矩陣（可選）
virtual bool read( std::istream& in )//讀函數
virtual bool write( std::ostream& out )//寫函數

這些定義完成後構建圖所需要的頂點和邊就準備好了，下面是優化算法。

優化算法

對於優化算法，由圖一可以看出它是有包含關係的，所以在構建它的時候可以按照結構，從後往前逐次構建。

using namespace g2o;
//估計這兩個參數都是優化變量維度
typedef BlockSolver< BlockSolverTraits<2, 1> >  Block;
//選擇線性求解器，因爲這裏求得的H矩陣是稠密的所以使用稠密求解器，當然也可以使用分解方法的求解器
Block::LinearSolverType* linear_solve = new LinearSolverDense<Block::PoseMatrixType>();
Block * solver_ptr = new Block(std::unique_ptr<Block::LinearSolverType>(linear_solve));
//選擇優化方法
OptimizationAlgorithmGaussNewton* solver = new OptimizationAlgorithmGaussNewton(std::unique_ptr<Block>(solver_ptr));

SparseOptimizer optimizer;
optimizer.setAlgorithm(solver);

這樣整個圖所需要的東西就準備好了，下面就是將信息添加進圖中。

對於頂點來說大概是這幾個操作步驟：

創建新頂點

給優化變量賦初值

給頂點編號

向圖中添加頂點

對於邊來說：

創建新邊

連接頂點

給觀測值

設置信息矩陣（類似於權重吧）

向圖中添加邊

3 實現及效果

（附帶生成數據、RANSAC）

還是用生成50個準確點，隨機生成50個噪點。先用RANSAC求解出一個內點數最多的模型，然後以此模型爲初值送入圖中進行迭代優化。

#include <iostream>
#include <vector>
#include <math.h>
#include <stdlib.h> //生成隨機點
#include <time.h> //根據系統時間生成隨機點種子
#include <random> //高斯噪聲生成
#include <fstream>


#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>



#define  random(a) (rand()%a) //0-a
struct DATA{
    int x;
    int y;
    int flag;
};

class RANSAC{
private:
    std::vector<DATA> _data;  //輸入數據
    int _min_size; //最小子集
    float _ratio; //數據外點比例，不知道的情況下默認0.5
    float _p; //至少一次採樣，沒有外點的概率,默認0.99
    //std::vector<float> infor_entro;  //信息熵
    std::vector<DATA>minset; //每次採樣的最小子集


public:
    double sum[2] = {0,0}; //內點誤差
    int times; //迭代次數
    std::vector<std::vector<float> > param; //計算的到的參數
    std::vector<int> inliners; //每個模型參數的內點個數
    std::vector<std::vector<DATA> > inliners_points; //每個模型的內點，在裏面的類型是結構體的時候需要保證裏面的空間足夠所以需要使用reserve
    RANSAC();
    RANSAC(std::vector<DATA>& data, int min_size, float p = 0.99, float ratio = 0.5){
        _data = data;
        _min_size = min_size;
        _ratio = ratio;
        _p = p;
    }
    void iterator();
    //void compute_infor_entro();
};

/**
 * 計算迭代次數，最小集採樣，計算模型參數，計算內點個數
 * 這裏默認當進行距離閾值判斷時，點爲內點的概率爲0.95,並且默認噪聲服從均值爲0,標準差爲1的高斯分佈
 */
void RANSAC::iterator() {
    times = (int)(log(1-_p)/log(1-pow((1-_ratio),_min_size)));//迭代次數

    minset.resize(_min_size);
    std::vector<DATA> temdata;
    param.resize(times);
    inliners_points.reserve(times + 1);

    for(int j = 0; j < times; j++)
    {
        temdata = _data;
        srand((unsigned)time(0) + j * 10);
        for(int i = 0; i < _min_size; i ++)//選取最小點集
        {
            int local = random(temdata.size());
            minset[i] = temdata[local];
            temdata[local] = temdata.back();
            temdata.pop_back();
        }

        //計算模型參數
        param[j].push_back((float)(minset[1].y - minset[0].y) / (minset[1].x - minset[0].x));
        param[j].push_back(minset[0].y - param[j][0] * minset[0].x) ;

        //計算每個參數對應的內點個數
        int inlinepoint = 0;

        for(int i = 0; i < _data.size(); i ++)
        {
            if(fabs(param[j][0]) > 100000 || fabs(param[j][1]) > 10000)//防止出現無窮大數
                break;
            sum[0] = abs(param[j][0] * _data[i].x - _data[i].y + param[j][1])/sqrt(pow(param[j][0],2.0) + 1);
            if(sum[0] <= 3.84)//距離閾值判斷，按照高斯分佈且判斷內點概率0.95的卡方檢驗設置
            {
                sum[1] += sum[0];
                inlinepoint ++;
                inliners_points[j].push_back(_data[i]);
            }

        }
        inliners.push_back(inlinepoint);
    }
}


//定義的頂點
class LineVertex : public  g2o::BaseVertex<2,Eigen::Vector2d>{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW //對齊Eigen指針
    virtual void setToOriginImpl() // 重置估計值
    {
        _estimate << 0,0;
    }

    virtual void oplusImpl( const double* update ) // 更新估計值
    {
        _estimate += Eigen::Vector2d(update);
    }
    // 存盤和讀盤：留空
    virtual bool read( std::istream& in ) {return 0;} //純虛函數需要重新定義一下
    virtual bool write( std::ostream& out ) const { return  0;}
};


//定義的邊
class LineEdge: public g2o::BaseUnaryEdge<1,double,LineVertex>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    LineEdge( int x ): BaseUnaryEdge(), _x(x) {}
    // 計算曲線模型誤差
    void computeError()
    {
        const LineVertex* v = static_cast<const LineVertex*> (_vertices[0]);//類型轉換，將向量轉化爲頂點類型
        const Eigen::Vector2d abc = v->estimate(); //獲得估計參數
        _error(0,0) = abs((abc(0,0) * _x - _measurement + abc(1,0)))/sqrt(pow(abc(0,0),2.0) + 1); //計算誤差
        //_error(0,0) = abs(_measurement - abc(0,0) * _x - abc(1,0));
    }
    //誤差對變量求導 可以不寫，那麼g2o會自動數值求導
    virtual  void linearizeOplus(){
        const LineVertex* v = static_cast<const LineVertex*> (_vertices[0]);
        const Eigen::Vector2d abc = v->estimate();
        double a = abc(0,0);
        double c = abc(1,0);
        _jacobianOplusXi(0,0) = (2*_x*(a*_x*_measurement+c)*(a*a+1)-2*a*(a*_x-_measurement+c))/(a*a+1)/(a*a+1);
        _jacobianOplusXi(0,1) = 2*(a*_x-_measurement+c)/(a*a+1);
    }
    virtual bool read( std::istream& in ) {return 0;} //純虛函數，需要重新寫
    virtual bool write( std::ostream& out ) const {return 0;}
public:
    int _x;  // x 值        y 值爲 _measurement
};


/**
 * 根據給定模型生成數據
 * @param data 存放生成數據
 * @param good 準確的數據個數
 * @param bad 加噪聲的數據個數
 */
void generatordata(std::vector<DATA>& data, int good = 50, int bad = 50)
{
    data.resize(good + bad);
    srand((unsigned)time(NULL));
    for(int i = 0; i < good; i ++) //生成準確點
    {
        data[i].x = random(good);
        data[i].y =  3 * data[i].x + 4;
        data[i].flag = 1;
    }
    double mean = 0;
    double stddev = 1;
    std::default_random_engine generator;
    std::normal_distribution<double> dist(mean, stddev);
    //srand((unsigned)time(0) + good*2);
    for(int i = good; i < good + bad; i ++)//噪點
    {
        data[i].x = random(good);
        data[i].y = random(good * 3);
                //data[i].y = 3 * data[i].x + 4 + dist(generator);
                ///如果按照測量誤差爲均值0 標準差1來生成數據的話，大多數情況下所有點都被判別爲內點，這樣就和處理大量外點情況不太相符
        data[i].flag = 0;
    }
}





int main ()
{

    std::vector<DATA> data;
    generatordata(data);//生成數據
    RANSAC it1(data,2);
    it1.iterator();//RANSAC迭代找到內點最多的模型

    //存放數據
    std::ofstream of("Ransac.txt");
    of << "數據（x y） 前50個爲準確點，後50個爲隨機生成的噪點: " << std::endl;
    for(int i = 0; i < data.size(); i ++)
    {
        of << data[i].x << " " << data[i].y << std::endl;
    }

    of << "擬合參數和內點個數（a c 內點個數）： " <<std::endl;
    for(int i = 0; i < it1.times; i ++)
    {
        of << it1.param[i][0] << " " << it1.param[i][1] << " " <<  it1.inliners[i]<< std::endl;
    }


    //輸出參數及內點個數
    int idex = 0, max_inliners = 0;
    for(int i = 0; i < it1.times; i ++)
    {
        if(it1.inliners[i] > max_inliners)
        {
            max_inliners = it1.inliners[i];
            idex = i;
        }
    }
    std::cout << "優化前最優模型： y = " << it1.param[idex][0] << "x + " << it1.param[idex][1]<< std::endl;
    std::cout << "優化前內點距離和： " << it1.sum[1] << std::endl;


    //g2o優化  根據結構從下往上
    using namespace g2o;
    //<int _PoseDim, int _LandmarkDim>   BlockSolverTraits<Eigen::Dynamic, Eigen::Dynamic>估計這兩個參數都是優化變量維度
    typedef BlockSolver< BlockSolverTraits<2, 1> >  Block;
    //選擇線性求解器，因爲這裏求得的H矩陣是稠密的所以使用稠密求解器，當然也可以使用分解方法的求解器
    Block::LinearSolverType* linear_solve = new LinearSolverDense<Block::PoseMatrixType>();
    Block * solver_ptr = new Block(std::unique_ptr<Block::LinearSolverType>(linear_solve));
    //選擇優化方法
    //OptimizationAlgorithmLevenberg* solver = new OptimizationAlgorithmLevenberg(std::unique_ptr<Block>(solver_ptr));
    OptimizationAlgorithmGaussNewton* solver = new OptimizationAlgorithmGaussNewton(std::unique_ptr<Block>(solver_ptr));
    //OptimizationAlgorithmDogleg* solver = new OptimizationAlgorithmDogleg(std::unique_ptr<Block>(solver_ptr));
    SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);

   // optimizer.setVerbose( true );       // 打開調試輸出

    // 往圖中增加頂點
    LineVertex* v = new LineVertex;
    v->setEstimate( Eigen::Vector2d(it1.param[idex][0],it1.param[idex][1]) );
    v->setId(0);
    optimizer.addVertex( v );

    // 往圖中增加邊
    for ( int i=0; i<max_inliners; i++ )
    {
        LineEdge* edge = new LineEdge( it1.inliners_points[idex][i].x );
        edge->setId(i);
        edge->setVertex( 0, v );                // 設置連接的頂點
        edge->setMeasurement( it1.inliners_points[idex][i].y );      // 觀測數值
        edge->setInformation( Eigen::Matrix<double,1,1>::Identity() ); // 信息矩陣如何設置？
        optimizer.addEdge( edge );
    }

    // 執行優化
    std::cout<<"開始優化......"<<std::endl;
    optimizer.initializeOptimization();//初始化圖
    optimizer.optimize(100);//最多迭代次數
    std::cout<<"優化結束！！！" << std::endl;

    // 輸出優化值
    Eigen::Vector2d abc_estimate = v->estimate();
    std::cout<<"優化後模型: " << "y = " << abc_estimate(0,0) << "x + " << abc_estimate(1,0) << std::endl;

    it1.sum[1] = 0;
    for(int i = 0; i < max_inliners; i ++)
    {
        it1.sum[0] = abs(abc_estimate(0,0) * it1.inliners_points[idex][i].x - it1.inliners_points[idex][i].y + abc_estimate(1,0))/sqrt(pow(abc_estimate(0,0),2.0) + 1);
        it1.sum[1] += it1.sum[0];
    }
    std::cout << "優化後內點距離和： " << it1.sum[1];


    return 0;
}

下面是一次運行結果：

優化前最優模型： y = 3.30435x + -6.04347
優化前內點距離和： 408.583
開始優化......
優化結束！！！
優化後模型: y = 3.30319x + -3.38884
優化後內點距離和： 74.7557
Process finished with exit code 0

4 參考資料

[1] R. Kümmerle, G. Grisetti, H. Strasdat, K. Konolige, and W. Burgard. g2o: A General Framework for
Graph Optimization. In Proc. of the IEEE Int. Conf. on Robotics and Automation (ICRA). Shanghai,
China, May 2011.

[2] 《視覺SLAM十四講從理論到實踐》第六講：非線性優化 P104

g2o優化

1 概述

2 g2o使用細節

構建圖

優化算法

3 實現及效果

4 參考資料

使用qt在windows的vs下寫串口助手

c++學習筆記----switch標籤

使用ｑｔ在ｗｉｎｄｏｗｓ的ｖｓ下寫串口助手

單應矩陣、基本矩陣和本質矩陣

g2o優化

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結