首页 > 嗟来之食 > 一起做RGBD SLAM 第二季 (二)
2016
05-16

一起做RGBD SLAM 第二季 (二)

本节目标
  我们要实现一个基本的文件IO,用于读取TUM数据集中的图像。顺带的,还要做一个参数文件的读取。

设计参数文件读取的类:ParameterReader  
  首先,我们来做一个参数读取的类。该类读取一个记录各种参数文本文件,例如数据集所在目录等。程序其他部分要用到参数时,可以从此类获得。这样,以后调参数时只需调整参数文件,而不用重新编译整个程序,可以节省调试时间。
  这种事情有点像在造轮子。但是既然咱们自己做slam本身就是在造轮子,那就索性造个痛快吧!
  参数文件一般是用yaml或xml来写的。不过为了保持简洁,我们就自己来设计这个文件的简单语法吧。一个参数文件大概长这样:

# 这是一个参数文件
# 这虽然只是个参数文件,但是是很厉害的呢!
# 去你妹的yaml! 我再也不用yaml了!简简单单多好!

# 数据相关
# 起始索引
start_index=1
# 数据所在目录
data_source=/home/xiang/Documents/data/rgbd_dataset_freiburg1_room/

# 相机内参

camera.cx=318.6
camera.cy=255.3
camera.fx=517.3
camera.fy=516.5
camera.scale=5000.0
camera.d0=0.2624
camera.d1=-0.9531
camera.d2=-0.0054
camera.d3=0.0026
camera.d4=1.1633

parameters.txt
  语法很简单,以行为单位,以#开头至末尾的都是注释。参数的名称与值用等号相连,即 名称=值 ,很容易吧!下面我们做一个ParameterReader类,来读取这个文件。
  在此之前,先新建一个 include/common.h 文件,把一些常用的头文件和结构体放到此文件中,省得以后写代码前面100行都是#include:
include/common.h:

1 #ifndef COMMON_H
2 #define COMMON_H
3
4 /**
5 * common.h
6 * 定义一些常用的结构体
7 * 以及各种可能用到的头文件,放在一起方便include
8 */
9
10 // C++标准库
11 #include <iostream>
12 #include <fstream>
13 #include <vector>
14 #include <map>
15 #include <string>
16 using namespace std;
17
18
19 // Eigen
20 #include <Eigen/Core>
21 #include <Eigen/Geometry>
22
23 // OpenCV
24 #include <opencv2/core/core.hpp>
25 #include <opencv2/highgui/highgui.hpp>
26 #include <opencv2/calib3d/calib3d.hpp>
27
28 // boost
29 #include <boost/format.hpp>
30 #include <boost/timer.hpp>
31 #include <boost/lexical_cast.hpp>
32
33 namespace rgbd_tutor
34 {
35
36 // 相机内参模型
37 // 增加了畸变参数,虽然可能不会用到
38 struct CAMERA_INTRINSIC_PARAMETERS
39 {
40 // 标准内参
41 double cx=0, cy=0, fx=0, fy=0, scale=0;
42 // 畸变因子
43 double d0=0, d1=0, d2=0, d3=0, d4=0;
44 };
45
46
47
48 // linux终端的颜色输出
49 #define RESET "\033[0m"
50 #define BLACK "\033[30m" /* Black */
51 #define RED "\033[31m" /* Red */
52 #define GREEN "\033[32m" /* Green */
53 #define YELLOW "\033[33m" /* Yellow */
54 #define BLUE "\033[34m" /* Blue */
55 #define MAGENTA "\033[35m" /* Magenta */
56 #define CYAN "\033[36m" /* Cyan */
57 #define WHITE "\033[37m" /* White */
58 #define BOLDBLACK "\033[1m\033[30m" /* Bold Black */
59 #define BOLDRED "\033[1m\033[31m" /* Bold Red */
60 #define BOLDGREEN "\033[1m\033[32m" /* Bold Green */
61 #define BOLDYELLOW "\033[1m\033[33m" /* Bold Yellow */
62 #define BOLDBLUE "\033[1m\033[34m" /* Bold Blue */
63 #define BOLDMAGENTA "\033[1m\033[35m" /* Bold Magenta */
64 #define BOLDCYAN "\033[1m\033[36m" /* Bold Cyan */
65 #define BOLDWHITE "\033[1m\033[37m" /* Bold White */
66
67
68 }
69
70 #endif // COMMON_H

common.h
  嗯,请注意我们使用rgbd_tutor作为命名空间,以后所有类都位于这个空间里。然后,文件里还定义了相机内参的结构,这个结构我们之后会用到,先放在这儿。接下来是include/parameter_reader.h:

1 #ifndef PARAMETER_READER_H
2 #define PARAMETER_READER_H
3
4 #include "common.h"
5
6 namespace rgbd_tutor
7 {
8
9 class ParameterReader
10 {
11 public:
12 // 构造函数:传入参数文件的路径
13 ParameterReader( const string& filename = "./parameters.txt" )
14 {
15 ifstream fin( filename.c_str() );
16 if (!fin)
17 {
18 // 看看上级目录是否有这个文件 ../parameter.txt
19 fin.open("."+filename);
20 if (!fin)
21 {
22 cerr<<"没有找到对应的参数文件:"<<filename<<endl;
23 return;
24 }
25 }
26
27 // 从参数文件中读取信息
28 while(!fin.eof())
29 {
30 string str;
31 getline( fin, str );
32 if (str[0] == '#')
33 {
34 // 以&lsquo;#&rsquo;开头的是注释
35 continue;
36 }
37 int pos = str.find('#');
38 if (pos != -1)
39 {
40 //从井号到末尾的都是注释
41 str = str.substr(0, pos);
42 }
43
44 // 查找等号
45 pos = str.find("=");
46 if (pos == -1)
47 continue;
48 // 等号左边是key,右边是value
49 string key = str.substr( 0, pos );
50 string value = str.substr( pos+1, str.length() );
51 data[key] = value;
52
53 if ( !fin.good() )
54 break;
55 }
56 }
57
58 // 获取数据
59 // 由于数据类型不确定,写成模板
60 template< class T >
61 T getData( const string& key ) const
62 {
63 auto iter = data.find(key);
64 if (iter == data.end())
65 {
66 cerr<<"Parameter name "<<key<<" not found!"<<endl;
67 return boost::lexical_cast<T>( "" );
68 }
69 // boost 的 lexical_cast 能把字符串转成各种 c++ 内置类型
70 return boost::lexical_cast<T>( iter->second );
71 }
72
73 // 直接返回读取到的相机内参
74 rgbd_tutor::CAMERA_INTRINSIC_PARAMETERS getCamera() const
75 {
76 static rgbd_tutor::CAMERA_INTRINSIC_PARAMETERS camera;
77 camera.fx = this->getData<double>("camera.fx");
78 camera.fy = this->getData<double>("camera.fy");
79 camera.cx = this->getData<double>("camera.cx");
80 camera.cy = this->getData<double>("camera.cy");
81 camera.d0 = this->getData<double>("camera.d0");
82 camera.d1 = this->getData<double>("camera.d1");
83 camera.d2 = this->getData<double>("camera.d2");
84 camera.d3 = this->getData<double>("camera.d3");
85 camera.d4 = this->getData<double>("camera.d4");
86 camera.scale = this->getData<double>("camera.scale");
87 return camera;
88 }
89
90 protected:
91 map<string, string> data;
92 };
93
94 };
95
96 #endif // PARAMETER_READER_H

parameter_reader.h
  为保持简单,我把实现也放到了类中。该类的构造函数里,传入参数文件所在的路径。在我们的代码里,parameters.txt位于代码根目录下。不过,如果找不到文件,我们也会在上一级目录中寻找一下,这是由于qtcreator在运行程序时默认使用程序所在的目录(./bin)而造成的。
  ParameterReader 实际存储的数据都是std::string类型(字符串),在需要转换为其他类型时,我们用 boost::lexical_cast 进行转换。
  ParameterReader::getData 函数返回一个参数的值。它有一个模板参数,你可以这样使用它:
  double d = parameterReader.getData<double>("d");
  如果找不到参数,则返回一个空值。
  最后,我们还用了一个函数返回相机的内参,这纯粹是为了外部类调用更方便。

设计RGBDFrame类:
  程序运行的基本单位是Frame,而我们从数据集中读取的数据也是以Frame为单位的。现在我们来设计一个RGBDFrame类,以及向数据集读取Frame的FrameReader类。
  我们把这两个类都放在 include/rgbdframe.h 中,如下所示(为了显示方便就都贴上来了):

1 #ifndef RGBDFRAME_H
2 #define RGBDFRAME_H
3
4 #include "common.h"
5 #include "parameter_reader.h"
6
7 #include"Thirdparty/DBoW2/DBoW2/FORB.h"
8 #include"Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h"
9
10 namespace rgbd_tutor{
11
12 //帧
13 class RGBDFrame
14 {
15 public:
16 typedef shared_ptr<RGBDFrame> Ptr;
17
18 public:
19 RGBDFrame() {}
20 // 方法
21 // 给定像素点,求3D点坐标
22 cv::Point3f project2dTo3dLocal( const int& u, const int& v ) const
23 {
24 if (depth.data == nullptr)
25 return cv::Point3f();
26 ushort d = depth.ptr<ushort>(v)[u];
27 if (d == 0)
28 return cv::Point3f();
29 cv::Point3f p;
30 p.z = double( d ) / camera.scale;
31 p.x = ( u – camera.cx) * p.z / camera.fx;
32 p.y = ( v – camera.cy) * p.z / camera.fy;
33 return p;
34 }
35
36 public:
37 // 数据成员
38 int id =-1; //-1表示该帧不存在
39
40 // 彩色图和深度图
41 cv::Mat rgb, depth;
42 // 该帧位姿
43 // 定义方式为:x_local = T * x_world 注意也可以反着定义;
44 Eigen::Isometry3d T=Eigen::Isometry3d::Identity();
45
46 // 特征
47 vector<cv::KeyPoint> keypoints;
48 cv::Mat descriptor;
49 vector<cv::Point3f> kps_3d;
50
51 // 相机
52 // 默认所有的帧都用一个相机模型(难道你还要用多个吗?)
53 CAMERA_INTRINSIC_PARAMETERS camera;
54
55 // BoW回环特征
56 // 讲BoW时会用到,这里先请忽略之
57 DBoW2::BowVector bowVec;
58
59 };
60
61 // FrameReader
62 // 从TUM数据集中读取数据的类
63 class FrameReader
64 {
65 public:
66 FrameReader( const rgbd_tutor::ParameterReader& para )
67 : parameterReader( para )
68 {
69 init_tum( );
70 }
71
72 // 获得下一帧
73 RGBDFrame::Ptr next();
74
75 // 重置index
76 void reset()
77 {
78 cout<<"重置 frame reader"<<endl;
79 currentIndex = start_index;
80 }
81
82 // 根据index获得帧
83 RGBDFrame::Ptr get( const int& index )
84 {
85 if (index < 0 || index >= rgbFiles.size() )
86 return nullptr;
87 currentIndex = index;
88 return next();
89 }
90
91 protected:
92 // 初始化tum数据集
93 void init_tum( );
94 protected:
95
96 // 当前索引
97 int currentIndex =0;
98 // 起始索引
99 int start_index =0;
100
101 const ParameterReader& parameterReader;
102
103 // 文件名序列
104 vector<string> rgbFiles, depthFiles;
105
106 // 数据源
107 string dataset_dir;
108
109 // 相机内参
110 CAMERA_INTRINSIC_PARAMETERS camera;
111 };
112
113 };
114 #endif // RGBDFRAME_H

include/rgbdframe.h
  关于RGBDFrame类的几点注释:

我们把这个类的指针定义成了shared_ptr,以后尽量使用这个指针管理此类的对象,这样可以免出一些变量作用域的问题。并且,智能指针可以自己去delete,不容易出现问题。
我们把与这个Frame相关的东西都放在此类的成员中,例如图像、特征、对应的相机模型、BoW参数等。关于特征和BoW,我们之后要详细讨论,这里你可以暂时不去管它们。
最后,project2dTo3dLocal 可以把一个像素坐标转换为当前Frame下的3D坐标。当然前提是深度图里探测到了深度点。

  接下来,来看FrameReader。它的构造函数中需要有一个parameterReader的引用,因为我们需要去参数文件里查询数据所在的目录。如果查询成功,它会做一些初始化的工作,然后外部类就可以通过next()函数得到下一帧的图像了。我们在src/rgbdframe.cpp中实现init_tum()和next()这两个函数:

1 #include "rgbdframe.h"
2 #include "common.h"
3 #include "parameter_reader.h"
4
5 using namespace rgbd_tutor;
6
7 RGBDFrame::Ptr FrameReader::next()
8 {
9 if (currentIndex < start_index || currentIndex >= rgbFiles.size())
10 return nullptr;
11
12 RGBDFrame::Ptr frame (new RGBDFrame);
13 frame->id = currentIndex;
14 frame->rgb = cv::imread( dataset_dir + rgbFiles[currentIndex]);
15 frame->depth = cv::imread( dataset_dir + depthFiles[currentIndex], -1);
16
17 if (frame->rgb.data == nullptr || frame->depth.data==nullptr)
18 {
19 // 数据不存在
20 return nullptr;
21 }
22
23 frame->camera = this->camera;
24 currentIndex ++;
25 return frame;
26 }
27
28 void FrameReader::init_tum( )
29 {
30 dataset_dir = parameterReader.getData<string>("data_source");
31 string associate_file = dataset_dir+"/associate.txt";
32 ifstream fin(associate_file.c_str());
33 if (!fin)
34 {
35 cerr<<"找不着assciate.txt啊!在tum数据集中这尼玛是必须的啊!"<<endl;
36 cerr<<"请用python assicate.py rgb.txt depth.txt > associate.txt生成一个associate文件,再来跑这个程序!"<<endl;
37 return;
38 }
39
40 while( !fin.eof() )
41 {
42 string rgbTime, rgbFile, depthTime, depthFile;
43 fin>>rgbTime>>rgbFile>>depthTime>>depthFile;
44 if ( !fin.good() )
45 {
46 break;
47 }
48 rgbFiles.push_back( rgbFile );
49 depthFiles.push_back( depthFile );
50 }
51
52 cout<<"一共找着了"<<rgbFiles.size()<<"个数据记录哦!"<<endl;
53 camera = parameterReader.getCamera();
54 start_index = parameterReader.getData<int>("start_index");
55 currentIndex = start_index;
56 }

src/rgbdframe.cpp
  可以看到,在init_tum中,我们从前一讲生成的associate.txt里获得图像信息,把文件名存储在一个vector中。然后,next()函数根据currentIndex返回对应的数据。

测试FrameReader
  现在我们来测试一下之前写的FrameReader。在experiment中添加一个reading_frame.cpp文件,测试文件是否正确读取。
experiment/reading_frame.cpp

1 #include "rgbdframe.h"
2
3 using namespace rgbd_tutor;
4 int main()
5 {
6 ParameterReader para;
7 FrameReader fr(para);
8 while( RGBDFrame::Ptr frame = fr.next() )
9 {
10 cv::imshow( "image", frame->rgb );
11 cv::waitKey(1);
12 }
13
14 return 0;
15 }

  由于之前定义好了接口,这部分就很简单,几乎不需要解释了。我们只是把数据从文件中读取出来,加以显示而已。
  下面我们来写编译此程序所用的CMakeLists。
  代码根目录下的CMakeLists.txt:

1 cmake_minimum_required( VERSION 2.8 )
2 project( rgbd-slam-tutor2 )
3
4 # 设置用debug还是release模式。debug允许断点,而release更快
5 #set( CMAKE_BUILD_TYPE Debug )
6 set( CMAKE_BUILD_TYPE Release )
7
8 # 设置编译选项
9 # 允许c++11标准、O3优化、多线程。match选项可避免一些cpu上的问题
10 set( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -march=native -O3 -pthread" )
11
12 # 常见依赖库:cv, eigen, pcl
13 find_package( OpenCV REQUIRED )
14 find_package( Eigen3 REQUIRED )
15 find_package( PCL 1.7 REQUIRED )
16
17 include_directories(
18 ${PCL_INCLUDE_DIRS}
19 ${PROJECT_SOURCE_DIR}/
20 )
21
22 set( thirdparty_libs
23 ${OpenCV_LIBS}
24 ${PCL_LIBRARY_DIRS}
25 ${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/libDBoW2.so
26 )
27
28 add_definitions(${PCL_DEFINITIONS})
29
30 # 二进制文件输出到bin
31 set( EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin )
32 # 库输出到lib
33 set( CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib )
34
35 # 头文件目录
36 include_directories(
37 ${PROJECT_SOURCE_DIR}/include
38 )
39
40 # 源文件目录
41 add_subdirectory( ${PROJECT_SOURCE_DIR}/src/ )
42 add_subdirectory( ${PROJECT_SOURCE_DIR}/experiment/ )

CMakeLists.txt:
  src/目录下的CMakeLists.txt:

1 add_library( rgbd_tutor
2 rgbdframe.cpp
3 )

  experiment下的CMakeLists.txt

1 add_executable( helloslam helloslam.cpp )
2
3 add_executable( reading_frame reading_frame.cpp )
4 target_link_libraries( reading_frame rgbd_tutor ${thirdparty_libs} )

  注意到,我们把rgbdframe.cpp编译成了库,然后把reading_frame链接到了这个库上。由于在RGBDFrame类中用到了DBoW库的代码,所以我们先去编译一下DBoW这个库。

1 cd Thirdparty/DBoW2
2 mkdir build lib
3 cd build
4 cmake ..
5 make -j4

  这样就把DBoW编译好了。这个库以后我们要在回环检测中用到。接下来就是编译咱们自己的程序了。如果你用qtCreator,可以直接打开根目录下的CMakeLists.txt,点击编译即可:   
  如果你不用这个IDE,遵循传统的cmake编译方式即可。编译后在bin/下面生成reading_frame程序,可以直接运行。
  运行后,你可以看到镜头在快速的运动。因为我们没做任何处理,这应该是你在电脑上能看到的最快的处理速度了(当然取决于你的配置)。随后我们要把特征提取、匹配和跟踪都加进去,但是希望它仍能保持在正常的视频速度。

下节预告
  下节我们将介绍orb特征的提取与匹配,并测试它的匹配速度与性能。

问题
  如果你有任何问题,请写在评论区中。有代表性的问题我会统一回复。

最后编辑:
作者:
这个作者貌似有点懒,什么都没有留下。

留下一个回复