本節目標
我們要實現一個基本的文件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：
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
嗯，請注意我們使用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
為保持簡單，我把實現也放到了類中。該類的構造函數里，傳入參數文件所在的路徑。在我們的代碼里，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
關於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 }
可以看到，在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/ )
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特征的提取與匹配，並測試它的匹配速度與性能。
問題
如果你有任何問題，請寫在評論區中。有代表性的問題我會統一回復。