一起做RGB-D SLAM (6)
来源:互联网 发布:w10无法连接到windows 编辑:程序博客网 时间:2024/06/05 15:56
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里最常用的方法之一。
姿态图(原理部分)
小萝卜:哦我明白了!是不是运筹学书里讲的非线性优化就是这个啊?
师兄:对!根据迭代策略的不同,又可分为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中,图
求解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 buildcd build cmake ..makesudo make install
多说两句,你可以安装cmake-curses-gui这个包,通过gui来选择你想编译的g2o模块并设定cmake编译过程中的flags。例如,当你实在装不好上面的libqglviewer时,你可以选择不编译g2o可视化模块(把G2O_BUILD_APPS关掉),这样即使没有libqglviewer,你也能编译过g2o。
1 cd build2 ccmake ..3 make4 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 )# CSparseFIND_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: 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);
最后,完成优化并存储优化结果:
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×4 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例子来说,似乎没什么可以优化的……
结束语
好了,因为篇幅已经有些长了,本讲到这里先告一段落。在这一讲中,我们给读者介绍了g2o的安装与基本使用方法。为保证程序简单易懂,我们暂时没有用它构建实用的图程序,这会在下一讲中实现。同时,g2o也可以用来做回环检测,丢失恢复等工作,使得slam过程更加稳定可靠,真是一个方便的工具呢!
本讲代码:https://github.com/gaoxiang12/rgbd-slam-tutorial-gx/tree/master/part%20VI
数据请见上一讲。
未完待续
TIPS
- 现在(2016.10)github上的g2o已经可以在14.04下正常编译安装了,所以本文当中有些迂回的安装步骤就没必要了。请读者按照g2o的readme文件进行编译安装即可。
- 一起做RGB-D SLAM (6)
- 一起做RGB-D SLAM (6)
- 一起做RGB-D SLAM (6)
- 一起做RGB-D SLAM (6)
- 一起做RGB-D SLAM(6)
- 【转】一起做RGB-D SLAM (1)
- 一起做RGB-D SLAM (1)
- 一起做RGB-D SLAM (2)
- 一起做RGB-D SLAM (3)
- 一起做RGB-D SLAM (4)
- 一起做RGB-D SLAM (5)
- 一起做RGB-D SLAM (3)
- 一起做RGB-D SLAM (1)
- 一起做RGB-D SLAM (1)
- 一起做RGB-D SLAM (2)
- 一起做RGB-D SLAM (3)
- 一起做RGB-D SLAM (4)
- 一起做RGB-D SLAM (5)
- 【2017/4/13】linux关于如何增加swap大小
- iOS中block块的存储位置&内存管理
- Spark 任务调度之Executor执行task并返回结果
- cd5WHjDyFWcpnNHvtHIbDA==
- SQLite的sqlite_sequence表
- 一起做RGB-D SLAM (6)
- 安利以及orz
- 有关ssh连接linux服务器后在mysql查询后输出中文结果乱码的的问题
- BZOJ 4316 小C的独立集
- JavaScript学习笔记之函数
- Spring配置数据源的三种方式 (JNDI\C3PO)
- height:100%失效
- 关于rxjava的知识
- 读者-写者同步问题的写者优先实现