本文转自高翔老师的博客,建议在学完教程的第二讲后,插入学习,做到工程快速入门。
原文链接:https://www.cnblogs.com/gaoxiang12/p/4754948.html
添加回环检测
简单回环检测的流程
上一讲中,我们介绍了图优化软件g2o的使用。本讲,我们将实现一个简单的回环检测程序,利用g2o提升slam轨迹与地图的质量。本讲结束后,读者朋友们将得到一个完整的slam程序,可以跑通我们在百度云上给出的数据了。所以呢,本讲也将成为我们“一起做”系列的终点啦。
小萝卜:这么快就要结束了吗师兄?
师兄:嗯,因为我想要说的都教给大家了嘛。不过,尽管如此,这个教程的程序还是比较初步的,可以进一步进行效率、鲁棒性方面的优化,这就要靠大家后续的努力啦。同时我的暑假也将近结束,要开始新一轮的工作了呢。
好的,话不多说,先来讲讲,上一讲的程序离完整的slam还有哪些距离。主要说来有两点:
- 关键帧的提取。把每一帧都拼到地图是去是不明智的。因为帧与帧之间距离很近,导致地图需要频繁更新,浪费时间与空间。所以,我们希望,当机器人的运动超过一定间隔,就增加一个“关键帧”。最后只需把关键帧拼到地图里就行了。
- 回环的检测。回环的本质是识别曾经到过的地方。最简单的回环检测策略,就是把新来的关键帧与之前所有的关键帧进行比较,不过这样会导致越往后,需要比较的帧越多。所以,稍微快速一点的方法是在过去的帧里随机挑选一些,与之进行比较。更进一步的,也可以用图像处理/模式识别的方法计算图像间的相似性,对相似的图像进行检测。
把这两者合在一起,就得到了我们slam程序的基本流程。以下为伪码:
- 初始化关键帧序列:FF,并将第一帧f0f0放入FF。
- 对于新来的一帧II,计算FF中最后一帧与II的运动,并估计该运动的大小ee。有以下几种可能性:
- 若e>Eerrore>Eerror,说明运动太大,可能是计算错误,丢弃该帧;
- 若没有匹配上(match太少),说明该帧图像质量不高,丢弃;
- 若e<Ekeye<Ekey,说明离前一个关键帧很近,同样丢弃;
- 剩下的情况,只有是特征匹配成功,运动估计正确,同时又离上一个关键帧有一定距离,则把II作为新的关键帧,进入回环检测程序:
- 近距离回环:匹配II与FF末尾mm个关键帧。匹配成功时,在图里增加一条边。
- 随机回环:随机在FF里取nn个帧,与II进行匹配。若匹配上,在图里增加一条边。
- 将II放入FF末尾。若有新的数据,则回2; 若无,则进行优化与地图拼接。
小萝卜:slam流程都是这样的吗?
师兄:大体上如此,也可以作一些更改。例如在线跑的话呢,可以定时进行一次优化与拼图。或者,在成功检测到回环时,同时检测这两个帧附近的帧,那样得到的边就更多啦。再有呢,如果要做实用的程序,还要考虑机器人如何运动,如果跟丢了怎么进行恢复等一些实际的问题呢。
实现代码
代码依旧是在上一讲的代码上进行更改得来的。由于是完整的程序,稍微有些长,请大家慢慢看:
src/slam.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 * 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 }
几点注解:
- 回环检测是很怕"false positive"的,即“将实际上不同的地方当成了同一处”,这会导致地图出现明显的不一致。所以,在使用g2o时,要在边里添加"robust kernel",保证一两个错误的边不会影响整体结果。
- 我在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就是mm,random_loops就是nn啦。这两个数如果设大一些,匹配的帧就会多,不过太大了就会影响整体速度了呢。
回环检测的效果
对代码进行编译,然后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有新的理解时,也会写新的博客为大家介绍的!