283
機器人
視覺SLAM漫淡：機器人即時定位與地圖構建
限時幹貨下載：回複“資料”獲取獲取機器視覺教程，行業報告等資源，百度盤群組分享鏈接更新時間：2017-09-26，失效請在文末留言，不要在後台留言，你也可以在後台菜單“資源搜索”搜索更多你想要的網盤資源！
導讀：開始做SLAM（機器人即時定位與建圖）研究已經近一年了。從一年級開始對這個方向產生興趣，到現在為止，也算是對這個領域有了大致的了解。然而越了解，越覺得這個方向難度很大。總體來講有以下幾個原因：
1、入門資料很少。雖然國內也有不少人在做，但這方麵現在沒有太好的入門教程。《SLAM for dummies》可以算是一篇。中文資料幾乎沒有。
2、SLAM研究已進行了三十多年，從上世紀的九十年代開始。其中又有若幹曆史分枝和爭論，要把握它的走向就很費工夫。
3、難以實現。SLAM是一個完整的係統，由許多個分支模塊組成。現在經典的方案是“圖像前端，優化後端，閉環檢測”的三部曲，很多文獻看完了自己實現不出來。
4、自己動手編程需要學習大量的先決知識。首先你要會C和C++，網上很多代碼還用了11標準的C++。第二要會用Linux。第三要會cmake，vim/emacs及一些編程工具。第四要會用openCV, PCL, Eigen等第三方庫。隻有學會了這些東西之後，你才能真正上手編一個SLAM係統。如果你要跑實際機器人，還要會ROS。
當然，困難多意味著收獲也多，坎坷的道路才能鍛煉人（比如說走著走著才發現Linux和C++才是我的真愛之類的。）鑒於目前網上關於視覺SLAM的資料極少，我於是想把自己這一年多的經驗與大家分享一下。說的不對的地方請大家批評指正。
這篇文章關注視覺SLAM，專指用攝像機，Kinect等深度像機來做導航和探索，且主要關心室內部分。到目前為止，室內的視覺SLAM仍處於研究階段，遠未到實際應用的程度。一方麵，編寫和使用視覺SLAM需要大量的專業知識，算法的實時性未達到實用要求；另一方麵，視覺SLAM生成的地圖（多數是點雲）還不能用來做機器人的路徑規劃，需要科研人員進一步的探索和研究。以下，我會介紹SLAM的曆史、理論以及實現的方式，且主要介紹視覺（Kinect）的實現方式。
SLAM問題
SLAM，全稱叫做Simultaneous Localization and Mapping，中文叫做同時定位與建圖。啊不行，這麼講下去，這篇文章肯定沒有人讀，所以我們換一個講法。
小蘿卜的故事
從前，有一個機器人叫“小蘿卜”。它長著一雙烏黑發亮的大眼睛，叫做Kinect。有一天，它被邪惡的科學家關進了一間空屋子，裏麵放滿了雜七雜八的東西。
小蘿卜感到很害怕，因為這個地方他從來沒來過，一點兒也不了解。讓他感到害怕的主要是三個問題：
1. 自己在哪裏？
2. 這是什麼地方？
3. 怎麼離開這個地方？
在SLAM理論中，第一個問題稱為定位 (Localization)，第二個稱為建圖 (Mapping)，第三個則是隨後的路徑規劃。我們希望借助Kinect工具，幫小蘿卜解決這個難題。各位同學有什麼思路呢？
Kinect數據
要打敗敵人，首先要了解你的武器。不錯，我們先介紹一下Kinect。眾所周知這是一款深度相機，你或許還聽說過別的牌子，但Kinect的價格便宜，測量範圍在3m-12m之間，精度約3cm，較適合於小蘿卜這樣的室內機器人。它采到的圖像是這個樣子的（從左往右依次為rgb圖，深度圖與點雲圖）：
Kinect的一大優勢在於能比較廉價地獲得每個像素的深度值，不管是從時間上還是從經濟上來說。OK，有了這些信息，小蘿卜事實上可以知道它采集到的圖片中，每一個點的3d位置。隻要我們事先標定了Kinect，或者采用出廠的標定值。
我們把坐標係設成這個樣子，這也是openCV中采用的默認坐標係。
o’-uv是圖片坐標係，o-xyz是Kinect的坐標係。假設圖片中的點為(u,v)，對應的三維點位置在(x,y,z)，那麼它們之間的轉換關係是這樣的：
或者更簡單的：
後一個公式給出了計算三維點的方法。先從深度圖中讀取深度數據（Kinect給的是16位無符號整數），除掉z方向的縮放因子，這樣你就把一個整數變到了以米為單位的數據。然後，x,y用上麵的公式算出。一點都不難，就是一個中心點位置和一個焦距而已。f代表焦距，c代表中心。如果你沒有自己標定你的Kinect，也可以采用默認的值：s=5000, cx = 320, cy=240, fx=fy=525。實際值會有一點偏差，但不會太大。
定位問題
知道了Kinect中每個點的位置後，接下來我們要做的，就是根據兩幀圖像間的差別計算小蘿卜的位移。比如下麵兩張圖，後一張是在前一張之後1秒采集到的：
你肯定可以看出，小蘿卜往右轉過了一定的角度。但究竟轉過多少度呢？這就要靠計算機來求解了。這個問題稱為相機相對姿態估計，經典的算法是ICP（Iterative Closest Point，迭代最近點）。這個算法要求知道這兩個圖像間的一組匹配點，說的通俗點，就是左邊圖像哪些點和右邊是一樣的。你當然看見那塊黑白相間的板子同時出現在兩張圖像中。在小蘿卜看來，這裏牽涉到兩個簡單的問題：特征點的提取和匹配。
如果你熟悉計算機視覺，那你應該聽說過SIFT, SURF之類的特征。不錯，要解決定位問題，首先要得到兩張圖像的一個匹配。匹配的基礎是圖像的特征，下圖就是SIFT提取的關鍵點與匹配結果：
對實現代碼感興趣的同學請Google“opencv 匹配”即可，在openCV的教程上也有很明白的例子。上麵的例子可以看出，我們找到了一些匹配，但其中有些是對的（基本平等的匹配線），有些是錯的。這是由於圖像中存在周期性出現的紋理（黑白塊），所以容易搞錯。但這並不是問題，在接下來的處理中我們會將這些影響消去。
得到了一組匹配點後，我們就可以計算兩個圖像間的轉換關係，也叫PnP問題。它的模型是這樣的：
R為相機的姿態，C為相機的標定矩陣。R是不斷運動的，而C則是隨著相機做死的。ICP的模型稍有不同，但原理上也是計算相機的姿態矩陣。原則上，隻要有四組匹配點，就可以算這個矩陣。你可以調用openCV的SolvePnPRANSAC函數或者PCL的ICP算法來求解。openCV提供的算法是RANSAC（Random Sample Consensus，隨機采樣一致性）架構，可以剔除錯誤匹配。所以代碼實際運行時，可以很好地找到匹配點。以下是一個結果的示例。
上麵兩張圖轉過了16.63度，位移幾乎沒有。
有同學會說，那隻要不斷匹配下去，定位問題不就解決了嗎？表麵上看來，的確是這樣的，隻要我們引入一個關鍵幀的結構（發現位移超過一個固定值時，定義成一個關鍵幀）。然後，把新的圖像與關鍵幀比較就行了。至於建圖，就是把這些關鍵幀的點雲拚起來，看著還有模有樣，煞有介事的：
1－200幀的匹配結果
然而，如果事情真這麼簡單，SLAM理論就不用那麼多人研究三十多年了（它是從上世紀90年代開始研究的）（上麵講的那些東西簡直隨便哪裏找個小碩士就能做出來……）。那麼，問題難在什麼地方呢？
SLAM端優化理論
最麻煩的問題，就是“噪聲”。這種漸近式的匹配方式，和那些慣性測量設備一樣，存在著累積噪聲。因為我們在不斷地更新關鍵幀，把新圖像與最近的關鍵幀比較，從而獲得機器人的位移信息。但是你要想到，如果有一個關鍵幀出現了偏移，那麼剩下的位移估計都會多出一個誤差。這個誤差還會累積，因為後麵的估計都基於前麵的機器人位置……哇！這後果簡直不堪設想啊（例如，你的機器人往右轉了30度，再往左轉了30度回到原來的位置。然而由於誤差，你算成了向右轉29度，再向左轉31度，這樣你構建的地圖中，會出現初始位置的兩個“重影”）。我們能不能想辦法消除這個該死的誤差呢？
朋友們，這才是SLAM的研究，前麵的可以說是“圖像前端”的處理方法。我們的解決思路是：如果你和最近的關鍵幀相比，會導致累計誤差。那麼，我們最好是和更前麵的關鍵幀相比，而且多比較幾個幀，不要隻比較一次。
我們用數學來描述這個問題。設：
不要怕，隻有借助數學才能把這個問題講清楚。上麵的公式中，xp是機器人小蘿卜的位置，我們假定由n個幀組成。xL則是路標，在我們的圖像處理過程中就是指SIFT提出來的關鍵點。如果你做2D SLAM，那麼機器人位置就是x, y加一個轉角theta。如果是3D SLAM，就是x,y,z加一個四元數姿態（或者rpy姿態）。這個過程叫做參數化（Parameterization）。
不管你用哪種參數，後麵兩個方程你都需要知道。前一個叫運動方程，描述機器人怎樣運動。u是機器人的輸入，w是噪聲。這個方程最簡單的形式，就是你能通過什麼方式（碼盤等）獲得兩幀間的位移差，那麼這個方程就直接是上一幀與u相加即得。另外，你也可以完全不用慣性測量設備，這樣我們就隻依靠圖像設備來估計，這也是可以的。
後一個方程叫觀測方程，描述那些路標是怎麼來的。你在第i幀看到了第j個路標，產生了一個測量值，就是圖像中的橫縱坐標。最後一項是噪聲。偷偷告訴你，這個方程形式上和上一頁的那個方程是一模一樣的。
在求解SLAM問題前，我們要看到，我們擁有的數據是什麼？在上麵的模型裏，我們知道的是運動信息u以及觀測z。用示意圖表示出來是這樣的：
我們要求解的，就是根據這些u和z，確定所有的xp和xL。這就是SLAM問題的理論。從SLAM誕生開始科學家們就一直在解決這個問題。最初，我們用Kalman濾波器，所以上麵的模型（運動方程和觀測方程）被建成這個樣子。直到21世紀初，卡爾曼濾波器仍在SLAM係統占據最主要的地位，Davison經典的單目SLAM就是用EKF做的。但是後來，出現了基於圖優化的SLAM方法，漸漸有取而代之的地位[1]。我們在這裏不介紹卡爾曼濾波器，有興趣的同學可以在wiki上找卡爾曼濾波器，另有一篇中文的《卡爾曼濾波器介紹》也很棒。由於濾波器方法存儲n個路標要消耗n平方的空間，在計算量上有點對不住大家。盡管08年有人提出分治法的濾波器能把複雜度弄到O(n) [2]，但實現手段比較複雜。我們要介紹那種新興的方法: Graph-based SLAM。
圖優化方法把SLAM問題做成了一個優化問題。學過運籌學的同學應該明白，優化問題對我們有多麼重要。我們不是要求解機器人的位置和路標位置嗎？我們可以先做一個猜測，猜想它們大概在什麼地方。這其實是不難的。然後呢，將猜測值與運動模型／觀測模型給出的值相比較，可以算出誤差：
通俗一點地講，例如，我猜機器人第一幀在(0,0,0)，第二幀在(0,0,1)。但是u1告訴我機器人往z方向（前方）走了0.9米，那麼運動方程就出現了0.1m的誤差。同時，第一幀中機器人發現了路標1，它在該機器人圖像的正中間；第二幀卻發現它在中間偏右的位置。這時我們猜測機器人隻是往前走，也是存在誤差的。至於這個誤差是多少，可以根據觀測方程算出來。
我們得到了一堆誤差，把這些誤差平方後加起來（因為單純的誤差有正有負，然而平方誤差可以改成其他的範數，隻是平方更常用），就得到了平方誤差和。我們把這個和記作phi，就是我們優化問題的目標函數。而優化變量就是那些個xp, xL。
改變優化變量，誤差平方和（目標函數）就會相應地變大或變小，我們可以用數值方法求它們的梯度和二階梯度矩陣，然後用梯度下降法求最優值。這些東西學過優化的同學都懂的。
注意到，一次機器人SLAM過程中，往往會有成千上萬幀。而每一幀我們都有幾百個關鍵點，一乘就是幾百萬個優化變量。這個規模的優化問題放到小蘿卜的機載小破本上可解嗎？是的，過去的同學都以為，Graph-based SLAM是無法計算的。但就在21世紀06，07年後，有些同學發現了，這個問題規模沒有想象的那麼大。上麵的J和H兩個矩陣是“稀疏矩陣”，於是呢，我們可以用稀疏代數的方法來解這個問題。“稀疏”的原因，在於每一個路標，往往不可能出現在所有運動過程中，通常隻出現在一小部分圖像裏。正是這個稀疏性，使得優化思路成為了現實。
優化方法利用了所有可以用到的信息（稱為full-SLAM, global SLAM），其精確度要比我們一開始講的幀間匹配高很多。當然計算量也要高一些。
由於優化的稀疏性，人們喜歡用“圖”來表達這個問題。所謂圖，就是由節點和邊組成的東西。我寫成G=，大家就明白了。V是優化變量節點，E表示運動/觀測方程的約束。什麼，更煳塗了嗎？那我就上一張圖，來自[3]。
圖有點模煳，而且數學符號和我用的不太一樣，我用它來給大家一個圖優化的直觀形象。上圖中，p是機器人位置，l是路標，z是觀測，t是位移。其中呢，p, l是優化變量，而z,t是優化的約束。看起來是不是像一些彈簧連接了一些質點呢？因為每個路標不可能出現在每一幀中，所以這個圖是蠻稀疏的。不過，“圖”優化隻是優化問題的一個表達形式，並不影響優化的含義。實際解起來時還是要用數值法找梯度的。這種思路在計算機視覺裏，也叫做Bundle Adjustment。它的具體方法請參見一篇經典文章[4]。
不過，BA的實現方法太複雜，不太建議同學們拿C來寫。好在2010年的ICRA上，其他的同學們提供了一個通用的開發包：g2o [5]。它是有圖優化通用求解器，很好用，我改天再詳細介紹這個軟件包。總之，我們隻要把觀測和運動信息丟到求解器裏就行。這個優化器會為我們求出機器人的軌跡和路標位置。如下圖，紅點是路標，藍色箭頭是機器人的位置和轉角（2D SLAM）。細心的同學會發現它往右偏轉了一些。：
閉環檢測
上麵提到，僅用幀間匹配最大的問題在於誤差累積，圖優化的方法可以有效地減少累計誤差。然而，如果把所有測量都丟進g2o，計算量還是有點兒大的。根據我自己測試，約10000多條邊，g2o跑起來就有些吃力了。這樣，就有同學說，能把這個圖構造地簡潔一些嗎？我們用不著所有的信息，隻需要把有用的拿出來就行了。
事實上，小蘿卜在探索房間時，經常會左轉一下，右轉一下。如果在某個時刻他回到了以前去過的地方，我們就直接與那時候采集的關鍵幀做比較，可以嗎？我們說，可以，而且那是最好的方法。這個問題叫做閉環檢測。
閉環檢測是說，新來一張圖像時，如何判斷它以前是否在圖像序列中出現過？有兩種思路：一是根據我們估計的機器人位置，看是否與以前某個位置鄰近；二是根據圖像的外觀，看它是否和以前關鍵幀相似。目前主流方法是後一種，因為很多科學家認為前一種依靠有噪聲的位置來減少位置的噪聲，有點循環論證的意思。後一種方法呢，本質上是個模式識別問題（非監督聚類，分類），常用的是Bag-of-Words (BOW)。但是BOW需要事先對字典進行訓練，因此SLAM研究者仍在探討有沒有更合適的方法。
在Kinect SLAM經典大作中[6]，作者采用了比較簡單的閉環方法：在前麵n個關鍵幀中隨機采k個，與當前幀兩兩匹配。匹配上後認為出現閉環。這個真是相當的簡單實用，效率也過得去。
高效的閉環檢測是SLAM精確求解的基礎。這方麵還有很多工作可以做。
參考文獻
[1] Visual SLAM: Why filter? Strasdat et. al., Image and Vision Computing, 2012.
[2] Divide and Conquer: EKF SLAM in O(n), Paz Lina M et al., IEEE Transaction on Robotics, 2008
[3] Relative bundle adjustment, Sibley, Gabe, 2009
[4] Bundle adjustment - a Modern Synthesis. Triggs B et. el., Springer, 2000
[5] g2o: A General Framework for Graph Optimization, Kummerle Rainer, et. al., ICRA, 2011
[6] 3-D Mapping with an RGB-D Camera, IEEE Transaction on Robotics, Endres et al., 2014
圖優化理論與g2o的使用
l 已知的東西：傳感器數據（圖像，點雲，慣性測量設備等）。我們的傳感器主要是一個Kinect，因此數據就是一個視頻序列，說的再詳細點就是一個RGB位圖序列與一個深度圖序列。至於慣性測量設備，可以有也可以沒有。
l 待求的東西：機器人的運動軌跡，地圖的描述。運動軌跡，畫出來應該就像是一條路徑。而地圖的描述，通常是點雲的描述。但是點雲描述是否可用於導航、規劃等後續問題，還有待研究。
這兩個點之間還是有挺長的路要走的。如果我們使用圖優化，往往會在整個視頻序列中，定義若幹個關鍵幀：
在這張圖中，我們有一個路標點（五角星），並在各個關鍵幀中都看到了這個點。於是，我們就能用PnP或ICP求解相鄰關鍵點的運動方向。這些在上篇文章都介紹過了，包括特征選擇，匹配及計算等等。那麼，這個過程中有什麼問題呢？
為什麼要用全局優化
你一定已經注意到，理想的計算總和實際有差距的。好比說理想的科研就是“看論文——產生想法——做實驗——發文章”，那麼現實的科研就是“看論文——產生想法——做實驗——發現該想法在二十年前就有人做過了”，這樣一個過程。實際當中，僅通過幀間運動（ego-motion）來計算機器人軌跡是遠遠不夠的。如下圖所示：
如果你隻用幀間匹配，那麼每一幀的誤差將對後麵所有的運動軌跡都要產生影響。例如第二幀往右偏了0.1，那麼後麵第三、四、五幀都要往右偏0.1，還要加上它們自己的估算誤差。所以結果就是：當程序跑上十幾秒之後早就不知道飛到哪兒去了。這是經典的SLAM現象，在EKF實現中，也會發現，當機器人不斷運動時，不確定性會不斷增長。當然不是我們所希望的結果。
那麼怎麼辦才好呢？想象你到了一個陌生的城市，安全地走出了火車站，並在附近遊蕩了一會兒。當你走的越遠，看到許多未知的建築。你就越搞不清楚自己在什麼地方。如果是你，你會怎麼辦？
通常的做法是認準一個標誌性建築物，在它周圍轉上幾圈，弄清楚附近的環境。然後再一點點兒擴大我們走過的範圍。在這個過程中，我們會時常回到之前已經見過的場景，因此對它周圍的景象就會很熟悉。
機器人的情形也差不多，除了大多數時候是人在遙控它行走。因而我們希望，機器人不要僅和它上一個幀進行比較，而是和更多先前的幀比較，找出其中的相似之處。這就是所謂的回環檢測（Loop closure detection）。用下麵的示意圖來說明：
沒有回環時，由於誤差對後續幀產生影響，機器人路徑估計很不穩定。加上一些局部回環，幾個相鄰幀就多了一些約束，因而誤差就減少了。你可以把它看成一個由彈簧連起來的鏈條（質點-彈簧模型）。當機器人經過若幹時間，回到最初地方時，檢測出了大回環時，整個環內的結構都會變得穩定很多。我們就可以籍此知道一個房間是方的還是圓的，麵前這堵牆對應著以前哪一堵牆，等等。
相信講到這裏，大家對回環檢測都有了一個感性的認識。那麼，這件事情具體是怎麼建模，怎麼計算，怎麼編程呢？下麵我們就一步步來介紹。
圖優化的數學模型
SLAM問題的優化模型可以有幾種不同的建模方式。我們挑選其中較簡單的一種進行介紹，即FrameSLAM，在2008年提出。它的特點是隻用位姿約束而不用特征約束，減少了很多計算量，表達起來也比較直觀。下麵我們給出一種6自由度的3D SLAM建模方法。
符號：
注意到這裏的建模與前文有所不同，是一個簡化版的模型。因為我們假設幀間匹配時得到了相鄰幀的變換矩陣，而不是把所有特征也放到優化問題裏麵來。所以這個模型看上去相對簡單。但是它很實用，因為不用引入特征，所以結點和邊的數量大大減少，要知道在圖像裏提特征動輒成百上千的。
g2o是什麼
g2o，就是對上述問題的一個求解器。它原理上是一個通用的求解器，並不限定於某些SLAM問題。你可以用它來求SLAM，也可以用ICP, PnP以及其他你能想到的可以用圖來表達的優化問題。它的代碼很規範，就是有一個缺點：文檔太少。唯一的說明文檔還有點太裝叉（個人感覺）了，有點擺弄作者數學水平的意思，反正那篇文檔很難懂就是了。話說程序文檔不應該是告訴我怎麼用才對麼……
言歸正傳。如果你想用g2o，請去它的github上麵下載：https://github.com/RainerKuemmerle/g2o
它的API在：https://www.rock-robotics.org/stable/api/slam/g2o/classg2o_1_1HyperGraph.html
安裝
g2o是一個用cmake管理的C++工程，我是用Linux編譯的，所以不要問我怎麼在win下麵用g2o，因為我也不會……不管怎麼說，你下載了它的zip包或者用git拷下來之後，裏麵有一個README文件。告訴你它的依賴項。在ubuntu下，直接鍵入命令：
sudo apt-get install cmake libeigen3-dev libsuitesparse-dev libqt4-dev qt4-qmake libqglviewer-qt4-dev
我個人感覺還要 libcsparse-dev和freeglut3這兩個庫，反正多裝了也無所謂。注意libqglviewer-qt4-dev隻在ubuntu 12.04庫裏有，14.04 裏換成另一個庫了。g2o的可視化工具g2o_viewer是依賴這個庫的，所以，如果你在14.04下麵編，要麼是去把12.04那個deb（以及它的依賴項）找出來裝好，要麼用ccmake，把build apps一項給去掉，這樣就不編譯這個工具了。否則編譯過不去。
解開zip後，新建一個build文件夾，然後就是：
cmake ..
make
sudo make install
這樣g2o就裝到了你的/usr/local/lib和/usr/local/include下麵。你可以到這兩個地方去看它的庫文件與頭文件。
學習g2o的使用
因為g2o的文檔真的很裝叉（不能忍），所以建議你直接看它的源代碼，耐心看，應該比文檔好懂些。它的example文檔夾下有一些示例代碼，其中有一個tutorial_slam2d文件夾下有2d slam仿真的一個程序。值得仔細閱讀。
使用g2o來實現圖優化還是比較容易的。它幫你把節點和邊的類型都定義好了，基本上隻需使用它內置的類型而不需自己重新定義。要構造一個圖，要做以下幾件事：
l 定義一個SparseOptimizer. 編寫方式參見tutorial_slam2d的聲明方式。你還要寫明它使用的算法。通常是Gauss-Newton或LM算法。個人覺得後者更好一些。
l 定義你要用到的邊、節點的類型。例如我們實現一個3D SLAM。那麼就要看它的g2o/types/slam3d下麵的頭文件。節點頭文件都以vertex_開頭，而邊則以edge_開頭。在我們上麵的模型中，可以選擇vertex_se3作為節點，edge_se3作為邊。這兩個類型的節點和邊的數據都可以直接來自於Eigen::Isometry，即上麵講到過的變換矩陣T。
l 編寫一個幀間匹配程序，通過兩張圖像算出變換矩陣。這個用opencv, pcl都可以做。
l 把你得到的關鍵幀作為節點，變換矩陣作為邊，加入到optimizer中。同時設定節點的估計值（如果沒有慣性測量就設成零）與邊的約束（變換矩陣）。此外，每條邊還需設定一個信息矩陣（協方差矩陣之逆）作為不確定性的度量。例如你覺得幀間匹配精度在0.1m，那麼把信息矩陣設成100的對角陣即可。
l 在程序運行過程中不斷作幀間檢測，維護你的圖。
l 程序結束時調用optimizer.optimize( steps )進行優化。優化完畢後讀取每個節點的估計值，此時就是優化後的機器人軌跡。
代碼這種東西展開來說會變得像字典一樣枯燥，所以具體的東西需要大家自己去看，自己去體會。這裏有我自己寫的一個程序，可以供大家參考。不過這個程序需要帶著數據集才能跑，學習g2o的同學隻需參考裏麵代碼的寫法即可：https://github.com/gaoxiang12/slam3d_gx
最近我跑了幾個公開數據集（https://vision.in.tum.de/data/datasets/rgbd-dataset）上的例子（fr1_desk, fr2_slam)(，感覺效果還不錯。有些數據集還是挺難的。最後一張圖是g2o_viewer，可以看到那些關鍵路徑點與邊的樣子。
研究點介紹
在《SLAM for Dummy》中，有一句話說的好："SLAM並不是一種算法，而是一個概念。（SLAM is more like a concept than a single algorithm.）"所以，你可以和導師、師兄弟（以及師妹，如果有的話）說你在研究SLAM，但是，作為同行，我可能更關心：你在研究SLAM中的哪一個問題。有些研究者專注於實現一個具體的SLAM係統，而更多的人則是在研究SLAM裏某些方法的改進。做應用和做理論的人往往彼此看不起，不過二者對科研都是有貢獻的。作為研究生，我還是建議各位抓住SLAM中一個小問題，看看能否對現有的算法進行改進或者比較。不要覺得這種事情膚淺，它是對研究有實際幫助和意義的。同時，我也有一些朋友，做了一個基於濾波器/圖優化的SLAM實現。程序是跑起來了，但他/她不知道自己有哪些貢獻，鑽研了哪個問題，寫論文的時候就很頭疼。所以，作為研究生，我建議你選擇SLAM中的一個問題，改進其中的算法，而不是先找一堆程序跑起來再說。
那麼問題來了：SLAM方麵究竟有哪些可以研究的地方呢？我為大家上一個腦圖。
這個圖是從我筆記本上拍下來的（請勿吐槽字和對焦）。可以看到，以SLAM為中心，有五個圈連接到它。我稱它為Basic Theory（基礎理論）、Sensor（傳感器）、Mapping（建圖）、Loop Detection（回環檢測）、Advanced Topic（高級問題）。這可以說是SLAM的研究方向。下麵我們"花開五朵，各表一枝"。
基本理論
SLAM的基本理論，是指它的數學建模。也就是你如何用數學模型來表達這個問題。為什麼說它"基本"呢？因為數學模型影響著整個係統的性能，決定了其他問題的處理方法。在早先的研究中（86年提出[1]至21世紀前期[2]），是使用卡爾曼濾波器的數學模型的。那裏的機器人，就是一個位姿的時間序列；而地圖，就是一堆路標點的集合。什麼是路標點的集合？就是用(x,y,z)表示每一個路標，然後在濾波器更新的過程中，讓這三個數慢慢收斂。
那麼，請問這樣的模型好不好？
好處是可以直接套用濾波器的求解方法。卡爾曼濾波器是很成熟的理論，比較靠譜。
缺點呢？首先，濾波器有什麼缺點，基於它的SLAM就有什麼缺點。所以EKF的線性化假設啊，必須存儲協方差矩陣帶來的資源消耗啊，都成了缺點（之後的文章裏會介紹）。然後呢，最直觀的就是，用(x,y,z)表示路標？萬一路標變了怎麼辦？平時我們不就把屋裏的桌子椅子挪來挪去的嗎？那時候濾波器就掛了。所以啊，它也不適用於動態的場合。
這種局限性就是數學模型本身帶來的，和其他的算法無關。如果你希望在動態環境中跑SLAM，就要使用其他模型或改進現有的模型了。
SLAM的基本理論，向來分為濾波器和優化方法兩類。濾波器有擴展卡爾曼濾波（EKF）、粒子濾波（PF），FastSLAM等，較早出現。而優化方向用姿態圖（Pose Graph），其思想在先前的文章中介紹過。近年來用優化的逐漸增多，而濾波器方麵則在13年出現了基於Random Finite Set的方法[3]，也是一個新興的浪潮[4]。關於這些方法的詳細內容，我們在今後的文章中再進行討論。
作為SLAM的研究人員，應該對各種基本理論以及優缺點有一個大致的了解，盡管它們的實現可能非常複雜。
傳感器
傳感器是機器人感知世界的方式。傳感器的選擇和安裝方式，決定了觀測方程的具體形式，也在很大程度上影響著SLAM問題的難度。早期的SLAM多使用激光傳感器（Laser Range Finder），而現在則多使用視覺相機、深度相機、聲呐（水下）以及傳感器融合。我覺得該方向可供研究點有如下幾個：
如何使用新興傳感器進行SLAM。 要知道傳感器在不斷發展，總有新式的東西會出來，所以這方麵研究肯定不會斷。
不同的安裝方式對SLAM的影響。 舉例來說，比如相機，頂視（看天花板）和下視（看地板）的SLAM問題要比平視容易很多。為什麼容易呢？因為頂/下視的數據非常穩定，不像平視，要受各種東西的幹擾。當然，你也可以研究其他的安裝方式。
改進傳統傳感器的數據處理。這部分就有些困難了，因為經常傳感器已經有很多人在使用，你做的改進，未必比現有的成熟方法更好。
建圖
建圖，顧名思議，就是如何畫地圖唄。其實，如果知道了機器人的真實軌跡，畫地圖是很簡單的一件事。不過，地圖的具體形式也是研究點之一。比如說常見的有以下幾種：
路標地圖。
地圖由一堆路標點組成。EKF中的地圖就是這樣的。但是，也有人說，這真的是地圖嗎（這些零零碎碎的點都是什麼啊喂）？所以路標圖盡管很方便，但多數人對這種地圖是不滿意的，至少看上去不像個地圖啊。於是就有了密集型地圖（Dense map）。
度量地圖（Metric map）
通常指2D/3D的網格地圖，也就是大家經常見的那種黑白的/點雲式地圖。點雲地圖比較酷炫，很有種高科技的感覺。它的優點是精度比較高，比如2D地圖可以用0-1表示某個點是否可通過，對導航很有用。缺點是相當吃存儲空間，特別是3D，把所有空間點都存起來了，然而大多數角角落落裏的點除了好看之外都沒什麼意義……
拓撲地圖（Topological map）
拓撲地圖是比度量地圖更緊湊的一種地圖。它將地圖抽象為圖論中的"點"和"邊"，使之更符合人類的思維。比如說我要去五道口，不知道路，去問別人。那人肯定不會說，你先往前走621米，向左拐94.2度，再走1035米……（這是瘋子吧）。正常人肯定會說，往前走到第二個十字路口，左拐，走到下一個紅綠燈，等等。這就是拓撲地圖。
混合地圖。
既然有人要分類，就肯定有人想把各類的好處揉到一起。這個就不多說了吧。
回環檢測
回環檢測，又稱閉環檢測（Loop closure detection），是指機器人識別曾到達場景的能力。如果檢測成功，可以顯著地減小累積誤差。
回環檢測目前多采用詞袋模型（Bag-of-Word），研究計算機視覺的同學肯定不會陌生。它實質上是一個檢測觀測數據相似性的問題。在詞袋模型中，我們提取每張圖像中的特征，把它們的特征向量（deor）進行聚類，建立類別數據庫。比如說，眼睛、鼻子、耳朵、嘴等等（實際當中沒那麼高級，基本上是一些邊緣和角）。假設有10000個類吧。然後，對於每一個圖像，可以分析它含有數據庫中哪幾個類。以1表示有，以0表示沒有。那麼，這個圖像就可用10000維的一個向量來表達。而不同的圖像，隻要比較它們的向量即可。
回環檢測也可以建成一個模型識別問題，所以你也可以使用各種機器學習的方法來做，比如什麼決策樹/SVM，也可以試試Deep Learning。不過實際當中要求實時檢測，沒有那麼多時間讓你訓練分類器。所以SLAM更側重在線的學習方法。
高級話題
前麵的都是基礎的SLAM，隻有"定位"和"建圖"兩件事。這兩件事在今天已經做的比較完善了。近幾年的RGB-D SLAM[5], SVO[6], Kinect Fusion[7]等等，都已經做出了十分炫的效果。但是SLAM還未走進人們的實際生活。為什麼呢？
因為實際環境往往非常複雜。燈光會變，太陽東升西落，不斷的有人從門裏麵進進出出，並不是一間安安靜靜的空屋子，讓一個機器人以2cm/s的速度慢慢逛。論文中看起來酷炫的算法，在實際環境中往往捉襟見肘，處處碰壁。向實際環境挑戰，是SLAM技術的主要發展方向，也就是我們所說的高級話題。主要有：動態場景、語義地圖、多機器人協作等等。
最後，讓我們想象一下未來SLAM的樣子吧：
有一天，小蘿卜被領進了一家新的實驗樓。在短暫的自我介紹之後，他飛快地在樓裏逛了一圈，記住了哪裏是走廊，哪兒是房間。他刻意地觀察各個房間特有的物品，以便區分這些看起來很相似的房間。然後，他回到了科學家身邊，協助他的研究。有時，科學家會讓他去各個屋裏找人，找資料，有時，也帶著他去認識新安裝的儀器和設備。在閑著沒事時，小蘿卜也會在樓裏逛逛，看看那些屋裏都有什麼變化。每當新的參觀人員到來，小蘿卜會給他們看樓裏的平麵圖，向他們介紹各個樓層的方位與狀況，為他們導航。大家都很喜歡小蘿卜。而小蘿卜明白，這一切，都是過去幾十年裏SLAM研究人員不斷探索的結果。
[1]. Smith, R.C. and P. Cheeseman, On the Representation and Estimation of Spatial Uncertainty. International Journal of Robotics Research, 1986. 5(4): p. 56--68.
[2]. Se, S., D. Lowe and J. Little, Mobile robot localization and mapping with uncertainty using scale-invariant visual landmarks. The international Journal of robotics Research, 2002. 21(8): p. 735--758.
[3]. Mullane, J., et al., A Random-Finite-Set Approach to Bayesian SLAM. IEEE Transactions on Robotics, 2011.
[4]. Adams, M., et al., SLAM Gets a PHD: New Concepts in Map Estimation. IEEE Robotics Automation Magazine, 2014. 21(2): p. 26--37.
[5]. Endres, F., et al., 3-D Mapping With an RGB-D Camera. IEEE Transactions on Robotics, 2014. 30(1): p. 177--187.
[6]. Forster, C., M. Pizzoli and D. Scaramuzza, SVO: Fast semi-direct monocular visual odometry. 2014, IEEE. p. 15--22.
[7]. Newcombe, R.A., et al., KinectFusion: Real-time dense surface mapping and tracking. 2011, IEEE. p. 127--136.
即時定位與地圖構建（SLAM）的相關研究
即時定位與地圖構建（SimultaneousLocalization AndMapping）指的是機器人在自身位置不確定的條件下,在完全未知環境中創建地圖,同時利用地圖進行自主定位和導航。
SLAM問題可以描述為:機器人在未知環境中從一個未知位置開始移動,在移動過程中根據位置估計和傳感器數據進行自身定位,同時建造增量式地圖。
（1）定位(localization)：機器人必須知道自己在環境中位置。
（2）建圖(mapping)：機器人必須記錄環境中特征的位置（如果知道自己的位置）
（3）SLAM：機器人在定位的同時建立環境地圖。其基本原理是運過概率統計的方法，通過多特征匹配來達到定位和減少定位誤差的。
圖 SLAM的基本過程
移動機器人自定位與環境建模問題是緊密相關的。環境模型的準確性依賴於定位精度，而定位的實現又離不開環境模型。在未知環境中，機器人沒有什麼參照物，隻能依靠自己並不十分準確的傳感器來獲取外界信息，如同一個盲人在一個陌生環境中摸索的情況。這種情況下，定位是比較困難的。有地圖的定位和有定位的地圖創建都是容易解決的，但無地圖的定位和未解決定位的地圖創建如同"雞--蛋"問題，無從下手。已有的研究中對這類問題的解決方法可分為兩類：一類利用自身攜帶的多種內部傳感器(包括裏程儀、羅盤、加速度計等)，通過多種傳感信息的融合減少定位的誤差，使用的融合算法多為基於卡爾曼濾波的方法。這類方法由於沒有參考外部信息，在長時間的漫遊後誤差的積累會比較大。另一類方法在依靠內部傳感器估計自身運動的同時，使用外部傳感器(如激光測距儀、視覺等)感知環境，對獲得的信息進行分析提取環境特征並保存，在下一步通過對環境特征的比較對自身位置進行校正。但這種方法依賴於能夠取得環境特征。
SLAM的三個基本問題
Leonard和Durrant-Whyte將移動機器人完成任務定義為三個問題“Wheream I?”、“Wheream I going?”和“Howdo I getthere”，就是定位、目標識別和路徑規劃，為了能實現導航，移動機器人需要靠本體感受傳感器和環境感知傳感器來實現對本體位姿估計和外部環境位姿的定位。依據環境空間的描述方法，Desouza等將視覺導航的方法化為三類：
(1)已知地圖的導航(Map-BasedNavigation)：表示地圖的方法幾何特征(GeometricPrimitives)、拓撲特征(TopologicalFeatures)或占據柵格(OccupancyGrids)移動機器人依據這些已知的環境地圖進行導航。
(2)地圖建立的導航(Map-Building-BasedNavigation)：在沒有已知環境地圖的情況下，移動機器人通過自身的導航運動和傳感器的不斷感知更新來進行導航。
(3)未知環境的導航(MaplessNavigation)：相對於上麵兩種方法，在實時的動態環境中無法建立明確的地圖表達形式，更多的是通過傳感器獲得的觀測信息用來識別或者跟蹤環境中的物體來導航。
但是由於感知信息的不確定性，移動機器人很難實現定位的準確，因而，在未知環境中的定位成為最關鍵的問題。
定位(Wheream I?)是實現自主能力的最基本問題，是為了確定機器人在運行環境中相對於世界坐標係的位置及其本身的位姿。
移動機器人的定位與其它領域研究課題的關係如圖所示：
圖 SLAM與各領域關係圖
現有的移動機器人自主定位方法主要是局部定位和全局定位。局部是通過測量相對於機器人初始位姿的距離和方向來確定當前的位姿，但隨著時間的累計造成定位的誤差較大，無法精確定位。全局定位則通過測機器人的絕對位置來定位，定位的精度較高，並且可以用來修正局部定位的定位誤差。
現在移動機器人定位的方法大致可分為三類
(1)相對定位(RelativePositionMeasurements)：主要依靠內部本體感受傳感器如裏程計（Odometry）、陀螺儀(Gyroscopes)等，通過給定初始位姿，來測量相對於機器人初始位姿的距離和方向來確定當前機器人的位姿，也叫做航跡推測(DeadReckoning, DR)。
(2)絕對定位(AbsolutePosition Measurements)：主要采用主動或被動標識(Activeor Passive Beacons)、地圖匹配(MapMatching)、全球定位係統(GlobalPositioning System,GPS)、或導航信標(LandmarkNavigation)進行定位。位置的計算方法包括有三角測量法(Triangulation)、三邊測量法(Trilateration)和模型匹配算法(ModelMatching)等。
(3)組合定位(CombinedPositionMethod)：雖然相對定位這種方法能夠根據運動學模型的自我推算移動機器人的位姿和軌跡而且具有自包含的有點。但是不可避免地會存在隨時間的增加和距離的增加而增加的累積航跡誤差。在絕對定位中，地圖匹配技術處理數據速度較慢，而信標或標識牌的建設和維護成本太高，GPS又隻能在室外使用。由於單一定位的方法的缺陷，移動機器人定位仍然是基於航跡的推算與絕對位姿和軌跡矯正相結合起來。
來源：本文作者半閑居士
最後更新：2017-10-08 08:01:27