一起做RGB-D SLAM (5)

第五讲 Visual Odometry (视觉里程计)

 2016.11 更新

  • 把原文的SIFT替换成了ORB,这样你可以在没有nonfree模块下使用本程序了。
  • 去掉了cv::cv2Eigen函数,因为有些读者找不到这个函数。
  • 检查了minDis为零的情况。
  • 之前t的访问时,行和列颠倒了,会对结果产生一定影响,现在修正了
  • 请以现在的github上源码为准。

  读者朋友们大家好,又到了我们开讲rgbd slam的时间了。由于前几天博主在忙开会拍婚纱照等一系列乱七八糟的事情,这一讲稍微做的慢了些,先向读者们道个歉!

  上几讲中,我们详细讲了两张图像间的匹配与运动估计。然而一个实际的机器人总不可能只有两个图像数据吧?那该多么寂寞呀。所以,本讲开始,我们要处理一个视频流,包含八百左右的数据啦。这才像是在做SLAM嘛!

  小萝卜:那我们去哪里下载这些数据呢?

  师兄:可以到我的百度云里去:http://yun.baidu.com/s/1i33uvw5

  因为有点大(400多M),我就没有传到git上。不然运行前四讲的代码就要下一堆东西啦。打开这个数据集,你会看到里头有 和 两个文件夹,分别是RGB图与深度图。前几讲的数据也是取自这里的哦。

  小萝卜:这算不算师兄你在偷懒呢?

  师兄:呃,这个,总、总之,我们这里暂时先用这些数据啦。它们取自nyuv2数据集:http://cs.nyu.edu/~silberman/datasets/nyu_depth_v2.html 这可是一个国际上认可的,相当有名的数据集哦。如果你想要跑自己的数据,当然也可以,不过需要你进行一些预处理啦。

  本讲中,我们利用前几讲写好的代码,完成一个视觉里程计(visual odometry)的编写。什么是视觉里程计呢?简而言之,就是把新来的数据与上一帧进行匹配,估计其运动,然后再把运动累加起来的东西。画成示意图的话,就是下面这个样子:

一起做RGB-D SLAM (5)_第1张图片 

  师兄:大家看懂了不?这实际上和滤波器很像,通过不断的两两匹配,估计机器人当前的位姿,过去的就给丢弃了。这个思路比较简单,实际当中也比较有效,能够保证局部运动的正确性。下面我们来实现一下visual odometry。

  小萝卜:道理我是明白了,可是师兄你这画风究竟是哪个年代的啊……


准备工作

  为了保证代码的简洁,我们要把以前做过的东西封装成函数,写在slamBase.cpp中,以便将来调用。(不过,由于是算法性质的内容,就不封成c++的对象了)。

  首先工具函数:将cv的旋转矢量与位移矢量转换为变换矩阵,类型为Eigen::Isometry3d; 

  src/slamBase.cpp

 1 // cvMat2Eigen
 2 Eigen::Isometry3d cvMat2Eigen( cv::Mat& rvec, cv::Mat& tvec )
 3 {
 4     cv::Mat R;
 5     cv::Rodrigues( rvec, R );
 6     Eigen::Matrix3d r;
 7     cv::cv2eigen(R, r);
 8   
 9     // 将平移向量和旋转矩阵转换成变换矩阵
10     Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
11 
12     Eigen::AngleAxisd angle(r);
13     Eigen::Translation<double,3> trans(tvec.at<double>(0,0), tvec.at<double>(0,1), tvec.at<double>(0,2));
14     T = angle;
15     T(0,3) = tvec.at<double>(0,0); 
16     T(1,3) = tvec.at<double>(0,1); 
17     T(2,3) = tvec.at<double>(0,2);
18     return T;
19 }

  另一个函数:将新的帧合并到旧的点云里:

 1 // joinPointCloud 
 2 // 输入:原始点云,新来的帧以及它的位姿
 3 // 输出:将新来帧加到原始帧后的图像
 4 PointCloud::Ptr joinPointCloud( PointCloud::Ptr original, FRAME& newFrame, Eigen::Isometry3d T, CAMERA_INTRINSIC_PARAMETERS& camera ) 
 5 {
 6     PointCloud::Ptr newCloud = image2PointCloud( newFrame.rgb, newFrame.depth, camera );
 7 
 8     // 合并点云
 9     PointCloud::Ptr output (new PointCloud());
10     pcl::transformPointCloud( *original, *output, T.matrix() );
11     *newCloud += *output;
12 
13     // Voxel grid 滤波降采样
14     static pcl::VoxelGrid voxel;
15     static ParameterReader pd;
16     double gridsize = atof( pd.getData("voxel_grid").c_str() );
17     voxel.setLeafSize( gridsize, gridsize, gridsize );
18     voxel.setInputCloud( newCloud );
19     PointCloud::Ptr tmp( new PointCloud() );
20     voxel.filter( *tmp );
21     return tmp;
22 }

  另外,在parameters.txt中,我们增加了几个参数,以便调节程序的性能:

# part 5 
# 数据相关
# 起始与终止索引
start_index=1
end_index=700
# 数据所在目录
rgb_dir=../data/rgb_png/
rgb_extension=.png
depth_dir=../data/depth_png/
depth_extension=.png
# 点云分辨率
voxel_grid=0.02
# 是否实时可视化
visualize_pointcloud=yes
# 最小匹配数量
min_good_match=10
# 最小内点
min_inliers=5
# 最大运动误差
max_norm=0.3

  前面几个参数是相当直观的:指定RGB图与深度图所在的目录,起始与终止的图像索引(也就是第1张到第700张的slam啦)。后面几个参数,会在后面进行解释。


 

实现VO

  最后,利用之前写好的工具函数,实现一个VO:

  src/visualOdometry.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月01日 星期六 15时35分42秒
  6  ************************************************************************/
  7 
  8 #include 
  9 #include 
 10 #include 
 11 using namespace std;
 12 
 13 #include "slamBase.h"
 14 
 15 // 给定index,读取一帧数据
 16 FRAME readFrame( int index, ParameterReader& pd );
 17 // 度量运动的大小
 18 double normofTransform( cv::Mat rvec, cv::Mat tvec );
 19 
 20 int main( int argc, char** argv )
 21 {
 22     ParameterReader pd;
 23     int startIndex  =   atoi( pd.getData( "start_index" ).c_str() );
 24     int endIndex    =   atoi( pd.getData( "end_index"   ).c_str() );
 25 
 26     // initialize
 27     cout<<"Initializing ..."<<endl;
 28     int currIndex = startIndex; // 当前索引为currIndex
 29     FRAME lastFrame = readFrame( currIndex, pd ); // 上一帧数据
 30     // 我们总是在比较currFrame和lastFrame
 31     string detector = pd.getData( "detector" );
 32     string descriptor = pd.getData( "descriptor" );
 33     CAMERA_INTRINSIC_PARAMETERS camera = getDefaultCamera();
 34     computeKeyPointsAndDesp( lastFrame, detector, descriptor );
 35     PointCloud::Ptr cloud = image2PointCloud( lastFrame.rgb, lastFrame.depth, camera );
 36     
 37     pcl::visualization::CloudViewer viewer("viewer");
 38 
 39     // 是否显示点云
 40     bool visualize = pd.getData("visualize_pointcloud")==string("yes");
 41 
 42     int min_inliers = atoi( pd.getData("min_inliers").c_str() );
 43     double max_norm = atof( pd.getData("max_norm").c_str() );
 44 
 45     for ( currIndex=startIndex+1; currIndex )
 46     {
 47         cout<<"Reading files "<endl;
 48         FRAME currFrame = readFrame( currIndex,pd ); // 读取currFrame
 49         computeKeyPointsAndDesp( currFrame, detector, descriptor );
 50         // 比较currFrame 和 lastFrame
 51         RESULT_OF_PNP result = estimateMotion( lastFrame, currFrame, camera );
 52         if ( result.inliers < min_inliers ) //inliers不够,放弃该帧
 53             continue;
 54         // 计算运动范围是否太大
 55         double norm = normofTransform(result.rvec, result.tvec);
 56         cout<<"norm = "<endl;
 57         if ( norm >= max_norm )
 58             continue;
 59         Eigen::Isometry3d T = cvMat2Eigen( result.rvec, result.tvec );
 60         cout<<"T="<endl;
 61         
 62         //cloud = joinPointCloud( cloud, currFrame, T.inverse(), camera );
 63         cloud = joinPointCloud( cloud, currFrame, T, camera );
 64         
 65         if ( visualize == true )
 66             viewer.showCloud( cloud );
 67 
 68         lastFrame = currFrame;
 69     }
 70 
 71     pcl::io::savePCDFile( "data/result.pcd", *cloud );
 72     return 0;
 73 }
 74 
 75 FRAME readFrame( int index, ParameterReader& pd )
 76 {
 77     FRAME f;
 78     string rgbDir   =   pd.getData("rgb_dir");
 79     string depthDir =   pd.getData("depth_dir");
 80     
 81     string rgbExt   =   pd.getData("rgb_extension");
 82     string depthExt =   pd.getData("depth_extension");
 83 
 84     stringstream ss;
 85     ss<rgbExt;
 86     string filename;
 87     ss>>filename;
 88     f.rgb = cv::imread( filename );
 89 
 90     ss.clear();
 91     filename.clear();
 92     ss<depthExt;
 93     ss>>filename;
 94 
 95     f.depth = cv::imread( filename, -1 );
 96     return f;
 97 }
 98 
 99 double normofTransform( cv::Mat rvec, cv::Mat tvec )
100 {
101     return fabs(min(cv::norm(rvec), 2*M_PI-cv::norm(rvec)))+ fabs(cv::norm(tvec));
102 }

  其实一个VO也就一百行的代码,相信大家很快就能读懂的。我们稍加解释。

  • FRAME readFrame( int index, ParameterReader& pd ) 是读取帧数据的函数。告诉它我要读第几帧的数据,它就会乖乖的把数据给找出来,返回一个FRAME结构体。
  • 在得到匹配之后,我们判断了匹配是否成功,并把失败的数据丢弃。为什么这样做呢?因为之前的算法,对于任意两张图像都能做出一个结果。对于无关的图像,就明显是不对的。所以要去除匹配失败的情形。
  • 如何检测匹配失败呢?我们采用了三个方法:
    1. 去掉goodmatch太少的帧,最少的goodmatch定义为:
      min_good_match=10
    2. 去掉solvePnPRASNAC里,inlier较少的帧,同理定义为:
      min_inliers=5
    3. 去掉求出来的变换矩阵太大的情况。因为假设运动是连贯的,两帧之间不会隔的太远:
      max_norm=0.3

  如何知道两帧之间不隔太远呢?我们计算了一个度量运动大小的值:$\| \Delta t \| + \min ( 2 \pi - \| r\|, \| r \|)$。它可以看成是位移与旋转的范数加和。当这个数大于阈值max_norm时,我们就认为匹配出错了。

  经过这三道工序处理后,vo的结果基本能保持正确啦。下面是一个gif图片:

一起做RGB-D SLAM (5)_第2张图片

  小萝卜:师兄!这效果相当不错啊!

  师兄:嗯,至少有点儿像样啦,虽然问题还是挺多的。具体有哪些问题呢?我们留到下一讲里再说。各位同学也可以运行一下自己的代码,看看结果哦。


 tips:

  1. 当点云出现时,可按5显示颜色,然后按r重置视角,快速查看点云;
  2. 可以调节parameters.txt中的voxel_grid值来设置点云分辨率。0.01表示每1$cm^3$的格子里有一个点。

课后作业

  请观察vo的运行状态并尝试不同参数,总结它有哪些局限性。

  本讲代码: https://github.com/gaoxiang12/rgbd-slam-tutorial-gx/tree/master/part%20V 数据链接见前面百度盘。

 


TIPS

  • 如果在编译时期出现Link错误,请检查你是否链接到了PCL的visualization模块。如果实在不确定,就照着github源码抄一遍。
  • 在运动时期,由于存在两张图像完全一样的情况,导致匹配时距离为0。由于本节程序的设置,这种情况会被当成没有匹配,导致VO丢失。请你自己fix一下这个bug,我在下一节当中也进行了检查。

你可能感兴趣的:(一起做RGB-D SLAM (5))