1.? ? 前言
開始做SLAM（機器人同時定位與建圖）研究已經(jīng)近一年了官硝。從一年級開始對這個方向產(chǎn)生興趣魄鸦，到現(xiàn)在為止，也算是對這個領(lǐng)域有了大致的了解丑慎。然而越了解，越覺得這個方向難度很大瓤摧。總體來講有以下幾個原因：
入門資料很少玉吁。雖然國內(nèi)也有不少人在做照弥，但這方面現(xiàn)在沒有太好的入門教程〗保《SLAM for dummies》可以算是一篇这揣。中文資料幾乎沒有。
SLAM研究已進行了三十多年影斑，從上世紀的九十年代開始给赞。其中又有若干歷史分枝和爭論，要把握它的走向就很費工夫矫户。
難以實現(xiàn)片迅。SLAM是一個完整的系統(tǒng)，由許多個分支模塊組成〗粤桑現(xiàn)在經(jīng)典的方案是“圖像前端柑蛇，優(yōu)化后端芥挣，閉環(huán)檢測”的三部曲，很多文獻看完了自己實現(xiàn)不出來耻台。
自己動手編程需要學習大量的先決知識空免。首先你要會C和C++，網(wǎng)上很多代碼還用了11標準的C++盆耽。第二要會用Linux蹋砚。第三要會cmake，vim/emacs及一些編程工具摄杂。第四要會用openCV, PCL, Eigen等第三方庫坝咐。只有學會了這些東西之后，你才能真正上手編一個SLAM系統(tǒng)匙姜。如果你要跑實際機器人畅厢，還要會ROS。
當然氮昧，困難多意味著收獲也多框杜，坎坷的道路才能鍛煉人（比如說走著走著才發(fā)現(xiàn)Linux和C++才是我的真愛之類的。）鑒于目前網(wǎng)上關(guān)于視覺SLAM的資料極少袖肥，我于是想把自己這一年多的經(jīng)驗與大家分享一下咪辱。說的不對的地方請大家批評指正。
這篇文章關(guān)注視覺SLAM椎组，專指用攝像機油狂，Kinect等深度像機來做導航和探索，且主要關(guān)心室內(nèi)部分寸癌。到目前為止专筷，室內(nèi)的視覺SLAM仍處于研究階段，遠未到實際應用的程度蒸苇。一方面磷蛹，編寫和使用視覺SLAM需要大量的專業(yè)知識，算法的實時性未達到實用要求溪烤；另一方面味咳，視覺SLAM生成的地圖（多數(shù)是點云）還不能用來做機器人的路徑規(guī)劃，需要科研人員進一步的探索和研究檬嘀。以下槽驶，我會介紹SLAM的歷史、理論以及實現(xiàn)的方式鸳兽，且主要介紹視覺（Kinect）的實現(xiàn)方式掂铐。
2.????SLAM問題
SLAM，全稱叫做Simultaneous Localization and Mapping，中文叫做同時定位與建圖堡纬。啊不行聂受，這么講下去，這篇文章肯定沒有人讀烤镐，所以我們換一個講法蛋济。
3.????小蘿卜的故事
從前，有一個機器人叫“小蘿卜”炮叶。它長著一雙烏黑發(fā)亮的大眼睛碗旅，叫做Kinect。有一天镜悉，它被邪惡的科學家關(guān)進了一間空屋子祟辟，里面放滿了雜七雜八的東西。
小蘿卜感到很害怕侣肄，因為這個地方他從來沒來過旧困，一點兒也不了解。讓他感到害怕的主要是三個問題：
1.??????????自己在哪里稼锅？
2.??????????這是什么地方吼具？
3.??????????怎么離開這個地方？
在SLAM理論中矩距，第一個問題稱為定位?(Localization)拗盒，第二個稱為建圖?(Mapping)，第三個則是隨后的路徑規(guī)劃锥债。我們希望借助Kinect工具陡蝇，幫小蘿卜解決這個難題。各位同學有什么思路呢哮肚？
4.????Kinect數(shù)據(jù)
要打敗敵人登夫，首先要了解你的武器。不錯允趟，我們先介紹一下Kinect悼嫉。眾所周知這是一款深度相機，你或許還聽說過別的牌子拼窥，但Kinect的價格便宜，測量范圍在3m-12m之間蹋凝，精度約3cm鲁纠，較適合于小蘿卜這樣的室內(nèi)機器人。它采到的圖像是這個樣子的（從左往右依次為rgb圖鳍寂，深度圖與點云圖）：
Kinect的一大優(yōu)勢在于能比較廉價地獲得每個像素的深度值改含，不管是從時間上還是從經(jīng)濟上來說。OK迄汛，有了這些信息捍壤，小蘿卜事實上可以知道它采集到的圖片中骤视，每一個點的3d位置。只要我們事先標定了Kinect鹃觉，或者采用出廠的標定值专酗。
我們把坐標系設(shè)成這個樣子，這也是openCV中采用的默認坐標系盗扇。
o’-uv是圖片坐標系祷肯，o-xyz是Kinect的坐標系。假設(shè)圖片中的點為(u,v)疗隶，對應的三維點位置在(x,y,z)佑笋，那么它們之間的轉(zhuǎn)換關(guān)系是這樣的：
左側(cè)的s為尺度因子，表示從相機光心出去的射線都會落在成像平面的同一個點上斑鼻。如果我們不知道該點的距離蒋纬，那么s就是一個自由變量。但在RGB-D相機中坚弱，我們在Depth圖中知道了這個距離蜀备，它的讀數(shù)dep(u,v)與真實距離相差一個倍數(shù)。如果也記作s史汗，那么：
后一個公式給出了計算三維點的方法琼掠。先從深度圖中讀取深度數(shù)據(jù)（Kinect給的是16位無符號整數(shù)），除掉z方向的縮放因子停撞，這樣你就把一個整數(shù)變到了以米為單位的數(shù)據(jù)瓷蛙。然后，x,y用上面的公式算出戈毒。一點都不難艰猬，就是一個中心點位置和一個焦距而已。f代表焦距埋市，c代表中心冠桃。如果你沒有自己標定你的Kinect，也可以采用默認的值：s=5000, cx = 320, cy=240, fx=fy=525道宅。實際值會有一點偏差食听，但不會太大。
5.????定位問題
知道了Kinect中每個點的位置后污茵，接下來我們要做的樱报，就是根據(jù)兩幀圖像間的差別計算小蘿卜的位移。比如下面兩張圖泞当，后一張是在前一張之后1秒采集到的：
你肯定可以看出迹蛤，小蘿卜往右轉(zhuǎn)過了一定的角度。但究竟轉(zhuǎn)過多少度呢？這就要靠計算機來求解了盗飒。這個問題稱為相機相對姿態(tài)估計嚷量，經(jīng)典的算法是ICP（Iterative Closest Point，迭代最近點）逆趣。這個算法要求知道這兩個圖像間的一組匹配點挎挖，說的通俗點厌均，就是左邊圖像哪些點和右邊是一樣的缭乘。你當然看見那塊黑白相間的板子同時出現(xiàn)在兩張圖像中耿战。在小蘿卜看來，這里牽涉到兩個簡單的問題：特征點的提取和匹配落包。
如果你熟悉計算機視覺部蛇，那你應該聽說過SIFT, SURF之類的特征。不錯咐蝇，要解決定位問題涯鲁，首先要得到兩張圖像的一個匹配。匹配的基礎(chǔ)是圖像的特征有序，下圖就是SIFT提取的關(guān)鍵點與匹配結(jié)果：
對實現(xiàn)代碼感興趣的同學請Google“opencv?匹配”即可抹腿，在openCV的教程上也有很明白的例子。上面的例子可以看出旭寿，我們找到了一些匹配警绩，但其中有些是對的（基本平等的匹配線），有些是錯的盅称。這是由于圖像中存在周期性出現(xiàn)的紋理（黑白塊）肩祥，所以容易搞錯。但這并不是問題缩膝，在接下來的處理中我們會將這些影響消去混狠。
得到了一組匹配點后，我們就可以計算兩個圖像間的轉(zhuǎn)換關(guān)系疾层，也叫PnP問題将饺。它的模型是這樣的：
[uv]=C(Rp+t)(1)(1)[uv]=C(Rp+t)
R為相機的姿態(tài)，C為相機的標定矩陣痛黎。R是不斷運動的予弧，而C則是隨著相機做死的。ICP的模型稍有不同湖饱，但原理上也是計算相機的姿態(tài)矩陣桌肴。原則上，只要有四組匹配點琉历，就可以算這個矩陣。你可以調(diào)用openCV的SolvePnPRANSAC函數(shù)或者PCL的ICP算法來求解。openCV提供的算法是RANSAC（Random Sample Consensus旗笔，隨機采樣一致性）架構(gòu)彪置，可以剔除錯誤匹配。所以代碼實際運行時蝇恶，可以很好地找到匹配點拳魁。以下是一個結(jié)果的示例。
上面兩張圖轉(zhuǎn)過了16.63度撮弧，位移幾乎沒有潘懊。
有同學會說，那只要不斷匹配下去贿衍，定位問題不就解決了嗎授舟？表面上看來，的確是這樣的贸辈，只要我們引入一個關(guān)鍵幀的結(jié)構(gòu)（發(fā)現(xiàn)位移超過一個固定值時释树，定義成一個關(guān)鍵幀）。然后擎淤，把新的圖像與關(guān)鍵幀比較就行了奢啥。至于建圖，就是把這些關(guān)鍵幀的點云拼起來嘴拢，看著還有模有樣桩盲，煞有介事的：
1－200幀的匹配結(jié)果
然而，如果事情真這么簡單席吴，SLAM理論就不用那么多人研究三十多年了（它是從上世紀90年代開始研究的）（上面講的那些東西簡直隨便哪里找個小碩士就能做出來……）赌结。那么，問題難在什么地方呢抢腐？
6.????SLAM端優(yōu)化理論
最麻煩的問題姑曙，就是“噪聲”。這種漸近式的匹配方式迈倍，和那些慣性測量設(shè)備一樣伤靠，存在著累積噪聲。因為我們在不斷地更新關(guān)鍵幀啼染，把新圖像與最近的關(guān)鍵幀比較宴合，從而獲得機器人的位移信息。但是你要想到迹鹅，如果有一個關(guān)鍵幀出現(xiàn)了偏移卦洽，那么剩下的位移估計都會多出一個誤差。這個誤差還會累積斜棚，因為后面的估計都基于前面的機器人位置……哇阀蒂！這后果簡直不堪設(shè)想案么啊（例如，你的機器人往右轉(zhuǎn)了30度蚤霞，再往左轉(zhuǎn)了30度回到原來的位置酗失。然而由于誤差，你算成了向右轉(zhuǎn)29度昧绣，再向左轉(zhuǎn)31度规肴，這樣你構(gòu)建的地圖中，會出現(xiàn)初始位置的兩個“重影”）夜畴。我們能不能想辦法消除這個該死的誤差呢拖刃？
朋友們，這才是SLAM的研究贪绘，前面的可以說是“圖像前端”的處理方法兑牡。我們的解決思路是：如果你和最近的關(guān)鍵幀相比，會導致累計誤差兔簇。那么发绢，我們最好是和更前面的關(guān)鍵幀相比，而且多比較幾個幀垄琐，不要只比較一次边酒。
我們用數(shù)學來描述這個問題。設(shè)：
不要怕狸窘，只有借助數(shù)學才能把這個問題講清楚墩朦。上面的公式中，xp是機器人小蘿卜的位置翻擒，我們假定由n個幀組成氓涣。xL則是路標，在我們的圖像處理過程中就是指SIFT提出來的關(guān)鍵點陋气。如果你做2D SLAM劳吠，那么機器人位置就是x, y加一個轉(zhuǎn)角theta。如果是3D SLAM巩趁，就是x,y,z加一個四元數(shù)姿態(tài)（或者rpy姿態(tài)）痒玩。這個過程叫做參數(shù)化（Parameterization）。
不管你用哪種參數(shù)议慰，后面兩個方程你都需要知道蠢古。前一個叫運動方程，描述機器人怎樣運動别凹。u是機器人的輸入草讶，w是噪聲。這個方程最簡單的形式炉菲，就是你能通過什么方式（碼盤等）獲得兩幀間的位移差堕战，那么這個方程就直接是上一幀與u相加即得坤溃。另外，你也可以完全不用慣性測量設(shè)備践啄，這樣我們就只依靠圖像設(shè)備來估計浇雹，這也是可以的。
后一個方程叫觀測方程屿讽，描述那些路標是怎么來的。你在第i幀看到了第j個路標吠裆，產(chǎn)生了一個測量值伐谈，就是圖像中的橫縱坐標。最后一項是噪聲试疙。偷偷告訴你诵棵，這個方程形式上和上一頁的那個方程是一模一樣的。
在求解SLAM問題前祝旷，我們要看到履澳，我們擁有的數(shù)據(jù)是什么？在上面的模型里怀跛，我們知道的是運動信息u以及觀測z距贷。用示意圖表示出來是這樣的：
我們要求解的，就是根據(jù)這些u和z吻谋，確定所有的xp和xL忠蝗。這就是SLAM問題的理論。從SLAM誕生開始科學家們就一直在解決這個問題漓拾。最初阁最，我們用Kalman濾波器，所以上面的模型（運動方程和觀測方程）被建成這個樣子骇两。直到21世紀初速种，卡爾曼濾波器仍在SLAM系統(tǒng)占據(jù)最主要的地位，Davison經(jīng)典的單目SLAM就是用EKF做的低千。但是后來配阵，出現(xiàn)了基于圖優(yōu)化的SLAM方法，漸漸有取而代之的地位[1]栋操。我們在這里不介紹卡爾曼濾波器闸餐，有興趣的同學可以在wiki上找卡爾曼濾波器，另有一篇中文的《卡爾曼濾波器介紹》也很棒矾芙。由于濾波器方法存儲n個路標要消耗n平方的空間舍沙，在計算量上有點對不住大家。盡管08年有人提出分治法的濾波器能把復雜度弄到O(n) [2]剔宪，但實現(xiàn)手段比較復雜拂铡。我們要介紹那種新興的方法: Graph-based SLAM壹无。
圖優(yōu)化方法把SLAM問題做成了一個優(yōu)化問題。學過運籌學的同學應該明白感帅，優(yōu)化問題對我們有多么重要斗锭。我們不是要求解機器人的位置和路標位置嗎？我們可以先做一個猜測失球，猜想它們大概在什么地方岖是。這其實是不難的。然后呢实苞，將猜測值與運動模型／觀測模型給出的值相比較豺撑，可以算出誤差：
通俗一點地講，例如黔牵，我猜機器人第一幀在(0,0,0)聪轿，第二幀在(0,0,1)。但是u1告訴我機器人往z方向（前方）走了0.9米猾浦，那么運動方程就出現(xiàn)了0.1m的誤差陆错。同時，第一幀中機器人發(fā)現(xiàn)了路標1金赦，它在該機器人圖像的正中間音瓷；第二幀卻發(fā)現(xiàn)它在中間偏右的位置。這時我們猜測機器人只是往前走素邪，也是存在誤差的外莲。至于這個誤差是多少，可以根據(jù)觀測方程算出來兔朦。
我們得到了一堆誤差偷线，把這些誤差平方后加起來（因為單純的誤差有正有負，然而平方誤差可以改成其他的范數(shù)沽甥，只是平方更常用）声邦，就得到了平方誤差和。我們把這個和記作phi摆舟，就是我們優(yōu)化問題的目標函數(shù)亥曹。而優(yōu)化變量就是那些個xp, xL。
改變優(yōu)化變量恨诱，誤差平方和（目標函數(shù)）就會相應地變大或變小媳瞪，我們可以用數(shù)值方法求它們的梯度和二階梯度矩陣，然后用梯度下降法求最優(yōu)值照宝。這些東西學過優(yōu)化的同學都懂的蛇受。
注意到，一次機器人SLAM過程中厕鹃，往往會有成千上萬幀兢仰。而每一幀我們都有幾百個關(guān)鍵點乍丈，一乘就是幾百萬個優(yōu)化變量。這個規(guī)模的優(yōu)化問題放到小蘿卜的機載小破本上可解嗎把将？是的轻专，過去的同學都以為，Graph-based SLAM是無法計算的察蹲。但就在21世紀06请垛，07年后，有些同學發(fā)現(xiàn)了洽议，這個問題規(guī)模沒有想象的那么大叼屠。上面的J和H兩個矩陣是“稀疏矩陣”，于是呢绞铃，我們可以用稀疏代數(shù)的方法來解這個問題∩┦蹋“稀疏”的原因儿捧，在于每一個路標，往往不可能出現(xiàn)在所有運動過程中挑宠，通常只出現(xiàn)在一小部分圖像里菲盾。正是這個稀疏性，使得優(yōu)化思路成為了現(xiàn)實各淀。
優(yōu)化方法利用了所有可以用到的信息（稱為full-SLAM, global SLAM）懒鉴，其精確度要比我們一開始講的幀間匹配高很多。當然計算量也要高一些碎浇。
由于優(yōu)化的稀疏性临谱，人們喜歡用“圖”來表達這個問題。所謂圖奴璃，就是由節(jié)點和邊組成的東西悉默。我寫成G={V,E}，大家就明白了苟穆。V是優(yōu)化變量節(jié)點抄课，E表示運動/觀測方程的約束。什么雳旅，更糊涂了嗎跟磨？那我就上一張圖，來自[3]攒盈。
圖有點模糊抵拘，而且數(shù)學符號和我用的不太一樣，我用它來給大家一個圖優(yōu)化的直觀形象沦童。上圖中仑濒，p是機器人位置叹话，l是路標，z是觀測墩瞳，t是位移驼壶。其中呢，p, l是優(yōu)化變量喉酌，而z,t是優(yōu)化的約束热凹。看起來是不是像一些彈簧連接了一些質(zhì)點呢泪电？因為每個路標不可能出現(xiàn)在每一幀中般妙，所以這個圖是蠻稀疏的。不過相速，“圖”優(yōu)化只是優(yōu)化問題的一個表達形式碟渺，并不影響優(yōu)化的含義。實際解起來時還是要用數(shù)值法找梯度的突诬。這種思路在計算機視覺里苫拍，也叫做Bundle Adjustment。它的具體方法請參見一篇經(jīng)典文章[4]旺隙。
不過绒极，BA的實現(xiàn)方法太復雜，不太建議同學們拿C來寫蔬捷。好在2010年的ICRA上垄提，其他的同學們提供了一個通用的開發(fā)包：g2o [5]。它是有圖優(yōu)化通用求解器周拐，很好用铡俐，我改天再詳細介紹這個軟件包∷傺總之高蜂，我們只要把觀測和運動信息丟到求解器里就行。這個優(yōu)化器會為我們求出機器人的軌跡和路標位置罕容。如下圖备恤，紅點是路標，藍色箭頭是機器人的位置和轉(zhuǎn)角（2D SLAM）锦秒。細心的同學會發(fā)現(xiàn)它往右偏轉(zhuǎn)了一些露泊。：
7.????閉環(huán)檢測
上面提到，僅用幀間匹配最大的問題在于誤差累積旅择，圖優(yōu)化的方法可以有效地減少累計誤差惭笑。然而，如果把所有測量都丟進g2o，計算量還是有點兒大的沉噩。根據(jù)我自己測試捺宗，約10000多條邊，g2o跑起來就有些吃力了川蒙。這樣蚜厉，就有同學說，能把這個圖構(gòu)造地簡潔一些嗎畜眨？我們用不著所有的信息昼牛，只需要把有用的拿出來就行了。
事實上康聂，小蘿卜在探索房間時贰健，經(jīng)常會左轉(zhuǎn)一下，右轉(zhuǎn)一下恬汁。如果在某個時刻他回到了以前去過的地方伶椿，我們就直接與那時候采集的關(guān)鍵幀做比較，可以嗎氓侧？我們說悬垃，可以，而且那是最好的方法甘苍。這個問題叫做閉環(huán)檢測。
閉環(huán)檢測是說烘豌，新來一張圖像時载庭，如何判斷它以前是否在圖像序列中出現(xiàn)過？有兩種思路：一是根據(jù)我們估計的機器人位置廊佩，看是否與以前某個位置鄰近囚聚；二是根據(jù)圖像的外觀，看它是否和以前關(guān)鍵幀相似标锄。目前主流方法是后一種顽铸，因為很多科學家認為前一種依靠有噪聲的位置來減少位置的噪聲，有點循環(huán)論證的意思料皇。后一種方法呢谓松，本質(zhì)上是個模式識別問題（非監(jiān)督聚類，分類）践剂，常用的是Bag-of-Words (BOW)鬼譬。但是BOW需要事先對字典進行訓練，因此SLAM研究者仍在探討有沒有更合適的方法逊脯。
在Kinect SLAM經(jīng)典大作中[6]优质，作者采用了比較簡單的閉環(huán)方法：在前面n個關(guān)鍵幀中隨機采k個，與當前幀兩兩匹配。匹配上后認為出現(xiàn)閉環(huán)巩螃。這個真是相當?shù)暮唵螌嵱醚菰酰室策^得去。
高效的閉環(huán)檢測是SLAM精確求解的基礎(chǔ)避乏。這方面還有很多工作可以做爷耀。
8.????小結(jié)
本文我們介紹了SLAM的基本概念，重點介紹了圖優(yōu)化解決SLAM問題的思路淑际。我最近正在編寫SLAM程序畏纲，它是一個Linux下基于cmake的工程。目前仍在開發(fā)當中春缕。歡迎感興趣的同學來交流研究心得盗胀，我的郵件是：gaoxiang12@mails.tsinghua.edu.cn。
參考文獻
[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
