小蘿卜：師兄！過年啦！是不是很無聊啊！普通人的生活就是賺錢花錢，實在是很沒意思啊！
師兄：是啊……
小蘿卜：他們都不懂搞科研和碼代碼的樂趣呀！
師兄：可不是嘛……
小蘿卜：所以今年過年，我們再做一個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文件）：
#!/usr/bin/python # Software License Agreement (BSD License) # # Copyright (c) 2013, Juergen Sturm, TUM # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions # are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above # copyright notice, this list of conditions and the following # disclaimer in the documentation and/or other materials provided # with the distribution. # * Neither the name of TUM nor the names of its # contributors may be used to endorse or promote products derived # from this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # # Requirements: # sudo apt-get install python-argparse """ The Kinect provides the color and depth images in an un-synchronized way. This means that the set of time stamps from the color images do not intersect with those of the depth images. Therefore, we need some way of associating color images to depth images. For this purpose, you can use the ''associate.py'' script. It reads the time stamps from the rgb.txt file and the depth.txt file, and joins them by finding the best matches. """ import argparse import sys import os import numpy def read_file_list(filename): """ Reads a trajectory from a text file. File format: The file format is "stamp d1 d2 d3 ...", where stamp denotes the time stamp (to be matched) and "d1 d2 d3.." is arbitary data (e.g., a 3D position and 3D orientation) associated to this timestamp. Input: filename -- File name Output: dict -- dictionary of (stamp,data) tuples """ file = open(filename) data = file.read() lines = data.replace(","," ").replace("\t"," ").split("\n") list = [[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"] list = [(float(l[0]),l[1:]) for l in list if len(l)>1] return dict(list) def associate(first_list, second_list,offset,max_difference): """ Associate two dictionaries of (stamp,data). As the time stamps never match exactly, we aim to find the closest match for every input tuple. Input: first_list -- first dictionary of (stamp,data) tuples second_list -- second dictionary of (stamp,data) tuples offset -- time offset between both dictionaries (e.g., to model the delay between the sensors) max_difference -- search radius for candidate generation Output: matches -- list of matched tuples ((stamp1,data1),(stamp2,data2)) """ first_keys = first_list.keys() second_keys = second_list.keys() potential_matches = [(abs(a - (b + offset)), a, b) for a in first_keys for b in second_keys if abs(a - (b + offset)) < max_difference] potential_matches.sort() matches = [] for diff, a, b in potential_matches: if a in first_keys and b in second_keys: first_keys.remove(a) second_keys.remove(b) matches.append((a, b)) matches.sort() return matches if __name__ == '__main__': # parse command line parser = argparse.ArgumentParser(description=''' This script takes two data files with timestamps and associates them ''') parser.add_argument('first_file', help='first text file (format: timestamp data)') parser.add_argument('second_file', help='second text file (format: timestamp data)') parser.add_argument('--first_only', help='only output associated lines from first file', action='store_true') parser.add_argument('--offset', help='time offset added to the timestamps of the second file (default: 0.0)',default=0.0) parser.add_argument('--max_difference', help='maximally allowed time difference for matching entries (default: 0.02)',default=0.02) args = parser.parse_args() first_list = read_file_list(args.first_file) second_list = read_file_list(args.second_file) matches = associate(first_list, second_list,float(args.offset),float(args.max_difference)) if args.first_only: for a,b in matches: print("%f %s"%(a," ".join(first_list[a]))) else: for a,b in matches: print("%f %s %f %s"%(a," ".join(first_list[a]),b-float(args.offset)," ".join(second_list[b])))
小蘿卜：那么這個文件要怎么用呢？
如果讀者熟悉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文件，輸入：
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中進行修改。其實白的也挺好看的。