一起做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：

 #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=, cy=, fx=, fy=, scale=;
     // 畸变因子
     double d0=, d1=, d2=, d3=, d4=;
 };

 // 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[] == '#')
             {
                 // 以‘＃’开头的是注释
                 continue;
             }
             int pos = str.find('#');
             if (pos != -)
             {
                 //从井号到末尾的都是注释
                 str = str.substr(, pos);
             }

             // 查找等号
             pos = str.find("=");
             if (pos == -)
                 continue;
             // 等号左边是key，右边是value
             string key = str.substr( , pos );
             string value = str.substr( pos+, 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");

　　如果找不到参数，则返回一个空值。

　　最后，我们还用了一个函数返回相机的内参，这纯粹是为了外部类调用更方便。

---

设计RGBDFrame类：

　　程序运行的基本单位是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 == )
             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表示该帧不存在

     // 彩色图和深度图
     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 <  || index >= rgbFiles.size() )
             return nullptr;
         currentIndex = index;
         return next();
     }

 protected:
     // 初始化tum数据集
     void    init_tum( );
 protected:

     // 当前索引
     int currentIndex =;
     // 起始索引
     int start_index  =;

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

     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

　　现在我们来测试一下之前写的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();
     }

     return ;
 }

　　由于之前定义好了接口，这部分就很简单，几乎不需要解释了。我们只是把数据从文件中读取出来，加以显示而已。

　　下面我们来写编译此程序所用的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：

 add_library( rgbd_tutor
     rgbdframe.cpp
 )

　　experiment下的CMakeLists.txt

 add_executable( helloslam helloslam.cpp )

 add_executable( reading_frame reading_frame.cpp )
 target_link_libraries( reading_frame rgbd_tutor ${thirdparty_libs} )

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

 cd Thirdparty/DBoW2
 mkdir build lib
 cd build
 cmake ..
 make -j4

　　这样就把DBoW编译好了。这个库以后我们要在回环检测中用到。接下来就是编译咱们自己的程序了。如果你用qtCreator，可以直接打开根目录下的CMakeLists.txt，点击编译即可： 　　

　　如果你不用这个IDE，遵循传统的cmake编译方式即可。编译后在bin/下面生成reading_frame程序，可以直接运行。

　　运行后，你可以看到镜头在快速的运动。因为我们没做任何处理，这应该是你在电脑上能看到的最快的处理速度了（当然取决于你的配置）。随后我们要把特征提取、匹配和跟踪都加进去，但是希望它仍能保持在正常的视频速度。

---

下节预告

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

---

问题

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