/***********************************************************************************************************
.....從前，一種叫WALL-E的小機器人被送往地球清除垃圾，但WALL-E並不適合地球的環境，大批量地來也大批量地壞，最后只剩下WALL Tang還在日復一日的按照程序收拾廢品。就這么過了幾百年，僅存的WALL Tang還在垃圾堆里淘到不少人造寶貝，它也開始有了自我意識，懂得什么是孤獨。有一天一艘飛船突然降落，一個女機器人Candy來到地球執行搜尋任務，撿垃圾的機器人“愛”上了Candy，自此他決定跟隨Candy遠離地球。然而，以前WALL Tang的兩只眼睛是兩個固定的RGB相機，通過視覺算法（SFM）自動計算垃圾和自己的距離。這幾年來，因為老化問題，它有一只眼睛已經看不見東西了，慶幸的是，它從垃圾堆里面撿到一種叫做深度相機的東西，它可以通過機構光，TOF或者激光掃描的方式直接計算自己和前面東西的位置。 就這樣為了更好的適應這對新的眼睛實現私奔計划，WALL TANG潛心研究，志在用一只RGB眼睛和一只深度（depth）眼睛來進行高精度定位，搞清楚自己到底在哪，自己旁邊都有神馬，自己怎么走出去。
多年以后，當WALL TANG站在多普拉多星球俯視地球上堆積如山的垃圾堆的時候，才知道這種定位技術叫做機器人定位與制圖（SLAM）技術，自此，WALL TANG走上了教書育人的道路，勢必要把這項技術發揚傳承。那么接下來就讓WALL TANG 講講RGB-D SLAM 涉及到的硬件和關鍵吧。
*********************************************************************************************************/
1) 硬件部分
目前用在SLAM上的Sensor主要分兩大類，激光雷達（單線陣，多線陣）, RGB相機（單目，雙目，多目）（這一部分就不詳細介紹了），另一類為近年來新興的RGB-D傳感器（如微軟的Kinect，Primer Sense的Structure Sensor），通過價格低廉，實時，以及可接受的量測精度打入室內建模市場， 其深度相機主要是以結構光原理進行成像，通常具有激光投射器、光學衍射元件（DOE）、紅外攝像頭三大核心器件。它可以同時產生RGB圖像和深度圖像，如下圖所示，工作機制和視頻流類似，以每秒30幀的速度收集數據。下圖中是Primer sense 的strucutre sensor相機，它的doe是由兩部分組成的，一個是擴散片，一個是衍射片。先通過擴散成一個區域的隨機散斑，然后復制成九份，投射到了被攝物體上。根據紅外攝像頭捕捉到的紅外散斑，PS1080這個芯片就可以快速解算出各個點的深度信息。
有了深度圖和RGB圖像之后呢，，SLAM算法就需要工作了，由於RGB-D傳感器實際上將結合了三維結構和二維圖像結合在一起，所以與傳統的只通過激光點雲或者只通過RGB圖像序列進行SLAM有所不同，但其思路以及涉及到的關鍵技術都大同小異。
1) 關鍵技術部分
（1）深度數據模型
RGB-D的一大優勢在於每一幀獲取的RGB圖像和深度圖像能夠逐像素匹配在一起，針對深度圖像上的每一個像素，都可以獲取到它對應的深度值，然而，RGB-D相機的量測范圍有限，其數據量測精度和量測距離有關，一般只有3-4米以下的深度數據可用於室內建模。下圖為量測距離與量測精度圖表。
通過圖像的像素坐標可通過以下公式來獲取對應的三維坐標：
其中，Xc,Yc,Zc是獲取的地面坐標，u’和v’是像素坐標，D是從深度圖像中獲取的深度值（一般都有一個scale，D=D'/Scale），fxD和fyD是圖像的焦距
（2）特征點探測
特征點探測和匹配有多種方法，最常用的屬SIFT算子，1999年提出，2004年完善，David G.Lowe提出
a.SIFT：1999年提出，2004年完善，David G.Lowe提出
b.FAST(Features from Accelerated Tegment Test)：2006年，Edward Rosten ,Tom
優缺點：計算速度快，只計算了灰度信息
c.SURF(Speed UpRobust Feature)：2006年，Bay等提出的
優缺點：由sift改進而來，比sift快，多幅圖片時魯棒性好。
d.CenSurE(Center SurroundExtremas for Realtime Feature Detection and Matching)
比較：文章提出了新的方法，並與已有特征點檢測進行比較，比較時用到的算子：Harris，FAST，SIFT，SURF.方法：1. 計算當特征點是800時，對於不同的序列，特征點的可重復性。2. 計算最小的歐式距離值，對比距離區間點的個數，畫折線圖。3. 計算不同搜索范圍下，當特征點個數是800時，每種特征點檢測的可以匹配成功的百分比。4.使用the visual odometry(VO)評估每種算法的表現。5.比較了每種算法所用的時間。
e. BRISK(Binary Robust invariant scalable keypoints) ：2011年，Leutenegger,S等提出
優缺點：是對FAST算法的改進
具體的算法實現大家可以去官網直接下載對應的庫文件，筆者使用的是最常規的SIFT算法，由於RGB-D圖像數量較多，最好使用SIFT-GPU，可通過GPU對特征提取進行加速。
（3）回環檢測 （大回環，隨機回環，局部回環）+ appearance-based navigation（新技術）
回環檢測的作用主要是去除Drift Error， 就好比一個人蒙着眼睛，如果讓你一直走直線，實際情況是，隨着你走的距離越長，產生的偏差越大，可能完全變成一條曲線，這時候就需要去有人提醒你你要往右邊多走一點還是往左邊都走一點。
1）大回環：也叫全局回環檢測，進行大回環檢測需要知道我們什么時候回到起點，一般在進行三維測圖時我們規定一個起始位置，最后結束測圖的時候會重新回來，一般這個時候，就用起始數據幀與終止數據幀進行回環檢測，得到對應的約束，如全局回環圖所示，為三維測圖時獲取的首尾圖像，在回環框架圖中，‘e14,1’，‘e14,2’就是全局回環檢測得到的邊界約束。
全局回環
2）隨機回環：隨機回環即是在當前關鍵幀時，與前面所有的關鍵幀進行隨機匹配，可以隨機選擇5個或者10個關鍵幀進行匹配，得到對應的邊界約束，下面回環檢測框架圖中短虛線表示的就是隨機回環檢測到的邊界約束，e4,8
3）局部回環：局部回環是最常用的回環檢測方法，實際上是在當前數據幀位置下，做一個緩沖區，如下圖局部回環檢測，得到相鄰的關鍵幀索引，然后將當前數據幀與緩沖區內所有的關鍵幀一一進行匹配，得到對應的邊界約束，回環檢測框架圖中adjacent edge即是局部回環檢測得到的邊界約束。
局部回環檢測
回環檢測框架
以上三種回環檢測可以應對大部分場景，但有些情況下我們並不知道什么時候會重新回到相同的位置，隨機回環和全局回環需要在一定的知識引導的情況下才能得到對應的邊界約束，隨意常規來說局部回環是不管在任何場景中都可以用來有效減少drift error。 近些年出現了一種appearance-based navigation（新技術）的新技術，輸入所有的圖像之后，可以得到圖像之間相似性關系，進而可以很容易判斷出是否回到同一個位置。