小蘿蔔：師兄！過年啦！是不是很無聊啊！普通人的生活就是賺錢花錢，實在是很沒意思啊！
師兄：是啊……
小蘿蔔：他們都不懂搞科研和碼代碼的樂趣呀！
師兄：可不是嘛……
小蘿蔔：所以今年過年，我們再做一個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 |
| Freiburg 1 RGB | 517.3 | 516.5 | 318.6 | 255.3 | 0.2624 | -0.9531 | -0.0054 | 0.0026 | 1.1633 |
| Freiburg 2 RGB | 520.9 | 521.0 | 325.1 | 249.7 | 0.2312 | -0.7849 | -0.0033 | -0.0001 | 0.9172 |
| Freiburg 3 RGB | 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文件，輸入：
#include<iostream>
using namespace std;
int main()
{
cout<<"Hello SLAM!"<<endl;
return 0;
}然後，修改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中進行修改。其實白的也挺好看的。