一起做RGB-D SLAM(7) (完结篇)

第七讲 添加回环检测

 2016.11 更新

  • 把原文的SIFT替换成了ORB,这样你可以在没有nonfree模块下使用本程序了。
  • 回环检测的阈值作出了相应的调整。
  • 请以现在的github上源码为准。

简单回环检测的流程

  上一讲中,我们介绍了图优化软件g2o的使用。本讲,我们将实现一个简单的回环检测程序,利用g2o提升slam轨迹与地图的质量。本讲结束后,读者朋友们将得到一个完整的slam程序,可以跑通我们在百度云上给出的数据了。所以呢,本讲也将成为我们“一起做”系列的终点啦。

  小萝卜:这么快就要结束了吗师兄?

  师兄:嗯,因为我想要说的都教给大家了嘛。不过,尽管如此,这个教程的程序还是比较初步的,可以进一步进行效率、鲁棒性方面的优化,这就要靠大家后续的努力啦。同时我的暑假也将近结束,要开始新一轮的工作了呢。

  好的,话不多说,先来讲讲,上一讲的程序离完整的slam还有哪些距离。主要说来有两点:

  1. 关键帧的提取。把每一帧都拼到地图是去是不明智的。因为帧与帧之间距离很近,导致地图需要频繁更新,浪费时间与空间。所以,我们希望,当机器人的运动超过一定间隔,就增加一个“关键帧”。最后只需把关键帧拼到地图里就行了。
  2. 回环的检测。回环的本质是识别曾经到过的地方。最简单的回环检测策略,就是把新来的关键帧与之前所有的关键帧进行比较,不过这样会导致越往后,需要比较的帧越多。所以,稍微快速一点的方法是在过去的帧里随机挑选一些,与之进行比较。更进一步的,也可以用图像处理/模式识别的方法计算图像间的相似性,对相似的图像进行检测。

  把这两者合在一起,就得到了我们slam程序的基本流程。以下为伪码:

  1. 初始化关键帧序列:$F$,并将第一帧$f_0$放入$F$。
  2. 对于新来的一帧$I$,计算$F$中最后一帧与$I$的运动,并估计该运动的大小$e$。有以下几种可能性:
    • 若$e>E_{error}$,说明运动太大,可能是计算错误,丢弃该帧; 
    • 若没有匹配上(match太少),说明该帧图像质量不高,丢弃; 
    • 若$e<E_{key}$,说明离前一个关键帧很近,同样丢弃;
    • 剩下的情况,只有是特征匹配成功,运动估计正确,同时又离上一个关键帧有一定距离,则把$I$作为新的关键帧,进入回环检测程序:
  3. 近距离回环:匹配$I$与$F$末尾$m$个关键帧。匹配成功时,在图里增加一条边。
  4. 随机回环:随机在$F$里取$n$个帧,与$I$进行匹配。若匹配上,在图里增加一条边。
  5. 将$I$放入$F$末尾。若有新的数据,则回2; 若无,则进行优化与地图拼接。

  小萝卜:slam流程都是这样的吗?

  师兄:大体上如此,也可以作一些更改。例如在线跑的话呢,可以定时进行一次优化与拼图。或者,在成功检测到回环时,同时检测这两个帧附近的帧,那样得到的边就更多啦。再有呢,如果要做实用的程序,还要考虑机器人如何运动,如果跟丢了怎么进行恢复等一些实际的问题呢。  


 实现代码

   代码依旧是在上一讲的代码上进行更改得来的。由于是完整的程序,稍微有些长,请大家慢慢看:

  src/slam.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     * add keyframe and simple loop closure
  8  ************************************************************************/
  9 
 10 #include <iostream>
 11 #include <fstream>
 12 #include <sstream>
 13 using namespace std;
 14 
 15 #include "slamBase.h"
 16 
 17 #include <pcl/filters/voxel_grid.h>
 18 #include <pcl/filters/passthrough.h>
 19 
 20 #include <g2o/types/slam3d/types_slam3d.h>
 21 #include <g2o/core/sparse_optimizer.h>
 22 #include <g2o/core/block_solver.h>
 23 #include <g2o/core/factory.h>
 24 #include <g2o/core/optimization_algorithm_factory.h>
 25 #include <g2o/core/optimization_algorithm_gauss_newton.h>
 26 #include <g2o/solvers/csparse/linear_solver_csparse.h>
 27 #include <g2o/core/robust_kernel.h>
 28 #include <g2o/core/robust_kernel_factory.h>
 29 #include <g2o/core/optimization_algorithm_levenberg.h>
 30 
 31 // 把g2o的定义放到前面
 32 typedef g2o::BlockSolver_6_3 SlamBlockSolver; 
 33 typedef g2o::LinearSolverCSparse< SlamBlockSolver::PoseMatrixType > SlamLinearSolver; 
 34 
 35 // 给定index,读取一帧数据
 36 FRAME readFrame( int index, ParameterReader& pd );
 37 // 估计一个运动的大小
 38 double normofTransform( cv::Mat rvec, cv::Mat tvec );
 39 
 40 // 检测两个帧,结果定义
 41 enum CHECK_RESULT {NOT_MATCHED=0, TOO_FAR_AWAY, TOO_CLOSE, KEYFRAME}; 
 42 // 函数声明
 43 CHECK_RESULT checkKeyframes( FRAME& f1, FRAME& f2, g2o::SparseOptimizer& opti, bool is_loops=false );
 44 // 检测近距离的回环
 45 void checkNearbyLoops( vector<FRAME>& frames, FRAME& currFrame, g2o::SparseOptimizer& opti );
 46 // 随机检测回环
 47 void checkRandomLoops( vector<FRAME>& frames, FRAME& currFrame, g2o::SparseOptimizer& opti );
 48 
 49 int main( int argc, char** argv )
 50 {
 51     // 前面部分和vo是一样的
 52     ParameterReader pd;
 53     int startIndex  =   atoi( pd.getData( "start_index" ).c_str() );
 54     int endIndex    =   atoi( pd.getData( "end_index"   ).c_str() );
 55 
 56     // 所有的关键帧都放在了这里
 57     vector< FRAME > keyframes; 
 58     // initialize
 59     cout<<"Initializing ..."<<endl;
 60     int currIndex = startIndex; // 当前索引为currIndex
 61     FRAME currFrame = readFrame( currIndex, pd ); // 当前帧数据
 62 
 63     string detector = pd.getData( "detector" );
 64     string descriptor = pd.getData( "descriptor" );
 65     CAMERA_INTRINSIC_PARAMETERS camera = getDefaultCamera();
 66     computeKeyPointsAndDesp( currFrame, detector, descriptor );
 67     PointCloud::Ptr cloud = image2PointCloud( currFrame.rgb, currFrame.depth, camera );
 68     
 69     /******************************* 
 70     // 新增:有关g2o的初始化
 71     *******************************/
 72     // 初始化求解器
 73     SlamLinearSolver* linearSolver = new SlamLinearSolver();
 74     linearSolver->setBlockOrdering( false );
 75     SlamBlockSolver* blockSolver = new SlamBlockSolver( linearSolver );
 76     g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( blockSolver );
 77 
 78     g2o::SparseOptimizer globalOptimizer;  // 最后用的就是这个东东
 79     globalOptimizer.setAlgorithm( solver ); 
 80     // 不要输出调试信息
 81     globalOptimizer.setVerbose( false );
 82     
 83 
 84     // 向globalOptimizer增加第一个顶点
 85     g2o::VertexSE3* v = new g2o::VertexSE3();
 86     v->setId( currIndex );
 87     v->setEstimate( Eigen::Isometry3d::Identity() ); //估计为单位矩阵
 88     v->setFixed( true ); //第一个顶点固定,不用优化
 89     globalOptimizer.addVertex( v );
 90     
 91     keyframes.push_back( currFrame );
 92 
 93     double keyframe_threshold = atof( pd.getData("keyframe_threshold").c_str() );
 94 
 95     bool check_loop_closure = pd.getData("check_loop_closure")==string("yes");
 96     for ( currIndex=startIndex+1; currIndex<endIndex; currIndex++ )
 97     {
 98         cout<<"Reading files "<<currIndex<<endl;
 99         FRAME currFrame = readFrame( currIndex,pd ); // 读取currFrame
100         computeKeyPointsAndDesp( currFrame, detector, descriptor ); //提取特征
101         CHECK_RESULT result = checkKeyframes( keyframes.back(), currFrame, globalOptimizer ); //匹配该帧与keyframes里最后一帧
102         switch (result) // 根据匹配结果不同采取不同策略
103         {
104         case NOT_MATCHED:
105             //没匹配上,直接跳过
106             cout<<RED"Not enough inliers."<<endl;
107             break;
108         case TOO_FAR_AWAY:
109             // 太近了,也直接跳
110             cout<<RED"Too far away, may be an error."<<endl;
111             break;
112         case TOO_CLOSE:
113             // 太远了,可能出错了
114             cout<<RESET"Too close, not a keyframe"<<endl;
115             break;
116         case KEYFRAME:
117             cout<<GREEN"This is a new keyframe"<<endl;
118             // 不远不近,刚好
119             /**
120              * This is important!!
121              * This is important!!
122              * This is important!!
123              * (very important so I've said three times!)
124              */
125             // 检测回环
126             if (check_loop_closure)
127             {
128                 checkNearbyLoops( keyframes, currFrame, globalOptimizer );
129                 checkRandomLoops( keyframes, currFrame, globalOptimizer );
130             }
131             keyframes.push_back( currFrame );
132             break;
133         default:
134             break;
135         }
136         
137     }
138 
139     // 优化
140     cout<<RESET"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     // 拼接点云地图
148     cout<<"saving the point cloud map..."<<endl;
149     PointCloud::Ptr output ( new PointCloud() ); //全局地图
150     PointCloud::Ptr tmp ( new PointCloud() );
151 
152     pcl::VoxelGrid<PointT> voxel; // 网格滤波器,调整地图分辨率
153     pcl::PassThrough<PointT> pass; // z方向区间滤波器,由于rgbd相机的有效深度区间有限,把太远的去掉
154     pass.setFilterFieldName("z");
155     pass.setFilterLimits( 0.0, 4.0 ); //4m以上就不要了
156 
157     double gridsize = atof( pd.getData( "voxel_grid" ).c_str() ); //分辨图可以在parameters.txt里调
158     voxel.setLeafSize( gridsize, gridsize, gridsize );
159 
160     for (size_t i=0; i<keyframes.size(); i++)
161     {
162         // 从g2o里取出一帧
163         g2o::VertexSE3* vertex = dynamic_cast<g2o::VertexSE3*>(globalOptimizer.vertex( keyframes[i].frameID ));
164         Eigen::Isometry3d pose = vertex->estimate(); //该帧优化后的位姿
165         PointCloud::Ptr newCloud = image2PointCloud( keyframes[i].rgb, keyframes[i].depth, camera ); //转成点云
166         // 以下是滤波
167         voxel.setInputCloud( newCloud );
168         voxel.filter( *tmp );
169         pass.setInputCloud( tmp );
170         pass.filter( *newCloud );
171         // 把点云变换后加入全局地图中
172         pcl::transformPointCloud( *newCloud, *tmp, pose.matrix() );
173         *output += *tmp;
174         tmp->clear();
175         newCloud->clear();
176     }
177 
178     voxel.setInputCloud( output );
179     voxel.filter( *tmp );
180     //存储
181     pcl::io::savePCDFile( "./data/result.pcd", *tmp );
182     
183     cout<<"Final map is saved."<<endl;
184     globalOptimizer.clear();
185 
186     return 0;
187 }
188 
189 FRAME readFrame( int index, ParameterReader& pd )
190 {
191     FRAME f;
192     string rgbDir   =   pd.getData("rgb_dir");
193     string depthDir =   pd.getData("depth_dir");
194     
195     string rgbExt   =   pd.getData("rgb_extension");
196     string depthExt =   pd.getData("depth_extension");
197 
198     stringstream ss;
199     ss<<rgbDir<<index<<rgbExt;
200     string filename;
201     ss>>filename;
202     f.rgb = cv::imread( filename );
203 
204     ss.clear();
205     filename.clear();
206     ss<<depthDir<<index<<depthExt;
207     ss>>filename;
208 
209     f.depth = cv::imread( filename, -1 );
210     f.frameID = index;
211     return f;
212 }
213 
214 double normofTransform( cv::Mat rvec, cv::Mat tvec )
215 {
216     return fabs(min(cv::norm(rvec), 2*M_PI-cv::norm(rvec)))+ fabs(cv::norm(tvec));
217 }
218 
219 CHECK_RESULT checkKeyframes( FRAME& f1, FRAME& f2, g2o::SparseOptimizer& opti, bool is_loops)
220 {
221     static ParameterReader pd;
222     static int min_inliers = atoi( pd.getData("min_inliers").c_str() );
223     static double max_norm = atof( pd.getData("max_norm").c_str() );
224     static double keyframe_threshold = atof( pd.getData("keyframe_threshold").c_str() );
225     static double max_norm_lp = atof( pd.getData("max_norm_lp").c_str() );
226     static CAMERA_INTRINSIC_PARAMETERS camera = getDefaultCamera();
227     static g2o::RobustKernel* robustKernel = g2o::RobustKernelFactory::instance()->construct( "Cauchy" );
228     // 比较f1 和 f2
229     RESULT_OF_PNP result = estimateMotion( f1, f2, camera );
230     if ( result.inliers < min_inliers ) //inliers不够,放弃该帧
231         return NOT_MATCHED;
232     // 计算运动范围是否太大
233     double norm = normofTransform(result.rvec, result.tvec);
234     if ( is_loops == false )
235     {
236         if ( norm >= max_norm )
237             return TOO_FAR_AWAY;   // too far away, may be error
238     }
239     else
240     {
241         if ( norm >= max_norm_lp)
242             return TOO_FAR_AWAY;
243     }
244 
245     if ( norm <= keyframe_threshold )
246         return TOO_CLOSE;   // too adjacent frame
247     // 向g2o中增加这个顶点与上一帧联系的边
248     // 顶点部分
249     // 顶点只需设定id即可
250     if (is_loops == false)
251     {
252         g2o::VertexSE3 *v = new g2o::VertexSE3();
253         v->setId( f2.frameID );
254         v->setEstimate( Eigen::Isometry3d::Identity() );
255         opti.addVertex(v);
256     }
257     // 边部分
258     g2o::EdgeSE3* edge = new g2o::EdgeSE3();
259     // 连接此边的两个顶点id
260     edge->vertices() [0] = opti.vertex( f1.frameID );
261     edge->vertices() [1] = opti.vertex( f2.frameID );
262     edge->setRobustKernel( robustKernel );
263     // 信息矩阵
264     Eigen::Matrix<double, 6, 6> information = Eigen::Matrix< double, 6,6 >::Identity();
265     // 信息矩阵是协方差矩阵的逆,表示我们对边的精度的预先估计
266     // 因为pose为6D的,信息矩阵是6*6的阵,假设位置和角度的估计精度均为0.1且互相独立
267     // 那么协方差则为对角为0.01的矩阵,信息阵则为100的矩阵
268     information(0,0) = information(1,1) = information(2,2) = 100;
269     information(3,3) = information(4,4) = information(5,5) = 100;
270     // 也可以将角度设大一些,表示对角度的估计更加准确
271     edge->setInformation( information );
272     // 边的估计即是pnp求解之结果
273     Eigen::Isometry3d T = cvMat2Eigen( result.rvec, result.tvec );
274     edge->setMeasurement( T.inverse() );
275     // 将此边加入图中
276     opti.addEdge(edge);
277     return KEYFRAME;
278 }
279 
280 void checkNearbyLoops( vector<FRAME>& frames, FRAME& currFrame, g2o::SparseOptimizer& opti )
281 {
282     static ParameterReader pd;
283     static int nearby_loops = atoi( pd.getData("nearby_loops").c_str() );
284     
285     // 就是把currFrame和 frames里末尾几个测一遍
286     if ( frames.size() <= nearby_loops )
287     {
288         // no enough keyframes, check everyone
289         for (size_t i=0; i<frames.size(); i++)
290         {
291             checkKeyframes( frames[i], currFrame, opti, true );
292         }
293     }
294     else
295     {
296         // check the nearest ones
297         for (size_t i = frames.size()-nearby_loops; i<frames.size(); i++)
298         {
299             checkKeyframes( frames[i], currFrame, opti, true );
300         }
301     }
302 }
303 
304 void checkRandomLoops( vector<FRAME>& frames, FRAME& currFrame, g2o::SparseOptimizer& opti )
305 {
306     static ParameterReader pd;
307     static int random_loops = atoi( pd.getData("random_loops").c_str() );
308     srand( (unsigned int) time(NULL) );
309     // 随机取一些帧进行检测
310     
311     if ( frames.size() <= random_loops )
312     {
313         // no enough keyframes, check everyone
314         for (size_t i=0; i<frames.size(); i++)
315         {
316             checkKeyframes( frames[i], currFrame, opti, true );
317         }
318     }
319     else
320     {
321         // randomly check loops
322         for (int i=0; i<random_loops; i++)
323         {
324             int index = rand()%frames.size();
325             checkKeyframes( frames[index], currFrame, opti, true );
326         }
327     }
328 }

 

   几点注解:

  1. 回环检测是很怕"false positive"的,即“将实际上不同的地方当成了同一处”,这会导致地图出现明显的不一致。所以,在使用g2o时,要在边里添加"robust kernel",保证一两个错误的边不会影响整体结果。
  2. 我在slambase.h里添加了一些彩色输出代码。运行此程序时,出现绿色信息则是添加新的关键帧,红色为出错。

  parameters.txt里定义了检测回环的一些参数:

#part 7
keyframe_threshold=0.1
max_norm_lp=5.0
# Loop closure
check_loop_closure=yes
nearby_loops=5
random_loops=5

 

   其中,nearby_loops就是$m$,random_loops就是$n$啦。这两个数如果设大一些,匹配的帧就会多,不过太大了就会影响整体速度了呢。


回环检测的效果

  对代码进行编译,然后bin/slam即可看到程序运行啦。

  添加了回环检测之后呢,g2o文件就不会像上次那样孤零零的啦,看起来是这样子的:

   

  怎么样?是不是感觉整条轨迹“如丝般顺滑”了呢?它不再是上一讲那样一根筋通到底,而是有很多帧间的匹配数据,保证了一两帧出错能被其他匹配数据给“拉回来”。

  百度云上的数据最后拼出来是这样的哦(780帧,关键帧62张,帧率5Hz左右):

  咖啡台左侧有明显的人通过的痕迹,导致地图上出现了他的身影(帅哥你好拉风):

  嗯,这个就可以算作是基本的地图啦。至此,slam的两大目标:“轨迹”和“地图”我们都已得到了,可以算是基本上解决了这个问题了。


 

一些后话

  这一个“一起做rgb-d slam”系列,前前后后花了我一个多月的时间。写代码,调代码,然后写成博文。虽然讲的比较啰嗦,总体上希望能对各位slam爱好者、研究者有帮助啦!这样我既然辛苦也很开心的!

  写作期间,得到了女朋友大脸莲的不少帮助,也得到了读者和同行之间的鼓励,谢谢各位啦!等有工夫,我会把这一堆东西整理成一个pdf供slam初学者进行研究学习的。

  slam仍是一个开放的问题,尽管有人曾说“在slam领域发文章越来越难”,然而现在机器人几大期刊和会议(IJRR/TRO/RAM/JFD/ICRA/IROS...)仍有不少slam方面的文章。虽然我们“获取轨迹与地图”的目标已基本实现,但仍有许多工作等我们去做,包括:

  • 更好的数学模型(新的滤波器/图优化理论); 
  • 新的视觉特征/不使用特征的直接方法;
  • 动态物体/人的处理;
  • 地图描述/点云地图优化/语义地图
  • 长时间/大规模/自动化slam
  • 等等……

  总之,大家千万别以为“slam问题已经有标准答案啦”。等我对slam有新的理解时,也会写新的博客为大家介绍的!

  如果你觉得我的博客有帮助,可以进行几块钱的小额赞助,帮助我把博客写得更好。(虽然我也是从别处学来的这招……)

  

  本讲代码:https://github.com/gaoxiang12/rgbd-slam-tutorial-gx/tree/master/part%20VII

  数据:http://yun.baidu.com/s/1i33uvw5

  交流群:374238181

posted @ 2015-08-24 15:44  半闲居士  阅读(48873)  评论(35编辑  收藏  举报