一起做RGB-D SLAM (6)-图优化工具g2o的入门

一起做RGB-D SLAM (6)-图优化工具g2o的入门

说明：

• 上节介绍了如何使用两两匹配，搭建一个视觉里程计
• 本节介绍如何修复视觉里程计的不足

视觉里程计的不足：

• 一旦出现了错误匹配，整个程序就会跑飞。

• 误差会累积。常见的现象是：相机转过去的过程能够做对，但转回来之后则出现明显的偏差。

• 效率方面不尽如人意。在线的点云显示比较费时。

• 累积误差是里程计中不可避免的，后续的相机姿态依赖着前面的姿态。

• 想要保证地图的准确，必须要保证每次匹配都精确无误，而这是难以实现的。

• 所以，我们希望用更好的方法来做slam。

• 不仅仅考虑两帧的信息，而要把所有整的信息都考虑进来，成为一个全slam问题（full slam）。

• 下图为累积误差的一个例子。右侧是原有扫过的地图，左侧是新扫的，可以看到出现了明显的不重合。
  

• 所以，我们这一讲要介绍姿态图（pose graph），它是目前视觉slam里最常用的方法之一

姿态图（原理部分）

• 
• 
• 
• 
• 根据迭代策略的不同，又可分为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。我们这里介绍的g2o（Generalized Graph Optimizer），就是近年很流行的一个图优化求解软件包。下面我们通过实例代码，帮助大家入门g2o。

姿态图（实现部分）

安装g2o

• 下载并安装它：https://github.com/RainerKuemmerle/g2o
• 在ubuntu 12.04下，安装g2o步骤如下：
• 安装依赖库:

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

• 1404或1604的最后一项改为 libqglviewer-dev 即可
• 解压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： vim src/slamEnd.cpp
• 代码如下：

/*************************************************************************
    > File Name: rgbd-slam-tutorial-gx/part V/src/visualOdometry.cpp
    > Author: xiang gao
    > Mail: gaoxiang12@mails.tsinghua.edu.cn
    > 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" );

代码解释:

• 顶点和边的类型

• 顶点和边有不同的类型，这要看我们想求解什么问题。由于我们是3D的slam，所以顶点取成了相机姿态：g2o::VertexSE3，而边则是连接两个VertexSE3的边：g2o::EdgeSE3。如果你想用别的类型的顶点（如2Dslam，路标点），你可以看看/usr/local/include/g2o/types/下的文件，基本上涵盖了各种slam的应用，应该能满足你的需求。

• 什么是SE3呢？简单地说，就是4×4的变换矩阵啦，也就是我们这里用的相机姿态了。更深层的解释需要李代数里的知识。相应的，2D slam就要用SE2作为姿态节点了。在我们引用<g2o/types/slam3d/types_slam3d.h>时，就已经把相关的点和边都包含进来了哦。

• 优化方法

• g2o允许你使用不同的优化求解器（然而实际效果似乎差别不大）。你可以选用csparse, pcg, cholmod等等。

• 我们这里使用csparse为例啦。

• g2o文件

• g2o的优化结果是存储在一个.g2o的文本文件里的，你可以用gedit等编辑软件打开它，结构是这样的:
  

• 这个文件前面是顶点的定义，包含 ID, x,y,z,qx,qy,qz,qw。后边则是边的定义：ID1, ID2, dx, T 以及信息阵的上半角。实际上，你也可以自己写个程序去生成这样一个文件，交给g2o去优化，写文本文件不会有啥困难的啦。

• 这个文件也可以用g2o_viewer打开，你还能直观地看到里面的节点与边的位置。同时你可以选一个优化方法对该图进行优化，这样你可以直观地看到优化的过程哦。然而对于我们现在的VO例子来说，似乎没什么可以优化的……
  

提示：

• 现在（2016.10）github上的g2o已经可以在14.04下正常编译安装了，所以本文当中有些迂回的安装步骤就没必要了。
• 请读者按照g2o的readme文件进行编译安装即可

参考：

• 本节github库：https://github.com/gaoxiang12/rgbd-slam-tutorial-gx/tree/master/part%20VI