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

　　语法很简单，以行为单位，以#开头至末尾的都是注释。参数的名称与值用等号相连，即 名称＝值 ，很容易吧！下面我们做一个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 "33[0m"
#define BLACK "33[30m" /* Black */
#define RED "33[31m" /* Red */
#define GREEN "33[32m" /* Green */
#define YELLOW "33[33m" /* Yellow */
#define BLUE "33[34m" /* Blue */
#define MAGENTA "33[35m" /* Magenta */
#define CYAN "33[36m" /* Cyan */
#define WHITE "33[37m" /* White */
#define BOLDBLACK "33[1m33[30m" /* Bold Black */
#define BOLDRED "33[1m33[31m" /* Bold Red */
#define BOLDGREEN "33[1m33[32m" /* Bold Green */
#define BOLDYELLOW "33[1m33[33m" /* Bold Yellow */
#define BOLDBLUE "33[1m33[34m" /* Bold Blue */
#define BOLDMAGENTA "33[1m33[35m" /* Bold Magenta */
#define BOLDCYAN "33[1m33[36m" /* Bold Cyan */
#define BOLDWHITE "33[1m33[37m" /* Bold White */


}

#endif // 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

 　　为保持简单，我把实现也放到了类中。该类的构造函数里，传入参数文件所在的路径。在我们的代码里，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 == 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

 　　关于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;
}

　　可以看到，在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(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：

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特征的提取与匹配，并测试它的匹配速度与性能。

---

问题

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