一塊兒作RGB-D SLAM 第二季（二）

時間 2019-12-09

標籤一塊兒 rgb slam 第二欄目 CSS 简体版

原文原文鏈接

本節目標

　　咱們要實現一個基本的文件IO，用於讀取TUM數據集中的圖像。順帶的，還要作一個參數文件的讀取。python

設計參數文件讀取的類：ParameterReader　　

　　首先，咱們來作一個參數讀取的類。該類讀取一個記錄各類參數文本文件，例如數據集所在目錄等。程序其餘部分要用到參數時，能夠今後類得到。這樣，之後調參數時只需調整參數文件，而不用從新編譯整個程序，能夠節省調試時間。linux

　　這種事情有點像在造輪子。可是既然我們本身作slam自己就是在造輪子，那就索性造個痛快吧！ios

　　參數文件通常是用yaml或xml來寫的。不過爲了保持簡潔，咱們就本身來設計這個文件的簡單語法吧。一個參數文件大概長這樣：c++

# 這是一個參數文件
# 這雖然只是個參數文件，可是是很厲害的呢！
# 去你妹的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：ide

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特徵的提取與匹配，並測試它的匹配速度與性能。

問題

　　若是你有任何問題，請寫在評論區中。有表明性的問題我會統一回復。

相關標籤/搜索

每日一句

每一个你不满意的现在，都有一个你没有努力的曾经。

一塊兒作RGB-D SLAM 第二季 （二）

本節目標

設計參數文件讀取的類：ParameterReader

設計RGBDFrame類：

測試FrameReader

一塊兒作RGB-D SLAM 第二季（二）

設計參數文件讀取的類：ParameterReader　　