一起做RGB-D SLAM (6)

第六讲 图优化工具g2o的入门

　　在上一讲中，我们介绍了如何使用两两匹配，搭建一个视觉里程计。那么，这个里程计有什么不足呢？

  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. 安装依赖项：

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

    如果你正使用14.04或其他版本，那么libQGLViewer可能会有点问题（这个包主要是用来构建g2o的可视化模块g2o_viewer的）。请你去google这个包并用dpkg -i命令安装它。一个可用的网址：http://www.ubuntuupdates.org/package/core/precise/universe/base/libqglviewer-qt4-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。

    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: [email protected]
  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" );

　　大致就是这样啦。

关于代码的一些解释：

  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

　　数据请见上一讲。

未完待续

时间： 2024-11-06 03:48:06

一起做RGB-D SLAM (6)的相关文章

一起做RGB-D SLAM (4)

第四讲 点云拼接 广告:“一起做”系列的代码网址:https://github.com/gaoxiang12/rgbd-slam-tutorial-gx 当博客更新时代码也会随着更新. SLAM技术交流群:254787961 读者朋友们大家好!尽管还没到一周,我们的教程又继续更新了,因为暑假实在太闲了嘛! 上讲回顾 上一讲中,我们理解了如何利用图像中的特征点,估计相机的运动.最后,我们得到了一个旋转向量与平移向量.那么读者可能会问:这两个向量有什么用呢?在这一讲里,我们就要使用这两个向量,把两张

QT使用QgraphicView/Qpainter展示RGB/YUV图像

本文记载说明使用Qt做RGB的渲染流程和问题.笔者认为:做媒体渲染这种数据量大而对象少的应用场景,使用Qpainter更佳,在多种RGB的转换中,YUV转RGB565最小而且字节对齐问题不容易发生,故推荐之. 加入与opengl  ddraw sdl 的 对比. RGB->QImage->QPixMap->Scene->GrapView 图像的数据是以字节为单位保存的,每一行的字节数必须是4的整数倍,不足的补0. (因为我们使用的是32操作系统,因此数据是按照32位对齐的,所以每行

Camera图像处理原理及实例分析-重要图像概念

Camera图像处理原理及实例分析 作者:刘旭晖  [email protected]  转载请注明出处 BLOG:http://blog.csdn.net/colorant/ 主页:http://rgbbones.googlepages.com/ 做为拍照手机的核心模块之一,camera sensor 效果的调整,涉及到众多的参数,如果对基本的光学原理及 sensor 软/硬件对图像处理的原理能有深入的理解和把握的话,对我们的工作将会起到事半功倍的效果.否则,缺乏了理论的指导,只能是凭感觉和经

Camera图像处理原理及实例分析

Camera图像处理原理及实例分析 作者:刘旭晖  [email protected]  转载请注明出处 BLOG:http://blog.csdn.net/colorant/ 主页:http://rgbbones.googlepages.com/ 做为拍照手机的核心模块之一,camera sensor 效果的调整,涉及到众多的参数,如果对基本的光学原理及 sensor 软/硬件对图像处理的原理能有深入的理解和把握的话,对我们的工作将会起到事半功倍的效果.否则,缺乏了理论的指导,只能是凭感觉和经

Unity -----一些可能存在的错误

关于Unity中的资源管理,你可能遇到这些问题 张鑫 8 个月前 原文链接:关于Unity中的资源管理,你可能遇到这些问题 - Blog 在优化Unity项目时,对资源的管理可谓是个系统纷繁的大工程.鉴于Unity独特又绝(cao)妙(dan)资源打包的AssetBundle管理机制,不同资源的属性适合于不同的存储和加载方式.此外,要处理好成百上千个资源之间的相互依赖关系也非易事.谁说良好的资源管理不是个艺术呢?:) ------------------ 关键字 AssetBundle资源制作

乐学成语项目的实现

首先给软件取名为HappyIdiom,在开始编码之前,需要先对程序进行需求分析,想一想HappyIdiom中应该具备哪些功能.将这些功能全部整理出来之后,我们才好动手一一实现.这里我认为HappyIdiom中至少应该具备以下功能: 1.成语分类学习:你可以根据自己喜欢的分类(动物类.人物类.季节类.自然类.数字类.语言类.其他类)来进行学习. 2.乐猜成语:学习之余玩玩猜成语游戏,寓教于乐. 3.成语收藏:当用户遇到自己想要保存的成语,点击保存按钮,即保存到收藏页面以方便用户再次查看,对已经掌握

一起做RGB-D SLAM (5)

第五讲 Visual Odometry (视觉里程计) 读者朋友们大家好,又到了我们开讲rgbd slam的时间了.由于前几天博主在忙开会拍婚纱照等一系列乱七八糟的事情,这一讲稍微做的慢了些,先向读者们道个歉! 上几讲中,我们详细讲了两张图像间的匹配与运动估计.然而一个实际的机器人总不可能只有两个图像数据吧?那该多么寂寞呀.所以,本讲开始,我们要处理一个视频流,包含八百左右的数据啦.这才像是在做SLAM嘛! 小萝卜:那我们去哪里下载这些数据呢? 师兄:可以到我的百度云里去:http://yun.

一起做RGB-D SLAM (2)

第二讲 从图像到点云 本讲中,我们将带领读者,编写一个将图像转换为点云的程序.该程序是后期处理地图的基础.最简单的点云地图即是把不同位置的点云进行拼接得到的. 当我们使用RGB-D相机时,会从相机里读到两种数据:彩色图像和深度图像.如果你有Kinect和ros,可以运行: 1 roslaunch openni_launch openni.launch 使Kinect工作.随后,如果PC连接上了Kinect,彩色图像与深度图像就会发布在 /camera/rgb/image_color 和 /cam

一起做RGB-D SLAM 第二季 （一）

小萝卜:师兄!过年啦!是不是很无聊啊!普通人的生活就是赚钱花钱,实在是很没意思啊! 师兄:是啊…… 小萝卜:他们都不懂搞科研和码代码的乐趣呀! 师兄:可不是嘛…… 小萝卜:所以今年过年,我们再做一个SLAM吧!之前写的那个太烂了啦,我都不好意思说是我做的了! 师兄:嗯那可真是对不住你啊…… 小萝卜:没事!你再写一个好一点的,我就原谅你了!写完再请我吃饭吧! 师兄:啊,好的…… 小萝卜:师兄你别这么没精神啊!加油咯! 前言 在经过了一番激烈的思想斗争之后呢,师兄厌倦了年假的无聊生活,开始写<一起做