本文共 7302 字,大约阅读时间需要 24 分钟。
在上一讲中,我们介绍了如何使用两两匹配,搭建一个视觉里程计。那么,这个里程计有什么不足呢?
累积误差是里程计中不可避免的,后续的相机姿态依赖着前面的姿态。想要保证地图的准确,必须要保证每次匹配都精确无误,而这是难以实现的。所以,我们希望用更好的方法来做slam。不仅仅考虑两帧的信息,而要把所有整的信息都考虑进来,成为一个全slam问题(full slam)。下图为累积误差的一个例子。右侧是原有扫过的地图,左侧是新扫的,可以看到出现了明显的不重合。
所以,我们这一讲要介绍姿态图(pose graph),它是目前视觉slam里最常用的方法之一。
姿态图,顾名思义,就是由相机姿态构成的一个图(graph)。这里的图,是从图论的意义上来说的。一个图由节点与边构成:
G={V,E}.
在最简单的情况下,节点代表相机的各个姿态(四元数形式或矩阵形式):
vi=[x,y,z,qx,qy,qz,qw]=Ti=[R3×3O1×3t3×11]i
而边指的是两个节点间的变换:
Ei,j=Ti,j=[R3×3O1×3t3×11]i,j.
于是乎,我们可以把前面计算的东西都放到了一个图里(请勿吐槽画风)。
对于vo,这个图应该像这样(同样请勿吐槽画风):
像vo这样的图呢,我们并没有什么可以做的。然而,当这个图不是vo那样的链状结构时,由于边Ti,j
中存在误差,使得所有的边给出的数据并不一致。这时节,我们就可以优化一个不一致性误差:
minE=∑i,j‖x∗i−Ti,jx∗j‖22.
这里x∗i表示xi
的估计值。
小萝卜:师兄,什么叫估计值啊?
师兄:嗯,每个xi
实质上都是优化变量啦。在优化过程中,它们有一个初始值。然后呢,根据目标函数对x的梯度:
x∗(t+1)=x∗(t)−η∗∇xE
调整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,首先你需要下载并安装它: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 即可。
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引入自己的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
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 #include10 #include 11 #include 12 using namespace std; 13 14 #include "slamBase.h" 15 16 //g2o的头文件 17 #include //顶点类型 18 #include 19 #include 20 #include 21 #include 22 #include 23 #include 24 #include 25 #include 26 #include 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 ..."< 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 = max_norm )100 continue;101 Eigen::Isometry3d T = cvMat2Eigen( result.rvec, result.tvec );102 cout<<" t="< setId( currIndex );111 v->setEstimate( Eigen::Isometry3d::Identity() );112 globalOptimizer.addVertex(v);113 // 边部分114 g2o::EdgeSE3* edge = new g2o::EdgeSE3();115 // 连接此边的两个顶点id116 edge->vertices() [0] = globalOptimizer.vertex( lastIndex );117 edge->vertices() [1] = globalOptimizer.vertex( currIndex );118 // 信息矩阵119 Eigen::Matrix 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: "< >filename;165 f.rgb = cv::imread( filename );166 167 ss.clear();168 filename.clear();169 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" );
大致就是这样啦。
<g2o/types/slam3d/types_slam3d.h>
时,就已经把相关的点和边都包含进来了哦。好了,因为篇幅已经有些长了,本讲到这里先告一段落。在这一讲中,我们给读者介绍了g2o的安装与基本使用方法。为保证程序简单易懂,我们暂时没有用它构建实用的图程序,这会在下一讲中实现。同时,g2o也可以用来做回环检测,丢失恢复等工作,使得slam过程更加稳定可靠,真是一个方便的工具呢!
本讲代码:https://github.com/gaoxiang12/rgbd-slam-tutorial-gx/tree/master/part%20VI
数据请见上一讲。
未完待续
转载地址:http://vgbab.baihongyu.com/