标签：
读者朋友们大家好，又到了我们开讲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)的编写。什么是视觉里程计呢？简而言之，就是把新来的数据与上一帧进行匹配，估计其运动，然后再把运动累加起来的东西。画成示意图的话，就是下面这个样子：
师兄：大家看懂了不？这实际上和滤波器很像，通过不断的两两匹配，估计机器人当前的位姿，过去的就给丢弃了。这个思路比较简单，实际当中也比较有效，能够保证局部运动的正确性。下面我们来实现一下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<PointT> 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:
src/visualOdometry.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月01日 星期六 15时35分42秒 6 ************************************************************************/ 7 8 #include <iostream> 9 #include <fstream> 10 #include <sstream> 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<endIndex; currIndex++ ) 46 { 47 cout<<"Reading files "<<currIndex<<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 = "<<norm<<endl; 57 if ( norm >= max_norm ) 58 continue; 59 Eigen::Isometry3d T = cvMat2Eigen( result.rvec, result.tvec ); 60 cout<<"T="<<T.matrix()<<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<<rgbDir<<index<<rgbExt; 86 string filename; 87 ss>>filename; 88 f.rgb = cv::imread( filename ); 89 90 ss.clear(); 91 filename.clear(); 92 ss<<depthDir<<index<<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也就一百行的代码，相信大家很快就能读懂的。我们稍加解释。
min_good_match=10
min_inliers=5
max_norm=0.3
如何知道两帧之间不隔太远呢？我们计算了一个度量运动大小的值：$\| \Delta t \| + \min ( 2 \pi - \| r\|, \| r \|)$。它可以看成是位移与旋转的范数加和。当这个数大于阈值max_norm时，我们就认为匹配出错了。
经过这三道工序处理后，vo的结果基本能保持正确啦。下面是一个gif图片：
小萝卜：师兄！这效果相当不错啊！
师兄：嗯，至少有点儿像样啦，虽然问题还是挺多的。具体有哪些问题呢？我们留到下一讲里再说。各位同学也可以运行一下自己的代码，看看结果哦。
tips：
请观察vo的运行状态并尝试不同参数，总结它有哪些局限性。
本讲代码： https://github.com/gaoxiang12/rgbd-slam-tutorial-gx/tree/master/part%20V 数据链接见前面百度盘。
标签：
原文地址：http://www.cnblogs.com/gaoxiang12/p/4719156.html
