第六讲 图优化工具g2o的入门
2016.11 更新
- 把原文的SIFT替换成了ORB，这样你可以在没有nonfree模块下使用本程序了。
- OpenCV可以使用 apt-get install libopencv-dev ，一样能成功。
- 因为换成了ORB，所以调整了good match的阈值，并且匹配时需要使用 Brute Force match。
- 请以现在的github上源码为准。
在上一讲中，我们介绍了如何使用两两匹配，搭建一个视觉里程计。那么，这个里程计有什么不足呢？
- 一旦出现了错误匹配，整个程序就会跑飞。
- 误差会累积。常见的现象是：相机转过去的过程能够做对，但转回来之后则出现明显的偏差。
- 效率方面不尽如人意。在线的点云显示比较费时。
累积误差是里程计中不可避免的，后续的相机姿态依赖着前面的姿态。想要保证地图的准确，必须要保证每次匹配都精确无误，而这是难以实现的。所以，我们希望用更好的方法来做slam。不仅仅考虑两帧的信息，而要把所有整的信息都考虑进来，成为一个全slam问题（full slam）。下图为累积误差的一个例子。右侧是原有扫过的地图，左侧是新扫的，可以看到出现了明显的不重合。
所以，我们这一讲要介绍姿态图（pose graph），它是目前视觉slam里最常用的方法之一。
姿态图（原理部分）
姿态图，顾名思义，就是由相机姿态构成的一个图（graph）。这里的图，是从图论的意义上来说的。一个图由节点与边构成：$$G=left{V,E ight}.$$ 在最简单的情况下，节点代表相机的各个姿态(四元数形式或矩阵形式）：$$v_i = left[ x,y,z,q_x, q_y, q_z, q_w ight] = T_i=left[ egin{array}{ll} R_{3 imes 3} & t_{3 imes 1} \ O_{1 imes 3} & 1 end{array} ight]_i $$
而边指的是两个节点间的变换：$$ E_{i,j} = T_{i,j} = left[ egin{array}{ll} R_{3 imes 3} & t_{3 imes 1} \ O_{1 imes 3} & 1 end{array} ight]_{i,j} .$$
于是乎，我们可以把前面计算的东西都放到了一个图里（请勿吐槽画风）。
对于vo，这个图应该像这样（同样请勿吐槽画风）：
像vo这样的图呢，我们并没有什么可以做的。然而，当这个图不是vo那样的链状结构时，由于边$T_{i,j}$中存在误差，使得所有的边给出的数据并不一致。这时节，我们就可以优化一个不一致性误差：$$min E = sumlimits_{i,j} | x_i^* - T_{i,j} x_j^* |_2^2 .$$这里$x_i^*$表示$x_i$的估计值。
小萝卜：师兄，什么叫估计值啊？
师兄：嗯，每个$x_i$实质上都是优化变量啦。在优化过程中，它们有一个初始值。然后呢，根据目标函数对$x$的梯度:$$ x_{(t+1)}^* = x_{(t)}^* - eta * abla_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 即可。
- 解压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。
1 cd build 2 ccmake .. 3 make 4 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
1 /************************************************************************* 2 > File Name: rgbd-slam-tutorial-gx/part V/src/visualOdometry.cpp 3 > Author: xiang gao 4 > Mail: gaoxiang12@mails.tsinghua.edu.cn 5 > Created Time: 2015年08月15日 星期六 15时35分42秒 6 * add g2o slam end to visual odometry 7 ************************************************************************/ 8 9 #include <iostream> 10 #include <fstream> 11 #include <sstream> 12 using namespace std; 13 14 #include "slamBase.h" 15 16 //g2o的头文件 17 #include <g2o/types/slam3d/types_slam3d.h> //顶点类型 18 #include <g2o/core/sparse_optimizer.h> 19 #include <g2o/core/block_solver.h> 20 #include <g2o/core/factory.h> 21 #include <g2o/core/optimization_algorithm_factory.h> 22 #include <g2o/core/optimization_algorithm_gauss_newton.h> 23 #include <g2o/solvers/csparse/linear_solver_csparse.h> 24 #include <g2o/core/robust_kernel.h> 25 #include <g2o/core/robust_kernel_factory.h> 26 #include <g2o/core/optimization_algorithm_levenberg.h> 27 28 29 // 给定index，读取一帧数据 30 FRAME readFrame( int index, ParameterReader& pd ); 31 // 估计一个运动的大小 32 double normofTransform( cv::Mat rvec, cv::Mat tvec ); 33 34 int main( int argc, char** argv ) 35 { 36 // 前面部分和vo是一样的 37 ParameterReader pd; 38 int startIndex = atoi( pd.getData( "start_index" ).c_str() ); 39 int endIndex = atoi( pd.getData( "end_index" ).c_str() ); 40 41 // initialize 42 cout<<"Initializing ..."<<endl; 43 int currIndex = startIndex; // 当前索引为currIndex 44 FRAME lastFrame = readFrame( currIndex, pd ); // 上一帧数据 45 // 我们总是在比较currFrame和lastFrame 46 string detector = pd.getData( "detector" ); 47 string descriptor = pd.getData( "descriptor" ); 48 CAMERA_INTRINSIC_PARAMETERS camera = getDefaultCamera(); 49 computeKeyPointsAndDesp( lastFrame, detector, descriptor ); 50 PointCloud::Ptr cloud = image2PointCloud( lastFrame.rgb, lastFrame.depth, camera ); 51 52 pcl::visualization::CloudViewer viewer("viewer"); 53 54 // 是否显示点云 55 bool visualize = pd.getData("visualize_pointcloud")==string("yes"); 56 57 int min_inliers = atoi( pd.getData("min_inliers").c_str() ); 58 double max_norm = atof( pd.getData("max_norm").c_str() ); 59 60 /******************************* 61 // 新增:有关g2o的初始化 62 *******************************/ 63 // 选择优化方法 64 typedef g2o::BlockSolver_6_3 SlamBlockSolver; 65 typedef g2o::LinearSolverCSparse< SlamBlockSolver::PoseMatrixType > SlamLinearSolver; 66 67 // 初始化求解器 68 SlamLinearSolver* linearSolver = new SlamLinearSolver(); 69 linearSolver->setBlockOrdering( false ); 70 SlamBlockSolver* blockSolver = new SlamBlockSolver( linearSolver ); 71 g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( blockSolver ); 72 73 g2o::SparseOptimizer globalOptimizer; // 最后用的就是这个东东 74 globalOptimizer.setAlgorithm( solver ); 75 // 不要输出调试信息 76 globalOptimizer.setVerbose( false ); 77 78 // 向globalOptimizer增加第一个顶点 79 g2o::VertexSE3* v = new g2o::VertexSE3(); 80 v->setId( currIndex ); 81 v->setEstimate( Eigen::Isometry3d::Identity() ); //估计为单位矩阵 82 v->setFixed( true ); //第一个顶点固定，不用优化 83 globalOptimizer.addVertex( v ); 84 85 int lastIndex = currIndex; // 上一帧的id 86 87 for ( currIndex=startIndex+1; currIndex<endIndex; currIndex++ ) 88 { 89 cout<<"Reading files "<<currIndex<<endl; 90 FRAME currFrame = readFrame( currIndex,pd ); // 读取currFrame 91 computeKeyPointsAndDesp( currFrame, detector, descriptor ); 92 // 比较currFrame 和 lastFrame 93 RESULT_OF_PNP result = estimateMotion( lastFrame, currFrame, camera ); 94 if ( result.inliers < min_inliers ) //inliers不够，放弃该帧 95 continue; 96 // 计算运动范围是否太大 97 double norm = normofTransform(result.rvec, result.tvec); 98 cout<<"norm = "<<norm<<endl; 99 if ( norm >= max_norm ) 100 continue; 101 Eigen::Isometry3d T = cvMat2Eigen( result.rvec, result.tvec ); 102 cout<<"T="<<T.matrix()<<endl; 103 104 // cloud = joinPointCloud( cloud, currFrame, T, camera ); 105 106 // 向g2o中增加这个顶点与上一帧联系的边 107 // 顶点部分 108 // 顶点只需设定id即可 109 g2o::VertexSE3 *v = new g2o::VertexSE3(); 110 v->setId( currIndex ); 111 v->setEstimate( Eigen::Isometry3d::Identity() ); 112 globalOptimizer.addVertex(v); 113 // 边部分 114 g2o::EdgeSE3* edge = new g2o::EdgeSE3(); 115 // 连接此边的两个顶点id 116 edge->vertices() [0] = globalOptimizer.vertex( lastIndex ); 117 edge->vertices() [1] = globalOptimizer.vertex( currIndex ); 118 // 信息矩阵 119 Eigen::Matrix<double, 6, 6> information = Eigen::Matrix< double, 6,6 >::Identity(); 120 // 信息矩阵是协方差矩阵的逆，表示我们对边的精度的预先估计 121 // 因为pose为6D的，信息矩阵是6*6的阵，假设位置和角度的估计精度均为0.1且互相独立 122 // 那么协方差则为对角为0.01的矩阵，信息阵则为100的矩阵 123 information(0,0) = information(1,1) = information(2,2) = 100; 124 information(3,3) = information(4,4) = information(5,5) = 100; 125 // 也可以将角度设大一些，表示对角度的估计更加准确 126 edge->setInformation( information ); 127 // 边的估计即是pnp求解之结果 128 edge->setMeasurement( T ); 129 // 将此边加入图中 130 globalOptimizer.addEdge(edge); 131 132 lastFrame = currFrame; 133 lastIndex = currIndex; 134 135 } 136 137 // pcl::io::savePCDFile( "data/result.pcd", *cloud ); 138 139 // 优化所有边 140 cout<<"optimizing pose graph, vertices: "<<globalOptimizer.vertices().size()<<endl; 141 globalOptimizer.save("./data/result_before.g2o"); 142 globalOptimizer.initializeOptimization(); 143 globalOptimizer.optimize( 100 ); //可以指定优化步数 144 globalOptimizer.save( "./data/result_after.g2o" ); 145 cout<<"Optimization done."<<endl; 146 147 globalOptimizer.clear(); 148 149 return 0; 150 } 151 152 FRAME readFrame( int index, ParameterReader& pd ) 153 { 154 FRAME f; 155 string rgbDir = pd.getData("rgb_dir"); 156 string depthDir = pd.getData("depth_dir"); 157 158 string rgbExt = pd.getData("rgb_extension"); 159 string depthExt = pd.getData("depth_extension"); 160 161 stringstream ss; 162 ss<<rgbDir<<index<<rgbExt; 163 string filename; 164 ss>>filename; 165 f.rgb = cv::imread( filename ); 166 167 ss.clear(); 168 filename.clear(); 169 ss<<depthDir<<index<<depthExt; 170 ss>>filename; 171 172 f.depth = cv::imread( filename, -1 ); 173 f.frameID = index; 174 return f; 175 } 176 177 double normofTransform( cv::Mat rvec, cv::Mat tvec ) 178 { 179 return fabs(min(cv::norm(rvec), 2*M_PI-cv::norm(rvec)))+ fabs(cv::norm(tvec)); 180 }
其中，大部分代码和上一讲是一样的，此外新增了几段g2o的初始化与简单使用。
使用g2o图优化的简要步骤：第一步，构建一个求解器：globalOptimizer
1 // 选择优化方法 2 typedef g2o::BlockSolver_6_3 SlamBlockSolver; 3 typedef g2o::LinearSolverCSparse< SlamBlockSolver::PoseMatrixType > SlamLinearSolver; 4 5 // 初始化求解器 6 SlamLinearSolver* linearSolver = new SlamLinearSolver(); 7 linearSolver->setBlockOrdering( false ); 8 SlamBlockSolver* blockSolver = new SlamBlockSolver( linearSolver ); 9 g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( blockSolver ); 10 11 g2o::SparseOptimizer globalOptimizer; // 最后用的就是这个东东 12 globalOptimizer.setAlgorithm( solver ); 13 // 不要输出调试信息 14 globalOptimizer.setVerbose( false );
然后，在求解器内添加点和边：
1 // 添加点 2 g2o::VertexSE3* v = new g2o::VertexSE3(); 3 // 设置点v ... 4 globalOptimizer.addVertex( v ); 5 6 // 添加边 7 g2o::EdgeSE3* edge = new g2o::EdgeSE3(); 8 // 设置边 edge ... 9 globalOptimizer.addEdge(edge);
最后，完成优化并存储优化结果：
1 globalOptimizer.save("./data/result_before.g2o"); 2 globalOptimizer.initializeOptimization(); 3 globalOptimizer.optimize( 100 ); //可以指定优化步数 4 globalOptimizer.save( "./data/result_after.g2o" );
大致就是这样啦。
关于代码的一些解释：
- 顶点和边的类型
顶点和边有不同的类型，这要看我们想求解什么问题。由于我们是3D的slam，所以顶点取成了相机姿态：g2o::VertexSE3，而边则是连接两个VertexSE3的边：g2o::EdgeSE3。如果你想用别的类型的顶点（如2Dslam，路标点），你可以看看/usr/local/include/g2o/types/下的文件，基本上涵盖了各种slam的应用，应该能满足你的需求。
小萝卜：师兄，什么是SE3呢？
师兄：简单地说，就是$4 imes 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例子来说，似乎没什么可以优化的……
结束语
好了，因为篇幅已经有些长了，本讲到这里先告一段落。在这一讲中，我们给读者介绍了g2o的安装与基本使用方法。为保证程序简单易懂，我们暂时没有用它构建实用的图程序，这会在下一讲中实现。同时，g2o也可以用来做回环检测，丢失恢复等工作，使得slam过程更加稳定可靠，真是一个方便的工具呢！
本讲代码：https://github.com/gaoxiang12/rgbd-slam-tutorial-gx/tree/master/part%20VI
数据请见上一讲。
未完待续
TIPS
- 现在（2016.10）github上的g2o已经可以在14.04下正常编译安装了，所以本文当中有些迂回的安装步骤就没必要了。请读者按照g2o的readme文件进行编译安装即可。