一起做RGB-D SLAM 第二季 (一)
小蘿蔔:師兄!過年啦!是不是很無聊啊!普通人的生活就是賺錢花錢,實在是很沒意思啊!
師兄:是啊……
小蘿蔔:他們都不懂搞科研和碼程式碼的樂趣呀!
師兄:可不是嘛……
小蘿蔔:所以今年過年,我們再做一個SLAM吧!之前寫的那個太爛了啦,我都不好意思說是我做的了!
師兄:嗯那可真是對不住你啊……
小蘿蔔:沒事!你再寫一個好一點的,我就原諒你了!寫完再請我吃飯吧!
師兄:啊,好的……
小蘿蔔:師兄你別這麼沒精神啊!加油咯!
前言
在經過了一番激烈的思想鬥爭之後呢,師兄厭倦了年假的無聊生活,開始寫《一起做RGBD SLAM》的第二季!在這一系列中,我們會討論RGBD SLAM程式中一些更深入的話題,從而編寫一個更快、更好用的程式。改進的地方大致如下:
- 多執行緒的優化:在建圖演算法計算時,定位演算法沒必要等待它結束。它們可以並行執行。
- 更好地跟蹤:選取參考幀,並對丟失情況進行處理;
- 基於外觀的迴環檢測:Appearance based loop closure;
- 八叉樹建圖:Octomap;
- 使用更快的特徵:Orb;
- 使用TUM資料集,並與標準軌跡進行比較;
- 線上的Kinect demo;
- 程式碼會寫得更像c++風格,而不是像上次的c風格;
這麼一看,其實整體上問題還是挺多的。在第二季中,我們將致力於解決這些問題,同時我們的程式也會變得相對比較複雜。鑑於很多基礎的問題我們在第一季中已經提過,本次我就不講怎麼安裝OpenCV之類的事情啦。但是,為了保證大家能理解部落格內容,我們和以往一樣,給出實現過程中的所有程式碼和資料。
程式碼請參見:https://github.com/gaoxiang12/rgbd-slam-tutor2
TUM資料集網址:http://vision.in.tum.de/data/datasets/rgbd-dataset
本系列使用TUM中的一個數據:fr1_room。讀者可以去TUM網站找,或者直接從我的百度雲裡下載: http://pan.baidu.com/s/1c1fviSS
TUM資料集的使用方法我們將在後文介紹。
關於程式碼
第二季中,我們仍使用C++和Cmake作為程式語言和框架。我使用的電腦是 Ubuntu 14.04 系統。讀者也可以自行挑選其他linux作業系統,但是我只給出在ubuntu下安裝各種工具的方式。
首先,請從github中下載這個系列用到的程式碼:
1 git clone https://github.com/gaoxiang12/rgbd-slam-tutor2.git
你會看到幾個資料夾。和第一個系列一樣,我們把不同的程式碼歸類放置。幾個資料夾的內容如下:
- bin 存放編譯好的可執行檔案;
- src 存放原始碼;
- include 存放標頭檔案;
- experiment 存放一些做實驗與測試用的原始檔;
- config 存放配置檔案;
- lib 存放編譯好的庫檔案;
- Thirdparty 一些小型的依賴庫,例如g2o,dbow2,octomap等;
第一講的程式碼還沒有那麼全。隨著講解的進行,我們會逐步將程式碼新增到各個資料夾中去。
我們構建程式碼的思路是這樣的。把與slam相關的程式碼(include和src下)編譯成一個庫,把測試用的程式(experiment下)編譯成可執行檔案,並連結到這個slam庫上。舉例來說,我們會把orb特徵的提取和匹配程式碼放到庫中,然後在experiment裡寫一個程式,讀一些具體的圖片並提取orb特徵。以後我們也將用這個方式來編寫回環檢測等模組。
至於為何要放Thirdparty呢?因為像g2o這樣的庫,版本有時會發生變化。所以我們就把它直接放到程式碼目錄裡,而不是讓讀者自己去找g2o的原始碼,這樣就可以保證我們的程式碼在讀者的電腦上也能順利編譯。但是像 opencv,pcl 這些大型又較穩定的庫,我們就交給讀者自行編譯安裝了。
除了Thirdparty下的庫,請讀者自行安裝這樣依賴庫:
- OpenCV 2.4.11 請往opencv.org下載,注意我們沒有使用3.1版本,而opencv2系列和3系列在介面上有較大差異。如果你用ubuntu,可以通過軟體倉庫來安裝opencv:
sudo apt-get install libopencv-dev
- PCL 1.7 來自pointclouds.org。
- Eigen3 安裝 sudo apt-get install libeigen3-dev
Thirdparty下的庫,多為cmake工程,所以按照通常的cmake編譯方式即可安裝。它們的依賴基本可以在ubuntu的軟體倉庫中找到, 我們會在用到時再加以介紹。
關於TUM資料集
本次我們使用tum提供的資料集。tum的資料集帶有標準的軌跡和一些比較工具,更適合用來研究。同時,相比於nyud資料集,它也要更加困難一些。使用這個資料集時應當注意它的儲存格式(當然使用任何資料集都應當注意)。
下面我們以fr1_room為例來說明TUM資料集的用法。fr1_room的下載方式見上面的百度雲或者TUM官網。
下載我們提供的 “rgbd_dataset_freiburg1_room.tgz”至任意目錄,解壓後像這樣:
rgb和depth資料夾下存放著彩色圖和深度圖。影象的檔名是以採集時間命名的。而rgb.txt和depth.txt則儲存了所有影象的採集時間和檔名稱,例如:
1305031910.765238 rgb/1305031910.765238.png
表示在機器時間1305031910.765238採集了一張RGB影象,存放於rgb/1305031910.765238.png中。
這種儲存方式的一個特點是,沒有直接的rgb-depth一一對應關係。由於採集時間的差異,幾乎沒有兩張影象是同一個時刻採集的。然而,我們在處理影象時,需要把一個RGB和一個depth當成一對來處理。所以,我們需要一步預處理,找到rgb和depth影象的一一對應關係。
TUM為我們提供了一個工具來做這件事,詳細的說明請看:http://vision.in.tum.de/data/datasets/rgbd-dataset/tools 該網頁整理了一些常用工具,包括時間配對,ground-truth誤差比對、影象到點雲的轉換等。對於現在預處理這一步,我們需要的是一個 associate.py 檔案,如下(你可以直接把內容拷下來,存成本地的associate.py檔案):
associate.py
小蘿蔔:那麼這個檔案要怎麼用呢?
如果讀者熟悉python,就很容易看懂它的用法。實際上,只要給它兩個檔名即可,它會輸出一個匹配好的序列,像這樣:
python associate.py rgb.txt depth.txt
輸出則是一行一行的資料,如:
1305031955.536891 rgb/1305031955.536891.png 1305031955.552015 depth/1305031955.552015.png
小蘿蔔:我知道!這一行就是配對好的RGB圖和深度圖了,對吧!
師兄:對!程式預設時間差在0.02內的就可以當成一對影象。為了儲存這個結果,我們可以把它輸出到一個檔案中去,如:
python associate.py rgb.txt depth.txt > associate.txt
這樣,只要有了這個associate.txt檔案,我們就可以找到一對對的RGB和彩色圖啦!
小蘿蔔:配對配對什麼的,總覺得像在相親啊……
關於ground truth
ground truth是TUM資料集提供的標準軌跡,它是由一個外部的(很高階的)運動捕捉裝置測量的,基本上你可以把它當成一個標準答案嘍!ground truth的記錄格式也和前面類似,像這樣:
1305031907.2496 -0.0730 -0.4169 1.5916 0.8772 -0.1170 0.0666 -0.4608
各個資料分別是:時間,位置(x,y,z),姿態四元數(qx, qy, qz, qw),對四元數不熟悉的同學可以看看“數學基礎”那幾篇部落格。那麼這個軌跡長什麼樣呢?我們寫個小指令碼來畫個圖看看:
#!/usr/bin/env python # coding=utf-8 import numpy as np import matplotlib.pyplot as plt import mpl_toolkits.mplot3d f = open("./groundtruth.txt") x = [] y = [] z = [] for line in f: if line[0] == '#': continue data = line.split() x.append( float(data[1] ) ) y.append( float(data[2] ) ) z.append( float(data[3] ) ) ax = plt.subplot( 111, projection='3d') ax.plot(x,y,z) plt.show()
把這部分程式碼複製儲存成draw_groundtruth.py存放到資料目錄中,再執行:
python draw_groundtruth.py
就能看到軌跡的形狀啦:
第二件事,因為外部那個運動捕捉裝置的記錄頻率比較高,得到的軌跡點也比影象密集很多,如何查詢每個影象的真實位置呢?
還記得associate.py不?我們可以用同樣的方式來匹配associate.txt和groundtruth.txt中的時間資訊哦:
python associate.py associate.txt groundtruth.txt > associate_with_groundtruth.txt
這時,我們的新檔案 associate_with_groundtruth.txt 中就含有每個幀的位姿資訊了:
1305031910.765238 rgb/1305031910.765238.png 1305031910.771502 depth/1305031910.771502.png 1305031910.769500 -0.8683 0.6026 1.5627 0.8219 -0.3912 0.1615 -0.3811
是不是很方便呢?對於TUM中其他的序列也可以同樣處理。
關於TUM中的相機
TUM資料集一共用了三個機器人,記成fr1, fr2, fr3。這三臺相機的引數在這裡: http://vision.in.tum.de/data/datasets/rgbd-dataset/file_formats#intrinsic_camera_calibration_of_the_kinect
資料當中,深度圖已經根據內參向RGB作了調整。所以相機內參以RGB為主:
Camera | fx | fy | cx | cy | d0 | d1 | d2 | d3 | d4 |
(ROS default) | 525.0 | 525.0 | 319.5 | 239.5 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 |
517.3 | 516.5 | 318.6 | 255.3 | 0.2624 | -0.9531 | -0.0054 | 0.0026 | 1.1633 | |
520.9 | 521.0 | 325.1 | 249.7 | 0.2312 | -0.7849 | -0.0033 | -0.0001 | 0.9172 | |
535.4 | 539.2 | 320.1 | 247.6 | 0 | 0 | 0 | 0 | 0 |
深度相機的scale為5000(和kinect預設的1000是不同的)。也就是depth/中影象畫素值5000為真實世界中的一米。
因此,你下載了哪個序列,就要用對應的內參哦!
挑選一個IDE
現在讓我們來寫第一部分程式碼:讀取tum資料集並以視訊的方式顯示出來。
嗯,在寫程式碼之前呢,師兄還有一些話要囉嗦。雖然我們用linux的同學以會用vim和emacs為傲,但是寫程式碼呢,還是希望有一個IDE可以用的。vim和emacs的編輯確實很方便,然而寫c++,你還需要在類定義/聲明裡跳轉,需要補全和提示。要讓vim和emacs來做這種事,不是不可以,但是極其麻煩。這次師兄給大家推薦一個可以用於c++和cmake的IDE,叫做qtcreator。
安裝qtcreator:
sudo apt-get install qtcreator
介面大概長這樣:
這東西直接的好處是支援cmake。只要是cmake工程就可以丟進去編譯。按住ctrl鍵可以在各個類定義/變數/實現之間快速導航。如果你的cmake設定成了debug模式,它還能進行斷點除錯,十分的好用!
此外,由於ROS使用的catkin也是cmake的形式,所以它還能用來除錯ROS程式!
當然,因為叫qtcreator,自然還能寫qt的程式……然而這似乎已經不重要了……具體配置請大家自行摸索啦!
使用qtcreator寫一個hello slam
這件事情其實很簡單的嘍!
首先,隨便找一個資料夾,作為你程式碼的根目錄。在此目錄下新建一個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}) link_directories(${PCL_LIBRARY_DIRS}) 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/ )
重要部分已經加上註釋。
然後,在src/和experiment/下也新建兩個CMakeLists.txt,暫不填寫內容:
touch src/CMakeLists.txt experiment/CMakeLists.txt
下面,用qtcreator選單中的File->Open file or project,開啟剛才寫的CMakeLists.txt,它會識別出這是個cmake工程,並提示你要在何出構建。通常我們是新建一個build資料夾來構建的,所以這次也這麼做好了:
這樣就設定好啦。這時,點選左側的小錘或按下Ctrl+B,就可以構建工程。但是由於現在工程是空的,並沒有什麼可以構建的。所以我們加一個helloslam試試。在experiment下新建一個helloslam.cpp檔案,輸入:
1 #include<iostream> 2 using namespace std; 3 4 int main() 5 { 6 cout<<"Hello SLAM!"<<endl; 7 return 0; 8 }
然後,修改experiment/CMakeLists.txt檔案,告訴它我們要編譯這個檔案:
add_executable( helloslam helloslam.cpp )
然後,按下Ctrl+B,完成構建。此時會出現一個小綠條,提示你構建完畢。最後,點選左下綠色的三角按鈕,執行此程式:
怎麼樣,是不是很輕鬆?
讀者可以嘗試按住Ctrl並點選變數,看看qtcreator是如何跳轉的。或者人為加一句錯誤程式碼,看它會不會提示錯誤。也可以輸入 cout. 看它會提示哪些東西。甚至可以調成Debug模式,設定斷點,看程式是否會停在斷點上。
下期預告
下期我們會講基本的IO操作,包括引數檔案的讀取,TUM影象讀取與顯示,以及程式的測速等等。
問題
1. draw_groundtruth.py 跑不起來?
sudo apt-get install python-matplotlib python-numpy
再試試。
2.為什麼我的qtcreator是白的?
黑色只是個配色,在Tools/optoins中進行修改。其實白的也挺好看的。