一起做RGB-D 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                 // 以‘＃’开头的是注释
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特征的提取与匹配，并测试它的匹配速度与性能。

问题

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

posted @ 2016-02-26 14:43 半闲居士阅读(18185) 评论(15) 编辑收藏举报

会员力量，点亮园子希望

刷新页面返回顶部

半闲居士

一起做RGB-D SLAM 第二季 （二）

本节目标

设计参数文件读取的类：ParameterReader

设计RGBDFrame类：

测试FrameReader

公告

一起做RGB-D SLAM 第二季（二）

设计参数文件读取的类：ParameterReader　　