我们要实现一个基本的文件IO，用于读取TUM数据集中的图像。顺带的，还要做一个参数文件的读取。
首先，我们来做一个参数读取的类。该类读取一个记录各种参数文本文件，例如数据集所在目录等。程序其他部分要用到参数时，可以从此类获得。这样，以后调参数时只需调整参数文件，而不用重新编译整个程序，可以节省调试时间。
这种事情有点像在造轮子。但是既然咱们自己做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：
#ifndef COMMON_H #define COMMON_H /** * common.h * 定义一些常用的结构体 * 以及各种可能用到的头文件，放在一起方便include */ // C++标准库 #include <iostream> #include <fstream> #include <vector> #include <map> #include <string> using namespace std; // Eigen #include <Eigen/Core> #include <Eigen/Geometry> // OpenCV #include <opencv2/core/core.hpp> #include <opencv2/highgui/highgui.hpp> #include <opencv2/calib3d/calib3d.hpp> // boost #include <boost/format.hpp> #include <boost/timer.hpp> #include <boost/lexical_cast.hpp> namespace rgbd_tutor { // 相机内参模型 // 增加了畸变参数，虽然可能不会用到 struct CAMERA_INTRINSIC_PARAMETERS { // 标准内参 double cx=0, cy=0, fx=0, fy=0, scale=0; // 畸变因子 double d0=0, d1=0, d2=0, d3=0, d4=0; }; // linux终端的颜色输出 #define RESET "\033[0m" #define BLACK "\033[30m" /* Black */ #define RED "\033[31m" /* Red */ #define GREEN "\033[32m" /* Green */ #define YELLOW "\033[33m" /* Yellow */ #define BLUE "\033[34m" /* Blue */ #define MAGENTA "\033[35m" /* Magenta */ #define CYAN "\033[36m" /* Cyan */ #define WHITE "\033[37m" /* White */ #define BOLDBLACK "\033[1m\033[30m" /* Bold Black */ #define BOLDRED "\033[1m\033[31m" /* Bold Red */ #define BOLDGREEN "\033[1m\033[32m" /* Bold Green */ #define BOLDYELLOW "\033[1m\033[33m" /* Bold Yellow */ #define BOLDBLUE "\033[1m\033[34m" /* Bold Blue */ #define BOLDMAGENTA "\033[1m\033[35m" /* Bold Magenta */ #define BOLDCYAN "\033[1m\033[36m" /* Bold Cyan */ #define BOLDWHITE "\033[1m\033[37m" /* Bold White */ } #endif // COMMON_H common.h 嗯，请注意我们使用rgbd_tutor作为命名空间，以后所有类都位于这个空间里。然后，文件里还定义了相机内参的结构，这个结构我们之后会用到，先放在这儿。接下来是include/parameter_reader.h：#ifndef PARAMETER_READER_H #define PARAMETER_READER_H #include "common.h" namespace rgbd_tutor { class ParameterReader { public: // 构造函数：传入参数文件的路径 ParameterReader( const string& filename = "./parameters.txt" ) { ifstream fin( filename.c_str() ); if (!fin) { // 看看上级目录是否有这个文件 ../parameter.txt fin.open("."+filename); if (!fin) { cerr<<"没有找到对应的参数文件："<<filename<<endl; return; } } // 从参数文件中读取信息 while(!fin.eof()) { string str; getline( fin, str ); if (str[0] == '#') { // 以‘＃’开头的是注释 continue; } int pos = str.find('#'); if (pos != -1) { //从井号到末尾的都是注释 str = str.substr(0, pos); } // 查找等号 pos = str.find("="); if (pos == -1) continue; // 等号左边是key，右边是value string key = str.substr( 0, pos ); string value = str.substr( pos+1, str.length() ); data[key] = value; if ( !fin.good() ) break; } } // 获取数据 // 由于数据类型不确定，写成模板 template< class T > T getData( const string& key ) const { auto iter = data.find(key); if (iter == data.end()) { cerr<<"Parameter name "<<key<<" not found!"<<endl; return boost::lexical_cast<T>( "" ); } // boost 的 lexical_cast 能把字符串转成各种 c++ 内置类型 return boost::lexical_cast<T>( iter->second ); } // 直接返回读取到的相机内参 rgbd_tutor::CAMERA_INTRINSIC_PARAMETERS getCamera() const { static rgbd_tutor::CAMERA_INTRINSIC_PARAMETERS camera; camera.fx = this->getData<double>("camera.fx"); camera.fy = this->getData<double>("camera.fy"); camera.cx = this->getData<double>("camera.cx"); camera.cy = this->getData<double>("camera.cy"); camera.d0 = this->getData<double>("camera.d0"); camera.d1 = this->getData<double>("camera.d1"); camera.d2 = this->getData<double>("camera.d2"); camera.d3 = this->getData<double>("camera.d3"); camera.d4 = this->getData<double>("camera.d4"); camera.scale = this->getData<double>("camera.scale"); return camera; } protected: map<string, string> data; }; }; #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");
如果找不到参数，则返回一个空值。
最后，我们还用了一个函数返回相机的内参，这纯粹是为了外部类调用更方便。
程序运行的基本单位是Frame，而我们从数据集中读取的数据也是以Frame为单位的。现在我们来设计一个RGBDFrame类，以及向数据集读取Frame的FrameReader类。
我们把这两个类都放在 include/rgbdframe.h 中，如下所示（为了显示方便就都贴上来了）：
#ifndef RGBDFRAME_H #define RGBDFRAME_H #include "common.h" #include "parameter_reader.h" #include"Thirdparty/DBoW2/DBoW2/FORB.h" #include"Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h" namespace rgbd_tutor{ //帧 class RGBDFrame { public: typedef shared_ptr<RGBDFrame> Ptr; public: RGBDFrame() {} // 方法 // 给定像素点，求3D点坐标 cv::Point3f project2dTo3dLocal( const int& u, const int& v ) const { if (depth.data == nullptr) return cv::Point3f(); ushort d = depth.ptr<ushort>(v)[u]; if (d == 0) return cv::Point3f(); cv::Point3f p; p.z = double( d ) / camera.scale; p.x = ( u - camera.cx) * p.z / camera.fx; p.y = ( v - camera.cy) * p.z / camera.fy; return p; } public: // 数据成员 int id =-1; //-1表示该帧不存在 // 彩色图和深度图 cv::Mat rgb, depth; // 该帧位姿 // 定义方式为：x_local = T * x_world 注意也可以反着定义； Eigen::Isometry3d T=Eigen::Isometry3d::Identity(); // 特征 vector<cv::KeyPoint> keypoints; cv::Mat descriptor; vector<cv::Point3f> kps_3d; // 相机 // 默认所有的帧都用一个相机模型（难道你还要用多个吗？） CAMERA_INTRINSIC_PARAMETERS camera; // BoW回环特征 // 讲BoW时会用到，这里先请忽略之 DBoW2::BowVector bowVec; }; // FrameReader // 从TUM数据集中读取数据的类 class FrameReader { public: FrameReader( const rgbd_tutor::ParameterReader& para ) : parameterReader( para ) { init_tum( ); } // 获得下一帧 RGBDFrame::Ptr next(); // 重置index void reset() { cout<<"重置 frame reader"<<endl; currentIndex = start_index; } // 根据index获得帧 RGBDFrame::Ptr get( const int& index ) { if (index < 0 || index >= rgbFiles.size() ) return nullptr; currentIndex = index; return next(); } protected: // 初始化tum数据集 void init_tum( ); protected: // 当前索引 int currentIndex =0; // 起始索引 int start_index =0; const ParameterReader& parameterReader; // 文件名序列 vector<string> rgbFiles, depthFiles; // 数据源 string dataset_dir; // 相机内参 CAMERA_INTRINSIC_PARAMETERS camera; }; }; #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()这两个函数：
#include "rgbdframe.h" #include "common.h" #include "parameter_reader.h" using namespace rgbd_tutor; RGBDFrame::Ptr FrameReader::next() { if (currentIndex < start_index || currentIndex >= rgbFiles.size()) return nullptr; RGBDFrame::Ptr frame (new RGBDFrame); frame->id = currentIndex; frame->rgb = cv::imread( dataset_dir + rgbFiles[currentIndex]); frame->depth = cv::imread( dataset_dir + depthFiles[currentIndex], -1); if (frame->rgb.data == nullptr || frame->depth.data==nullptr) { // 数据不存在 return nullptr; } frame->camera = this->camera; currentIndex ++; return frame; } void FrameReader::init_tum( ) { dataset_dir = parameterReader.getData<string>("data_source"); string associate_file = dataset_dir+"/associate.txt"; ifstream fin(associate_file.c_str()); if (!fin) { cerr<<"找不着assciate.txt啊！在tum数据集中这尼玛是必须的啊!"<<endl; cerr<<"请用python assicate.py rgb.txt depth.txt > associate.txt生成一个associate文件，再来跑这个程序！"<<endl; return; } while( !fin.eof() ) { string rgbTime, rgbFile, depthTime, depthFile; fin>>rgbTime>>rgbFile>>depthTime>>depthFile; if ( !fin.good() ) { break; } rgbFiles.push_back( rgbFile ); depthFiles.push_back( depthFile ); } cout<<"一共找着了"<<rgbFiles.size()<<"个数据记录哦！"<<endl; camera = parameterReader.getCamera(); start_index = parameterReader.getData<int>("start_index"); currentIndex = start_index; } src/rgbdframe.cpp可以看到，在init_tum中，我们从前一讲生成的associate.txt里获得图像信息，把文件名存储在一个vector中。然后，next()函数根据currentIndex返回对应的数据。
现在我们来测试一下之前写的FrameReader。在experiment中添加一个reading_frame.cpp文件，测试文件是否正确读取。
experiment/reading_frame.cpp
#include "rgbdframe.h" using namespace rgbd_tutor; int main() { ParameterReader para; FrameReader fr(para); while( RGBDFrame::Ptr frame = fr.next() ) { cv::imshow( "image", frame->rgb ); cv::waitKey(1); } return 0; }由于之前定义好了接口，这部分就很简单，几乎不需要解释了。我们只是把数据从文件中读取出来，加以显示而已。
下面我们来写编译此程序所用的CMakeLists。
代码根目录下的CMakeLists.txt：
cmake_minimum_required( VERSION 2.8 ) project( rgbd-slam-tutor2 ) # 设置用debug还是release模式。debug允许断点，而release更快 #set( CMAKE_BUILD_TYPE Debug ) set( CMAKE_BUILD_TYPE Release ) # 设置编译选项 # 允许c++11标准、O3优化、多线程。match选项可避免一些cpu上的问题 set( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -march=native -O3 -pthread" ) # 常见依赖库：cv, eigen, pcl find_package( OpenCV REQUIRED ) find_package( Eigen3 REQUIRED ) find_package( PCL 1.7 REQUIRED ) include_directories( ${PCL_INCLUDE_DIRS} ${PROJECT_SOURCE_DIR}/ ) set( thirdparty_libs ${OpenCV_LIBS} ${PCL_LIBRARY_DIRS} ${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/libDBoW2.so ) add_definitions(${PCL_DEFINITIONS}) # 二进制文件输出到bin set( EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin ) # 库输出到lib set( CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib ) # 头文件目录 include_directories( ${PROJECT_SOURCE_DIR}/include ) # 源文件目录 add_subdirectory( ${PROJECT_SOURCE_DIR}/src/ ) 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特征的提取与匹配，并测试它的匹配速度与性能。
