小蘿蔔：師兄！過年啦！是不是很無聊啊！普通人的生活就是賺錢花錢，實在是很沒意思啊！

　　師兄：是啊……

　　小蘿蔔：他們都不懂搞科研和碼程式碼的樂趣呀！

　　師兄：可不是嘛……

　　小蘿蔔：所以今年過年，我們再做一個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為主：

　　深度相機的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中進行修改。其實白的也挺好看的。