SLAM專題（11）-- G2O圖優化從入門到放棄（一）

目錄

1. Introduction 

1.1 什麼是優化？

1.2  什麼是圖優化？ 

1.3 圖優化理論（公式推導）

1.4  什麼是G2O？

1.5 圖優化與g2o流程小結

2. G2O安裝指南

2.1 安裝其他依賴庫

2.2 克隆、下載源代碼

2.3 cmake、make工程

2.4 測試g2o

3. 小結

---

1. Introduction 

本節我們將深入研究高翔博客（https://www.cnblogs.com/gaoxiang12/p/5244828.html）視覺slam中的主流優化方法——圖優化（graph-based optimization）。

　　除了這篇文檔，讀者還可以找到一篇關於圖優化的博客： http://blog.csdn.net/heyijia0327 那篇文章有作者介紹的一個簡單案例，而本文則更注重對圖優化和g2o的理解與評註。

文章  從零帶你開始學SLAM，理解圖優化和G2O代碼，頗有所得 。https://mp.weixin.qq.com/s/j9h9lT14jCu-VvEPHQhtBw

1.1 什麼是優化？

1.2  什麼是圖優化？ 

1.3 圖優化理論（公式推導）

1.4  什麼是G2O？

圖優化g2o核心框架和技術路線

1.5 圖優化與g2o流程小結

2. G2O安裝指南

2.1 安裝其他依賴庫

本文默認安裝環境爲Ubuntu16.04LTS版本，其他版本如Windows、MacOS、Android 系統安裝要求請移步作者Github。

https://github.com/RainerKuemmerle/g2o

---

Ubuntu16.04需要安裝的依賴庫（Requirements）如下：

終端輸入安裝依賴庫命令：

sudo apt-get install libeigen3-dev libsuitesparse-dev qtdeclarative5-dev qt5-qmake libqglviewer-dev

輸入sudo管理員密碼，安裝期間無需操作完成後退出終端即可。若出現錯誤代碼：~dpkg被其他程序佔用，則在資源管理器中將apt相關進程殺死重新安裝即可。

---

2.2 克隆、下載源代碼

g2o - General Graph Optimization是一個Cmake工程，其開源代碼託管在github上（https://github.com/RainerKuemmerle/g2o），下載或者克隆該項目文件，切絲，備用。

克隆完成之後看一下文件結構tree，看到一大堆文件就對了，

---

2.3 cmake、make工程

編譯方法類似於其他cmake工程項目，cmake、make、make install三步走。

1. 新建build文件夾

mkdir build

cd build

2. cmake工程構建

cmake ../

出現Build files have been written to：~~~~~，構建Cmake項目成功。

3. 編譯make，make install

make -j4

等待編譯完成

安裝bin文件，sudo make install

安裝成功，安裝位置爲 /usr/local/include/g2o/..

---

2.4 測試g2o

我們梳理是從頂層到底層，但是編程實現時需要反過來，像建房子一樣，從底層開始搭建框架一直到頂層。g2o的整個框架就是按照下圖中我標的這個順序來寫的。

高博在十四講中g2o求解曲線參數的例子來說明，源代碼地址：https://www.jianshu.com/p/da2f4ad9e918，

https://www.cnblogs.com/qi-zhang/p/9365199.html

#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(std::unique_ptr<Block::LinearSolverType>(linearSolver)  );      // 矩陣塊求解器
    // 梯度下降方法，從GN, LM, DogLeg 中選
     g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( std::unique_ptr<Block>(solver_ptr) );
//    g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( std::unique_ptr<Block>(solver_ptr) );
//    g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( std::unique_ptr<Block>(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;
}

結果：

---

3. 小結

本文從優化理論開始，詳細研究了圖優化理論在SLAM中的應用原理，並簡略介紹了g2o圖優化的技術路線和核心框架，並介紹了在Ubuntu系統上編譯g2o的方法和測試過程。