寫在前面
首先打個廣告。SLAM研究者交流QQ群：254787961。歡迎各路大神和小白前來交流。
看了前面三篇博文之後，是不是有同學要問：博主你扯了那麼多有用沒用的東西，能不能再給力一點，拿出一個我們能實際上手玩玩的東西啊？沒錯，接下來我們就帶著大家，實際地跑一下視覺SLAM裡的那些經典程式，給大家一個直觀的印象——因此博文就叫"視覺SLAM實戰"啦。這些程式包括：
- RGBD SLAM V2
- SVO
- KinectFusion
- Orb-SLAM
如果你有什麼建議，可以發我的郵件或來群裡交流。當然，我是邊跑邊寫部落格，而不是一口氣調通再稀里嘩啦寫的。所以呢，最後跑的程式可能會和現在的計劃有些出入啦。好了，廢話不多說，先來介紹下實驗器材。
實驗器材
1. 硬體
說好的SLAM，沒有機器人怎麼行？老闆，先給我來三份！
這種機器人是Turtlebot的一個改裝版本：Viewbot。Turtlebot的詳情請參見：http://wiki.ros.org/Robots/TurtleBot/。 Viewbot是上海物景公司做的改裝版本，包括可以裝些額外的感測器，以及把原來的黑色底板變成透明底板什麼的，用起來和turtlebot大同小異，價格1W+。為了避免做廣告的嫌疑我就不放連結了。用這個機器人的好處，是ROS有直接對應的包，不用你自己寫。一句話即可開啟感測器讀資料，再一句話就可遙控它，方便省事。
機器人的主要部件就是它的底座和上頭的Kinect啦。座子自帶慣導，可以估計自己的位置；Kinect麼，大家都知道了，不多講。事實上今天要跑的rgbd-slam-v2不需要座子，只要一臺kinect就能動起來。我們用不著那麼多機器人，只用一個就行啦（就是貼了標籤那臺）。
2. 軟體
軟體方面只需一臺筆記，放在機器上跑程式。我用的是一臺華碩的ubuntu，裝了mac主題包真是山寨氣十足啊：
具體的軟體配置後面會細說。
3. 環境
環境嘛就是我的實驗室，這個也不細說了。
SLAM程式
RGB-D-SLAM-V2程式是由F. Endres大大寫的。論文見[1]。為什麼首先選這個程式呢？因為它的原理在我們前面的部落格中介紹過了。它是一個基於深度相機、Pose graph （圖優化）的程式，後端用的是g2o。另一方面，它的程式碼直接相容 ROS hydro版，基本不用配置就可以執行，非常方便。下面我們一步步地教大家執行這個程式：
- 從作者主頁下載原始碼。連結：http://felixendres.github.io/rgbdslam_v2/ 點選右側的tar.gz或zip下載至本地。
- 下載完成後解壓，得到一個包，裡面好多檔案呀：
不著急，先從README看起："RGBDSLAMv2 is based on the ROS project, OpenCV, PCL, OctoMap, SiftGPU and more – thanks!" 你還在等什麼？裝裝裝！好在這些東西在Ubuntu下安裝，就是幾句話搞定的事。
ROS hydro安裝指南： http://wiki.ros.org/cn/hydro/Installation/Ubuntu （加ppa源後直接安裝）
Linux OpenCV安裝指南：http://blog.sciencenet.cn/blog-571755-694742.html （從原始碼編譯）
PCL：http://www.pointclouds.org/downloads/linux.html （加ppa後安裝）
後面幾個嘛，裝不裝就看心情了，即使沒有裝，rgbd-slam-v2也能執行起來。
3. 裝完之後，看"Installation from scratch"一欄，基本照著做一遍即可。作者把命令都給出來了，我就不貼了。裝完之後，rgbdslam就在你的ros包裡啦。
4. 把機器人的Kinect usb口插進電腦，執行 roslaunch rgbdslam openni+rgbdslam.launch，就能看到一個漂亮的介面了。
可以看出作者真的挺用心，居然做了UI。像我這樣的懶人絕對不可能做UI的……不談這些，底下的兩個圖就是Kinect當前採到的彩色圖與深度圖，而上面則是3D線上點雲圖（可以用滑鼠轉視角）。現在，程式還在待機狀態，敲下Enter會採集單幀資料，而敲下空白鍵則會連續採集。
此外，程式的引數可以在openni+rgbdslam.launch檔案中調整。例如特徵點型別呀（支援SIFT，SURF，ORB，SIFTGPU），最大特徵數量等等。
執行程式
現在，我們連上Turtlebot的遙控端：
roslaunch turtlebot_bringup minimal.launch (啟動底座)
roslaunch turtlebot_teleop keyboard_teleop.launch (啟動遙控)
按下rgbd-slam-v2的空白鍵，讓機器人四處走起來。UI的狀態列會顯示程式的執行狀態，我看到過的有 正在提取特徵、加入新Frame，等等。如果它成功匹配上，上圖的點雲就會更新，並且會跟著機器人轉動。
我讓小車在實驗室的一個角上轉了幾圈。它把中間放置的一堆箱子（實際上是垃圾）都掃出來了。當你覺得滿意後，再按下Space鍵，停止程式。然後從選單中選出需要儲存的東西：機器人的軌跡，最終點雲圖，等等。軌跡是一個txt檔案，而點雲圖則一個pcd，在安裝PCL之後呼叫pcl_viewer即可檢視。
別看上面像是打了碼一樣的，最後出的點雲圖可是高清無碼的：
軌跡方面，用Matlab寫個指令碼plot一下即可：
可以看到軌跡上出現了斷層，實際上是機器人轉的比較快，演算法跟丟了，然而之後又通過迴環檢測給找了回來。
評價
最後，總結一下這個實驗吧。
Rgbd-slam-v2是14年論文裡提到的演算法。它整合了SLAM領域裡的各種技術：影象特徵、迴環檢測、點雲、圖優化等等，是一個非常全面且優秀的程式。它的UI做的也很漂亮，你可以在它的原始碼上繼續開發。作者也提供了資料集供研究者們測試。
缺點：在美觀之外，由於要提特徵（SIFT很費時）、渲染點雲，這些事情是很吃資源的，導致演算法實時性不太好。有時你會發現它卡在那兒不動了，不得不等它一小會。機器人如果走的太快，很容易跟丟。於是我的機器人就真的像只烏龜一樣慢慢地在地板上爬了……一旦轉頭快了，軌跡基本就斷掉了。此外，程式採集關鍵幀的頻率很高，稍微一會就採出幾十個幀，不太適合做長時間的SLAM。最後合出來的點雲有300W+個點，我用網格濾波之後才能勉強顯示出來。
參考文獻
[1]. Endres et al, 3D Mapping with an RGB-D camera, TRO, 2014.