一起做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

 /*************************************************************************

     > File Name: rgbd-slam-tutorial-gx/part V/src/visualOdometry.cpp

     > Author: xiang gao

     > Mail: [email protected]

     > Created Time: 2015年08月15日 星期六 15时35分42秒

     * add g2o slam end to visual odometry

     * add keyframe and simple loop closure

  ************************************************************************/

 #include <iostream>

 #include <fstream>

 #include <sstream>

 using namespace std;

 #include "slamBase.h"

 #include <pcl/filters/voxel_grid.h>

 #include <pcl/filters/passthrough.h>

 #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>

 // 把g2o的定义放到前面

 typedef g2o::BlockSolver_6_3 SlamBlockSolver;

 typedef g2o::LinearSolverCSparse< SlamBlockSolver::PoseMatrixType > SlamLinearSolver;

 // 给定index，读取一帧数据

 FRAME readFrame( int index, ParameterReader& pd );

 // 估计一个运动的大小

 double normofTransform( cv::Mat rvec, cv::Mat tvec );

 // 检测两个帧，结果定义

 enum CHECK_RESULT {NOT_MATCHED=, TOO_FAR_AWAY, TOO_CLOSE, KEYFRAME};

 // 函数声明

 CHECK_RESULT checkKeyframes( FRAME& f1, FRAME& f2, g2o::SparseOptimizer& opti, bool is_loops=false );

 // 检测近距离的回环

 void checkNearbyLoops( vector<FRAME>& frames, FRAME& currFrame, g2o::SparseOptimizer& opti );

 // 随机检测回环

 void checkRandomLoops( vector<FRAME>& frames, FRAME& currFrame, g2o::SparseOptimizer& opti );

 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() );

     // 所有的关键帧都放在了这里

     vector< FRAME > keyframes;

     // initialize

     cout<<"Initializing ..."<<endl;

     int currIndex = startIndex; // 当前索引为currIndex

     FRAME currFrame = readFrame( currIndex, pd ); // 当前帧数据

     string detector = pd.getData( "detector" );

     string descriptor = pd.getData( "descriptor" );

     CAMERA_INTRINSIC_PARAMETERS camera = getDefaultCamera();

     computeKeyPointsAndDesp( currFrame, detector, descriptor );

     PointCloud::Ptr cloud = image2PointCloud( currFrame.rgb, currFrame.depth, camera );

     /*******************************

     // 新增:有关g2o的初始化

     *******************************/

     // 初始化求解器

     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 );

     keyframes.push_back( currFrame );

     double keyframe_threshold = atof( pd.getData("keyframe_threshold").c_str() );

     bool check_loop_closure = pd.getData("check_loop_closure")==string("yes");

     for ( currIndex=startIndex+; currIndex<endIndex; currIndex++ )

     {

         cout<<"Reading files "<<currIndex<<endl;

         FRAME currFrame = readFrame( currIndex,pd ); // 读取currFrame

         computeKeyPointsAndDesp( currFrame, detector, descriptor ); //提取特征

         CHECK_RESULT result = checkKeyframes( keyframes.back(), currFrame, globalOptimizer ); //匹配该帧与keyframes里最后一帧

         switch (result) // 根据匹配结果不同采取不同策略

         {

         case NOT_MATCHED:

             //没匹配上，直接跳过

             cout<<RED"Not enough inliers."<<endl;

             break;

         case TOO_FAR_AWAY:

             // 太近了，也直接跳

             cout<<RED"Too far away, may be an error."<<endl;

             break;

         case TOO_CLOSE:

             // 太远了，可能出错了

             cout<<RESET"Too close, not a keyframe"<<endl;

             break;

         case KEYFRAME:

             cout<<GREEN"This is a new keyframe"<<endl;

             // 不远不近，刚好

             /**

              * This is important!!

              * This is important!!

              * This is important!!

              * (very important so I've said three times!)

              */

             // 检测回环

             if (check_loop_closure)

             {

                 checkNearbyLoops( keyframes, currFrame, globalOptimizer );

                 checkRandomLoops( keyframes, currFrame, globalOptimizer );

             }

             keyframes.push_back( currFrame );

             break;

         default:

             break;

         }

     }

     // 优化

     cout<<RESET"optimizing pose graph, vertices: "<<globalOptimizer.vertices().size()<<endl;

     globalOptimizer.save("./data/result_before.g2o");

     globalOptimizer.initializeOptimization();

     globalOptimizer.optimize(  ); //可以指定优化步数

     globalOptimizer.save( "./data/result_after.g2o" );

     cout<<"Optimization done."<<endl;

     // 拼接点云地图

     cout<<"saving the point cloud map..."<<endl;

     PointCloud::Ptr output ( new PointCloud() ); //全局地图

     PointCloud::Ptr tmp ( new PointCloud() );

     pcl::VoxelGrid<PointT> voxel; // 网格滤波器，调整地图分辨率

     pcl::PassThrough<PointT> pass; // z方向区间滤波器，由于rgbd相机的有效深度区间有限，把太远的去掉

     pass.setFilterFieldName("z");

     pass.setFilterLimits( 0.0, 4.0 ); //4m以上就不要了

     double gridsize = atof( pd.getData( "voxel_grid" ).c_str() ); //分辨图可以在parameters.txt里调

     voxel.setLeafSize( gridsize, gridsize, gridsize );

     for (size_t i=; i<keyframes.size(); i++)

     {

         // 从g2o里取出一帧

         g2o::VertexSE3* vertex = dynamic_cast<g2o::VertexSE3*>(globalOptimizer.vertex( keyframes[i].frameID ));

         Eigen::Isometry3d pose = vertex->estimate(); //该帧优化后的位姿

         PointCloud::Ptr newCloud = image2PointCloud( keyframes[i].rgb, keyframes[i].depth, camera ); //转成点云

         // 以下是滤波

         voxel.setInputCloud( newCloud );

         voxel.filter( *tmp );

         pass.setInputCloud( tmp );

         pass.filter( *newCloud );

         // 把点云变换后加入全局地图中

         pcl::transformPointCloud( *newCloud, *tmp, pose.matrix() );

         *output += *tmp;

         tmp->clear();

         newCloud->clear();

     }

     voxel.setInputCloud( output );

     voxel.filter( *tmp );

     //存储

     pcl::io::savePCDFile( "./data/result.pcd", *tmp );

     cout<<"Final map is saved."<<endl;

     globalOptimizer.clear();

     return ;

 }

 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, - );

     f.frameID = index;

     return f;

 }

 double normofTransform( cv::Mat rvec, cv::Mat tvec )

 {

     return fabs(min(cv::norm(rvec), *M_PI-cv::norm(rvec)))+ fabs(cv::norm(tvec));

 }

 CHECK_RESULT checkKeyframes( FRAME& f1, FRAME& f2, g2o::SparseOptimizer& opti, bool is_loops)

 {

     static ParameterReader pd;

     static int min_inliers = atoi( pd.getData("min_inliers").c_str() );

     static double max_norm = atof( pd.getData("max_norm").c_str() );

     static double keyframe_threshold = atof( pd.getData("keyframe_threshold").c_str() );

     static double max_norm_lp = atof( pd.getData("max_norm_lp").c_str() );

     static CAMERA_INTRINSIC_PARAMETERS camera = getDefaultCamera();

     static g2o::RobustKernel* robustKernel = g2o::RobustKernelFactory::instance()->construct( "Cauchy" );

     // 比较f1 和 f2

     RESULT_OF_PNP result = estimateMotion( f1, f2, camera );

     if ( result.inliers < min_inliers ) //inliers不够，放弃该帧

         return NOT_MATCHED;

     // 计算运动范围是否太大

     double norm = normofTransform(result.rvec, result.tvec);

     if ( is_loops == false )

     {

         if ( norm >= max_norm )

             return TOO_FAR_AWAY;   // too far away, may be error

     }

     else

     {

         if ( norm >= max_norm_lp)

             return TOO_FAR_AWAY;

     }

     if ( norm <= keyframe_threshold )

         return TOO_CLOSE;   // too adjacent frame

     // 向g2o中增加这个顶点与上一帧联系的边

     // 顶点部分

     // 顶点只需设定id即可

     if (is_loops == false)

     {

         g2o::VertexSE3 *v = new g2o::VertexSE3();

         v->setId( f2.frameID );

         v->setEstimate( Eigen::Isometry3d::Identity() );

         opti.addVertex(v);

     }

     // 边部分

     g2o::EdgeSE3* edge = new g2o::EdgeSE3();

     // 连接此边的两个顶点id

     edge->vertices() [] = opti.vertex( f1.frameID );

     edge->vertices() [] = opti.vertex( f2.frameID );

     edge->setRobustKernel( robustKernel );

     // 信息矩阵

     Eigen::Matrix<double, , > information = Eigen::Matrix< double, , >::Identity();

     // 信息矩阵是协方差矩阵的逆，表示我们对边的精度的预先估计

     // 因为pose为6D的，信息矩阵是6*6的阵，假设位置和角度的估计精度均为0.1且互相独立

     // 那么协方差则为对角为0.01的矩阵，信息阵则为100的矩阵

     information(,) = information(,) = information(,) = ;

     information(,) = information(,) = information(,) = ;

     // 也可以将角度设大一些，表示对角度的估计更加准确

     edge->setInformation( information );

     // 边的估计即是pnp求解之结果

     Eigen::Isometry3d T = cvMat2Eigen( result.rvec, result.tvec );

     edge->setMeasurement( T.inverse() );

     // 将此边加入图中

     opti.addEdge(edge);

     return KEYFRAME;

 }

 void checkNearbyLoops( vector<FRAME>& frames, FRAME& currFrame, g2o::SparseOptimizer& opti )

 {

     static ParameterReader pd;

     static int nearby_loops = atoi( pd.getData("nearby_loops").c_str() );

     // 就是把currFrame和 frames里末尾几个测一遍

     if ( frames.size() <= nearby_loops )

     {

         // no enough keyframes, check everyone

         for (size_t i=; i<frames.size(); i++)

         {

             checkKeyframes( frames[i], currFrame, opti, true );

         }

     }

     else

     {

         // check the nearest ones

         for (size_t i = frames.size()-nearby_loops; i<frames.size(); i++)

         {

             checkKeyframes( frames[i], currFrame, opti, true );

         }

     }

 }

 void checkRandomLoops( vector<FRAME>& frames, FRAME& currFrame, g2o::SparseOptimizer& opti )

 {

     static ParameterReader pd;

     static int random_loops = atoi( pd.getData("random_loops").c_str() );

     srand( (unsigned int) time(NULL) );

     // 随机取一些帧进行检测

     if ( frames.size() <= random_loops )

     {

         // no enough keyframes, check everyone

         for (size_t i=; i<frames.size(); i++)

         {

             checkKeyframes( frames[i], currFrame, opti, true );

         }

     }

     else

     {

         // randomly check loops

         for (int i=; i<random_loops; i++)

         {

             int index = rand()%frames.size();

             checkKeyframes( frames[index], currFrame, opti, true );

         }

     }

 }

　　几点注解：

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有新的理解时，也会写新的博客为大家介绍的！

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