一起做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)的编写。什么是视觉里程计呢？简而言之，就是把新来的数据与上一帧进行匹配，估计其运动，然后再把运动累加起来的东西。画成示意图的话，就是下面这个样子：

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

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

准备工作

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

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

　　src/slamBase.cpp

 // cvMat2Eigen

 Eigen::Isometry3d cvMat2Eigen( cv::Mat& rvec, cv::Mat& tvec )

 {

     cv::Mat R;

     cv::Rodrigues( rvec, R );

     Eigen::Matrix3d r;

     cv::cv2eigen(R, r);

     // 将平移向量和旋转矩阵转换成变换矩阵

     Eigen::Isometry3d T = Eigen::Isometry3d::Identity();

     Eigen::AngleAxisd angle(r);

     Eigen::Translation<double,> trans(tvec.at<double>(,), tvec.at<double>(,), tvec.at<double>(,));

     T = angle;

     T(,) = tvec.at<double>(,);

     T(,) = tvec.at<double>(,);

     T(,) = tvec.at<double>(,);

     return T;

 }

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

 // joinPointCloud

 // 输入：原始点云，新来的帧以及它的位姿

 // 输出：将新来帧加到原始帧后的图像

 PointCloud::Ptr joinPointCloud( PointCloud::Ptr original, FRAME& newFrame, Eigen::Isometry3d T, CAMERA_INTRINSIC_PARAMETERS& camera )

 {

     PointCloud::Ptr newCloud = image2PointCloud( newFrame.rgb, newFrame.depth, camera );

     // 合并点云

     PointCloud::Ptr output (new PointCloud());

     pcl::transformPointCloud( *original, *output, T.matrix() );

     *newCloud += *output;

     // Voxel grid 滤波降采样

     static pcl::VoxelGrid<PointT> voxel;

     static ParameterReader pd;

     double gridsize = atof( pd.getData("voxel_grid").c_str() );

     voxel.setLeafSize( gridsize, gridsize, gridsize );

     voxel.setInputCloud( newCloud );

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

     voxel.filter( *tmp );

     return tmp;

 }

　　另外，在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

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

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

     > Author: xiang gao

     > Mail: gaoxiang12@mails.tsinghua.edu.cn

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

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

 #include <iostream>

 #include <fstream>

 #include <sstream>

 using namespace std;

 #include "slamBase.h"

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

 FRAME readFrame( int index, ParameterReader& pd );

 // 度量运动的大小

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

 int main( int argc, char** argv )

 {

     ParameterReader pd;

     int startIndex  =   atoi( pd.getData( "start_index" ).c_str() );

     int endIndex    =   atoi( pd.getData( "end_index"   ).c_str() );

     // initialize

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

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

     FRAME lastFrame = readFrame( currIndex, pd ); // 上一帧数据

     // 我们总是在比较currFrame和lastFrame

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

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

     CAMERA_INTRINSIC_PARAMETERS camera = getDefaultCamera();

     computeKeyPointsAndDesp( lastFrame, detector, descriptor );

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

     pcl::visualization::CloudViewer viewer("viewer");

     // 是否显示点云

     bool visualize = pd.getData("visualize_pointcloud")==string("yes");

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

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

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

     {

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

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

         computeKeyPointsAndDesp( currFrame, detector, descriptor );

         // 比较currFrame 和 lastFrame

         RESULT_OF_PNP result = estimateMotion( lastFrame, currFrame, camera );

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

             continue;

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

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

         cout<<"norm = "<<norm<<endl;

         if ( norm >= max_norm )

             continue;

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

         cout<<"T="<<T.matrix()<<endl;

         //cloud = joinPointCloud( cloud, currFrame, T.inverse(), camera );

         cloud = joinPointCloud( cloud, currFrame, T, camera );

         if ( visualize == true )

             viewer.showCloud( cloud );

         lastFrame = currFrame;

     }

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

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

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

 }

　　其实一个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图片：

　　小萝卜：师兄！这效果相当不错啊！

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

tips：

当点云出现时，可按5显示颜色，然后按r重置视角，快速查看点云；
可以调节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，我在下一节当中也进行了检查。