最后，利用之前写好的工具函数，实现一个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
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<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也就一百行的代码，相信大家很快就能读懂的。我们稍加解释。
FRAME readFrame( int index, ParameterReader& pd ) 是读取帧数据的函数。告诉它我要读第几帧的数据，它就会乖乖的把数据给找出来，返回一个FRAME结构体。
在得到匹配之后，我们判断了匹配是否成功，并把失败的数据丢弃。为什么这样做呢？因为之前的算法，对于任意两张图像都能做出一个结果。对于无关的图像，就明显是不对的。所以要去除匹配失败的情形。
如何检测匹配失败呢？我们采用了三个方法：
去掉goodmatch太少的帧，最少的goodmatch定义为：
min_good_match=10
去掉solvePnPRASNAC里，inlier较少的帧，同理定义为：
min_inliers=5
去掉求出来的变换矩阵太大的情况。因为假设运动是连贯的，两帧之间不会隔的太远：
max_norm=0.3
