一起做RGB-D SLAM (6)

第六講 圖優化工具g2o的入門

 2016.11 更新

• 把原文的SIFT替換成了ORB，這樣你可以在沒有nonfree模塊下使用本程序了。
• OpenCV可以使用 apt-get install libopencv-dev ，一樣能成功。
• 因為換成了ORB，所以調整了good match的閾值，並且匹配時需要使用 Brute Force match。
• 請以現在的github上源碼為准。

　　在上一講中，我們介紹了如何使用兩兩匹配，搭建一個視覺里程計。那么，這個里程計有什么不足呢？

1.  一旦出現了錯誤匹配，整個程序就會跑飛。
2. 誤差會累積。常見的現象是：相機轉過去的過程能夠做對，但轉回來之后則出現明顯的偏差。
3. 效率方面不盡如人意。在線的點雲顯示比較費時。

　　累積誤差是里程計中不可避免的，后續的相機姿態依賴着前面的姿態。想要保證地圖的准確，必須要保證每次匹配都精確無誤，而這是難以實現的。所以，我們希望用更好的方法來做slam。不僅僅考慮兩幀的信息，而要把所有整的信息都考慮進來，成為一個全slam問題（full slam）。下圖為累積誤差的一個例子。右側是原有掃過的地圖，左側是新掃的，可以看到出現了明顯的不重合。

　　所以，我們這一講要介紹姿態圖（pose graph），它是目前視覺slam里最常用的方法之一。

---

 姿態圖（原理部分）

　　姿態圖，顧名思義，就是由相機姿態構成的一個圖（graph）。這里的圖，是從圖論的意義上來說的。一個圖由節點與邊構成：$$G=\left\{V,E\right\}.$$ 在最簡單的情況下，節點代表相機的各個姿態(四元數形式或矩陣形式）：$$v_i = \left[ x,y,z,q_x, q_y, q_z, q_w \right] = T_i=\left[ \begin{array}{ll} R_{3 \times 3} & t_{3 \times 1} \\ O_{1 \times 3} & 1 \end{array} \right]_i $$

　　而邊指的是兩個節點間的變換：$$ E_{i,j} = T_{i,j} = \left[ \begin{array}{ll} R_{3 \times 3} & t_{3 \times 1} \\ O_{1 \times 3} & 1 \end{array} \right]_{i,j} .$$

　　於是乎，我們可以把前面計算的東西都放到了一個圖里（請勿吐槽畫風）。

　　

 　　對於vo，這個圖應該像這樣（同樣請勿吐槽畫風）：

 

　　像vo這樣的圖呢，我們並沒有什么可以做的。然而，當這個圖不是vo那樣的鏈狀結構時，由於邊$T_{i,j}$中存在誤差，使得所有的邊給出的數據並不一致。這時節，我們就可以優化一個不一致性誤差：$$\min E = \sum\limits_{i,j} \| x_i^* - T_{i,j} x_j^* \|_2^2 .$$這里$x_i^*$表示$x_i$的估計值。

　　小蘿卜：師兄，什么叫估計值啊？

　　師兄：嗯，每個$x_i$實質上都是優化變量啦。在優化過程中，它們有一個初始值。然后呢，根據目標函數對$x$的梯度:$$ x_{(t+1)}^* = x_{(t)}^* - \eta * \nabla_x E$$ 調整$x$的值使得$E$縮小。最后，如果這個問題收斂的話，$x$的變化就會越來越小，$E$也收斂到一個極小值。在這個迭代的過程中，$x$那不斷變化的值就是$x^*$啦。

　　小蘿卜：哦我明白了！是不是運籌學書里講的非線性優化就是這個啊？

　　師兄：對！根據迭代策略的不同，又可分為Gauss-Netwon(GN)下山法，Levenberg-Marquardt(LM)方法等等。這個問題也稱為Bundle Adjustment(BA)，我們通常使用LM方法優化這個非線性平方誤差函數。

　　BA方法是近年來視覺slam里用的很多的方法（所以很多研究者吐槽slam和sfm(structure from motion)越來越像了）。早些年間（2005以前），人們還認為用BA求解slam非常困難，因為計算量太大。不過06年之后，人們注意到slam構建的ba問題的稀疏性質，所以用稀疏的BA算法（sparse BA）求解這個圖，才使BA在slam里廣泛地應用起來。

　　為什么說slam里的BA問題稀疏呢？因為同樣的場景很少出現在許多位置中。這導致上面的pose graph中，圖$G$離全圖很遠，只有少部分的節點存在直接邊的聯系。這就是姿態圖的稀疏性。

　　求解BA的軟件包有很多，感興趣的讀者可以去看wiki: https://en.wikipedia.org/wiki/Bundle_adjustment。我們這里介紹的g2o（Generalized Graph Optimizer），就是近年很流行的一個圖優化求解軟件包。下面我們通過實例代碼，幫助大家入門g2o。

---

 姿態圖（實現部分）

• 安裝g2o：

　　要使用g2o，首先你需要下載並安裝它：https://github.com/RainerKuemmerle/g2o。 在ubuntu 12.04下，安裝g2o步驟如下：

1. 安裝依賴項：
   

   sudo apt-get install libeigen3-dev libsuitesparse-dev libqt4-dev qt4-qmake libqglviewer-qt4-dev

   1404或1604的最后一項改為 libqglviewer-dev 即可。

2. 解壓g2o並編譯安裝：
   進入g2o的代碼目錄，並：
   

   mkdir build
   cd build 
   cmake ..
   make
   sudo make install

    多說兩句，你可以安裝cmake-curses-gui這個包，通過gui來選擇你想編譯的g2o模塊並設定cmake編譯過程中的flags。例如，當你實在裝不好上面的libqglviewer時，你可以選擇不編譯g2o可視化模塊（把G2O_BUILD_APPS關掉)，這樣即使沒有libqglviewer，你也能編譯過g2o。

   cd build
   ccmake ..
   make
   sudo make install

　　　　

　　安裝成功后，你可以在/usr/local/include/g2o中找到它的頭文件，而在/usr/local/lib中找到它的庫文件。

---

•  使用g2o

　　安裝完成后，我們把g2o引入自己的cmake工程：

# 添加g2o的依賴
# 因為g2o不是常用庫，要添加它的findg2o.cmake文件
LIST( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules )
SET( G2O_ROOT /usr/local/include/g2o )
FIND_PACKAGE( G2O )
# CSparse
FIND_PACKAGE( CSparse )
INCLUDE_DIRECTORIES( ${G2O_INCLUDE_DIR} ${CSPARSE_INCLUDE_DIR} )

　　同時，在代碼根目錄下新建cmake_modules文件夾，把g2o代碼目錄下的cmake_modules里的東西都拷進來，保證cmake能夠順利找到g2o。

　　現在，復制一個上一講的visualOdometry.cpp，我們把它改成slamEnd.cpp：

　　src/slamEnd.cpp

/*************************************************************************
    > File Name: rgbd-slam-tutorial-gx/part V/src/visualOdometry.cpp
    > Author: xiang gao
    > Mail: [e-mail]
    > Created Time: 2015年08月15日 星期六 15時35分42秒
    * add g2o slam end to visual odometry
 ************************************************************************/

#include <iostream>
#include <fstream>
#include <sstream>
using namespace std;

#include "slamBase.h"

//g2o的頭文件
#include <g2o/types/slam3d/types_slam3d.h> //頂點類型
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/factory.h>
#include <g2o/core/optimization_algorithm_factory.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/core/robust_kernel.h>
#include <g2o/core/robust_kernel_factory.h>
#include <g2o/core/optimization_algorithm_levenberg.h>


// 給定index，讀取一幀數據
FRAME readFrame( int index, ParameterReader& pd );
// 估計一個運動的大小
double normofTransform( cv::Mat rvec, cv::Mat tvec );

int main( int argc, char** argv )
{
    // 前面部分和vo是一樣的
    ParameterReader pd;
    int startIndex  =   atoi( pd.getData( "start_index" ).c_str() );
    int endIndex    =   atoi( pd.getData( "end_index"   ).c_str() );

    // initialize
    cout<<"Initializing ..."<<endl;
    int currIndex = startIndex; // 當前索引為currIndex
    FRAME lastFrame = readFrame( currIndex, pd ); // 上一幀數據
    // 我們總是在比較currFrame和lastFrame
    string detector = pd.getData( "detector" );
    string descriptor = pd.getData( "descriptor" );
    CAMERA_INTRINSIC_PARAMETERS camera = getDefaultCamera();
    computeKeyPointsAndDesp( lastFrame, detector, descriptor );
    PointCloud::Ptr cloud = image2PointCloud( lastFrame.rgb, lastFrame.depth, camera );
    
    pcl::visualization::CloudViewer viewer("viewer");

    // 是否顯示點雲
    bool visualize = pd.getData("visualize_pointcloud")==string("yes");

    int min_inliers = atoi( pd.getData("min_inliers").c_str() );
    double max_norm = atof( pd.getData("max_norm").c_str() );
    
    /******************************* 
    // 新增:有關g2o的初始化
    *******************************/
    // 選擇優化方法
    typedef g2o::BlockSolver_6_3 SlamBlockSolver; 
    typedef g2o::LinearSolverCSparse< SlamBlockSolver::PoseMatrixType > SlamLinearSolver; 

    // 初始化求解器
    SlamLinearSolver* linearSolver = new SlamLinearSolver();
    linearSolver->setBlockOrdering( false );
    SlamBlockSolver* blockSolver = new SlamBlockSolver( linearSolver );
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( blockSolver );

    g2o::SparseOptimizer globalOptimizer;  // 最后用的就是這個東東
    globalOptimizer.setAlgorithm( solver ); 
    // 不要輸出調試信息
    globalOptimizer.setVerbose( false );

    // 向globalOptimizer增加第一個頂點
    g2o::VertexSE3* v = new g2o::VertexSE3();
    v->setId( currIndex );
    v->setEstimate( Eigen::Isometry3d::Identity() ); //估計為單位矩陣
    v->setFixed( true ); //第一個頂點固定，不用優化
    globalOptimizer.addVertex( v );

    int lastIndex = currIndex; // 上一幀的id

    for ( currIndex=startIndex+1; currIndex<endIndex; currIndex++ )
    {
        cout<<"Reading files "<<currIndex<<endl;
        FRAME currFrame = readFrame( currIndex,pd ); // 讀取currFrame
        computeKeyPointsAndDesp( currFrame, detector, descriptor );
        // 比較currFrame 和 lastFrame
        RESULT_OF_PNP result = estimateMotion( lastFrame, currFrame, camera );
        if ( result.inliers < min_inliers ) //inliers不夠，放棄該幀
            continue;
        // 計算運動范圍是否太大
        double norm = normofTransform(result.rvec, result.tvec);
        cout<<"norm = "<<norm<<endl;
        if ( norm >= max_norm )
            continue;
        Eigen::Isometry3d T = cvMat2Eigen( result.rvec, result.tvec );
        cout<<"T="<<T.matrix()<<endl;
        
        // cloud = joinPointCloud( cloud, currFrame, T, camera );

        // 向g2o中增加這個頂點與上一幀聯系的邊
        // 頂點部分
        // 頂點只需設定id即可
        g2o::VertexSE3 *v = new g2o::VertexSE3();
        v->setId( currIndex );
        v->setEstimate( Eigen::Isometry3d::Identity() );
        globalOptimizer.addVertex(v);
        // 邊部分
        g2o::EdgeSE3* edge = new g2o::EdgeSE3();
        // 連接此邊的兩個頂點id
        edge->vertices() [0] = globalOptimizer.vertex( lastIndex );
        edge->vertices() [1] = globalOptimizer.vertex( currIndex );
        // 信息矩陣
        Eigen::Matrix<double, 6, 6> information = Eigen::Matrix< double, 6,6 >::Identity();
        // 信息矩陣是協方差矩陣的逆，表示我們對邊的精度的預先估計
        // 因為pose為6D的，信息矩陣是6*6的陣，假設位置和角度的估計精度均為0.1且互相獨立
        // 那么協方差則為對角為0.01的矩陣，信息陣則為100的矩陣
        information(0,0) = information(1,1) = information(2,2) = 100;
        information(3,3) = information(4,4) = information(5,5) = 100;
        // 也可以將角度設大一些，表示對角度的估計更加准確
        edge->setInformation( information );
        // 邊的估計即是pnp求解之結果
        edge->setMeasurement( T );
        // 將此邊加入圖中
        globalOptimizer.addEdge(edge);

        lastFrame = currFrame;
        lastIndex = currIndex;

    }

    // pcl::io::savePCDFile( "data/result.pcd", *cloud );
    
    // 優化所有邊
    cout<<"optimizing pose graph, vertices: "<<globalOptimizer.vertices().size()<<endl;
    globalOptimizer.save("./data/result_before.g2o");
    globalOptimizer.initializeOptimization();
    globalOptimizer.optimize( 100 ); //可以指定優化步數
    globalOptimizer.save( "./data/result_after.g2o" );
    cout<<"Optimization done."<<endl;

    globalOptimizer.clear();

    return 0;
}

FRAME readFrame( int index, ParameterReader& pd )
{
    FRAME f;
    string rgbDir   =   pd.getData("rgb_dir");
    string depthDir =   pd.getData("depth_dir");
    
    string rgbExt   =   pd.getData("rgb_extension");
    string depthExt =   pd.getData("depth_extension");

    stringstream ss;
    ss<<rgbDir<<index<<rgbExt;
    string filename;
    ss>>filename;
    f.rgb = cv::imread( filename );

    ss.clear();
    filename.clear();
    ss<<depthDir<<index<<depthExt;
    ss>>filename;

    f.depth = cv::imread( filename, -1 );
    f.frameID = index;
    return f;
}

double normofTransform( cv::Mat rvec, cv::Mat tvec )
{
    return fabs(min(cv::norm(rvec), 2*M_PI-cv::norm(rvec)))+ fabs(cv::norm(tvec));
}

　　其中，大部分代碼和上一講是一樣的，此外新增了幾段g2o的初始化與簡單使用。

　　使用g2o圖優化的簡要步驟：第一步，構建一個求解器：globalOptimizer

    // 選擇優化方法
    typedef g2o::BlockSolver_6_3 SlamBlockSolver; 
    typedef g2o::LinearSolverCSparse< SlamBlockSolver::PoseMatrixType > SlamLinearSolver; 

    // 初始化求解器
    SlamLinearSolver* linearSolver = new SlamLinearSolver();
    linearSolver->setBlockOrdering( false );
    SlamBlockSolver* blockSolver = new SlamBlockSolver( linearSolver );
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( blockSolver );

    g2o::SparseOptimizer globalOptimizer;  // 最后用的就是這個東東
    globalOptimizer.setAlgorithm( solver ); 
    // 不要輸出調試信息
    globalOptimizer.setVerbose( false );

 　　然后，在求解器內添加點和邊：

// 添加點
g2o::VertexSE3* v = new g2o::VertexSE3();
// 設置點v ...
globalOptimizer.addVertex( v );

// 添加邊
g2o::EdgeSE3* edge = new g2o::EdgeSE3();
// 設置邊 edge ...
globalOptimizer.addEdge(edge);

 　　最后，完成優化並存儲優化結果：

globalOptimizer.save("./data/result_before.g2o");
globalOptimizer.initializeOptimization();
globalOptimizer.optimize( 100 ); //可以指定優化步數
globalOptimizer.save( "./data/result_after.g2o" );

　　大致就是這樣啦。

---

關於代碼的一些解釋：

1. 頂點和邊的類型
   頂點和邊有不同的類型，這要看我們想求解什么問題。由於我們是3D的slam，所以頂點取成了相機姿態：g2o::VertexSE3，而邊則是連接兩個VertexSE3的邊：g2o::EdgeSE3。如果你想用別的類型的頂點（如2Dslam，路標點），你可以看看/usr/local/include/g2o/types/下的文件，基本上涵蓋了各種slam的應用，應該能滿足你的需求。
   小蘿卜：師兄，什么是SE3呢？
   師兄：簡單地說，就是$4 \times 4$的變換矩陣啦，也就是我們這里用的相機姿態了。更深層的解釋需要李代數里的知識。相應的，2D slam就要用SE2作為姿態節點了。在我們引用

   <g2o/types/slam3d/types_slam3d.h>
   時，就已經把相關的點和邊都包含進來了哦。

2. 優化方法
   g2o允許你使用不同的優化求解器（然而實際效果似乎差別不大）。你可以選用csparse, pcg, cholmod等等。我們這里使用csparse為例啦。
3. g2o文件
   g2o的優化結果是存儲在一個.g2o的文本文件里的，你可以用gedit等編輯軟件打開它，結構是這樣的：
   
   嗯，這個文件前面是頂點的定義，包含 ID, x,y,z,qx,qy,qz,qw。后邊則是邊的定義：ID1, ID2, dx, T 以及信息陣的上半角。實際上，你也可以自己寫個程序去生成這樣一個文件，交給g2o去優化，寫文本文件不會有啥困難的啦。
   這個文件也可以用g2o_viewer打開，你還能直觀地看到里面的節點與邊的位置。同時你可以選一個優化方法對該圖進行優化，這樣你可以直觀地看到優化的過程哦。然而對於我們現在的VO例子來說，似乎沒什么可以優化的……
   

---

結束語

　　好了，因為篇幅已經有些長了，本講到這里先告一段落。在這一講中，我們給讀者介紹了g2o的安裝與基本使用方法。為保證程序簡單易懂，我們暫時沒有用它構建實用的圖程序，這會在下一講中實現。同時，g2o也可以用來做回環檢測，丟失恢復等工作，使得slam過程更加穩定可靠，真是一個方便的工具呢！

　　本講代碼：https://github.com/gaoxiang12/rgbd-slam-tutorial-gx/tree/master/part%20VI

　　數據請見上一講。

未完待續

---

TIPS

• 現在（2016.10）github上的g2o已經可以在14.04下正常編譯安裝了，所以本文當中有些迂回的安裝步驟就沒必要了。請讀者按照g2o的readme文件進行編譯安裝即可。