什么是SLAM？視覺SLAM怎么實現(xiàn)？

上周的組會上，我給研一的萌新們講解什么是SLAM，為了能讓他們在沒有任何基礎(chǔ)的情況下大致聽懂，PPT只能多圖少字沒公式，這里我就把上周的組會匯報總結(jié)一下。

這次匯報的題目我定為“視覺SLAM：一直在入門，從未能精通”，那是因為視覺SLAM真的是博大精深，就像C++一樣，連說入門都底氣不足，只能說了解，更不敢說精通。

從五月份開始學(xué)《視覺SLAM十四講》算起，我已經(jīng)正式接觸SLAM四個多月了，到現(xiàn)在還是很懵懂的，當(dāng)然也有可能是自己的吸收能力還不夠強吧！

下面就以我這段時間的積累斗膽簡單談?wù)剬σ曈XSLAM的認(rèn)識，如有不當(dāng)，還請指教。

1 什么是SLAM？

SLAM的英文全名叫做Simultaneous Localization and Mapping，中文名是同時定位與建圖，從字面上來看就是同時解決定位和地圖構(gòu)建問題。

定位主要是解決“在什么地方”的問題，比如你目前在哪國哪省哪市哪區(qū)哪路哪棟幾號幾樓哪個房間哪個角落。

建圖主要是解決“周圍環(huán)境是什么樣”的問題，你可以回憶一下百度高德地圖，甚至一些景點的手繪地圖。

下面就以掃地機器人作為例子再稍微詳細(xì)地講解一下。

對于掃地機器人來說，定位就是要知道自己在房間里的具體位置，建圖就是知道整個房間的地面結(jié)構(gòu)信息，而有了這些信息才能做路徑規(guī)劃，以最短的距離到達(dá)目的地。

下面右圖就是掃地機器人在家里移動時的定位和建圖，藍(lán)點是它自己目前所在的位置，它已經(jīng)把房間能抵達(dá)的地方都構(gòu)建出來了。

總的來說，機器人從未知環(huán)境未知地點出發(fā)，通過傳感器（這里主要說的是相機）觀測環(huán)境獲取信息，利用相機的信息估算機器人的位置、姿態(tài)和運動軌跡，并且根據(jù)位姿構(gòu)建地圖，從而實現(xiàn)同時定位和建圖。

利用不同的傳感器實現(xiàn)SLAM的方法不同，目前主流的傳感器有激光雷達(dá)（LiDAR SLAM）、相機（Visual SLAM）和慣性測量單元（Visual-inertial SLAM）。

接下來我們介紹的是視覺SLAM的方法，也就是只采用相機作為傳感器的SLAM。

當(dāng)然，用相機作為傳感器的話，還是有不同的方法，因為相機也有不同的種類，常見的有單目相機、雙目相機和RGB-D相機。

2 為什么用SLAM？

可能你會有疑問了，為什么不用GPS定位？為什么不用現(xiàn)成的地圖？下面就來一一解答。

對于定位來說，我要反問一下，沒有GPS怎么破？比如在一些建筑物內(nèi)、隧道或者偏遠(yuǎn)地方，我們是無法獲取GPS的。這種情況下機器人或者無人車是不是得自己定位了。

再者，GPS的定位精度不夠高，最多也就達(dá)到幾米的精度，要是在室內(nèi)定位的話，一套房就這么巴掌大的地方，幾米的誤差也許就讓你的掃地機器人誤以為是在你鄰居家打掃了。。

對于建圖來說，我還是要反問一下，沒有現(xiàn)成地圖怎么破？比如你的家、公司或者工廠，我們很難直接拿到現(xiàn)成的地圖，家家戶戶的圖紙數(shù)據(jù)量得多大，而且每一戶裝修也不一樣，家私的擺放位置更是無從得知。

而且，不同場景需求下的地圖也是不一樣的，后面我們會提到有各式各樣的地圖，不存在解決各種問題的地圖。

所以，在沒有GPS和現(xiàn)成地圖的場景下，同時考慮到定位的精度和地圖的需求，SLAM對于機器人來說簡直是雪中送炭。

3 視覺SLAM怎么實現(xiàn)？

下面就用高博《視覺SLAM十四講》里的框圖來講解視覺SLAM大致是怎么實現(xiàn)的。

首先通過傳感器（這里利用的是相機）獲取環(huán)境中的數(shù)據(jù)信息，也就是一幀一幀的圖像，在前端視覺里程計中通過這些圖像信息計算出相機的位置（準(zhǔn)確來說是位姿，后面會細(xì)說）。

同時進(jìn)行閉環(huán)檢測，判斷機器人是否到達(dá)先前經(jīng)過的地方。然后利用后端非線性優(yōu)化，對前端得出的相機位姿進(jìn)行優(yōu)化，得到全局最優(yōu)的狀態(tài)。

最后根據(jù)每一時刻的相機位姿和空間中目標(biāo)的信息，根據(jù)需求建立相應(yīng)的地圖。

接下來就來詳細(xì)說說其中每一個模塊都是怎么操作的。

4 前端視覺里程計

視覺里程計的英文名稱是Visual Odometry，簡稱VO，主要是研究怎么通過相鄰兩幀圖像計算兩幀之間相機的運動。

這里面涉及了不少知識，其中包括圖像處理中的特征提取和特征匹配、三維視覺中的剛體運動和對極幾何、數(shù)學(xué)中的李群李代數(shù)等等。。

不要慌！天空飄來五個字，那都不是事！

在視覺SLAM中，主流的方法根據(jù)前端的不同分為特征點法和直接法，下面介紹的是利用特征點法的視覺SLAM。

對于我們?nèi)搜壑庇^判斷來說，從前一張圖像到后一張圖像應(yīng)該是往右上的方向稍微旋轉(zhuǎn)了一下。

但是，對于機器人來說它可沒那么“直觀”。

首先它要對這兩張圖像進(jìn)行特征提取，也就是找到圖像中特別的地方，比如角點、邊緣點等。然后，對這些特征點在兩張圖像之間進(jìn)行特征匹配。

把匹配對中誤匹配的篩選掉之后，就能得到較為準(zhǔn)確的匹配了。

有了這些匹配點對之后，就能利用它們瘋狂計算相機的位姿了。對了，這里得講講位姿究竟是什么東西？

位姿其實就是位置和姿態(tài)的合稱，位置也就是在三維空間中的坐標(biāo)(x,y,z)，而姿態(tài)是在三維空間中的旋轉(zhuǎn)(r,p,y)，因此位姿總共包含6個自由度。

還沒理解？來做做頭部健康運動就明白了，左右歪頭，上下點頭，左右搖頭。怎么樣，既能預(yù)防頸椎病，還能理解三維旋轉(zhuǎn)。

左右歪頭是滾轉(zhuǎn)角roll，上下點頭是俯仰角pitch，左右搖頭則是偏航角yaw。

既然要表示坐標(biāo)，那我們總得知道坐標(biāo)系是什么吧？在相機運動過程中，有四個常見的坐標(biāo)系需要我們了解。

分別是世界坐標(biāo)系、相機坐標(biāo)系、歸一化坐標(biāo)系還有像素坐標(biāo)系。下面這張圖讓人一目了然。

世界坐標(biāo)系比較好理解，就是我們身處的整個三維空間的坐標(biāo)系，坐標(biāo)原點由我們自己定，可以是某一張桌子的邊角，也可以是相機第一時刻的位置。

相機坐標(biāo)系是以相機光心為坐標(biāo)原點，光軸為z軸的坐標(biāo)系。

歸一化坐標(biāo)系就是原點在相機坐標(biāo)系下(0,0,1)處的二維平面坐標(biāo)系。

像素坐標(biāo)系是以圖像左上角的像素為原點，以一個像素為最小單元的離散坐標(biāo)系。

既然有不同的坐標(biāo)，當(dāng)知道點在一個坐標(biāo)系下的坐標(biāo)時，如何求得該點在其他坐標(biāo)系下的坐標(biāo)呢？

舉個栗子，我們能獲取到的是圖像的像素信息，通過轉(zhuǎn)換（相機投影模型）之后能得到該像素（特征點）在相機坐標(biāo)系中的坐標(biāo)位置。

但是在構(gòu)建地圖的時候我們得知道這個像素（特征點）在整個三維空間中的哪個位置呀，也就是相機坐標(biāo)系中的坐標(biāo)怎么轉(zhuǎn)化到世界坐標(biāo)系下。

這就涉及到了三維空間剛體運動中坐標(biāo)系的變換。直接上圖就曉得了。

上圖展示的是世界坐標(biāo)系轉(zhuǎn)化為相機坐標(biāo)系的過程，當(dāng)然方法都是一樣的。

這里獻(xiàn)上整篇文章唯一的一條數(shù)學(xué)公式：

pc是點p在相機坐標(biāo)系下的坐標(biāo)，pw是世界坐標(biāo)，Rcw是描述從世界坐標(biāo)系轉(zhuǎn)化為相機坐標(biāo)系旋轉(zhuǎn)的旋轉(zhuǎn)矩陣，tcw是描述從世界坐標(biāo)系轉(zhuǎn)化相機坐標(biāo)系平移的平移向量。

可以看出，坐標(biāo)系的轉(zhuǎn)換我們可以用一個旋轉(zhuǎn)矩陣R(3*3)來表示旋轉(zhuǎn)，也就是決定姿態(tài)，還有一個平移向量t(3*1)來表示平移，也就是決定位置。

相機的位姿其實就是指相機在世界坐標(biāo)系下的位置坐標(biāo)和旋轉(zhuǎn)姿態(tài)，位姿估計就是根據(jù)兩幀之間匹配點的關(guān)系計算當(dāng)前時刻相機的位姿。

假如我們將第一幀時刻的相機作為世界坐標(biāo)系原點，那么通過第1、2幀圖像的匹配點就可以計算從第2幀相機坐標(biāo)系到第1幀相機坐標(biāo)系（世界坐標(biāo)系）的旋轉(zhuǎn)矩陣R12和平移向量t12。

然后再通過第2、3幀圖像的匹配點計算R23和t23，利用和R12相乘再加上t12就能求得第3幀時刻相機在世界坐標(biāo)系下的位姿。依此類推。。。

當(dāng)然，根據(jù)不同情況可以用不同的方法求R和t：

2D-2D：對極約束，在單目相機中，我們只能獲取二維圖像，利用兩幀圖像的匹配點關(guān)系通過對極幾何的關(guān)系可以求出一個叫本質(zhì)矩陣E的東西，再求得R和t。

3D-3D：ICP（迭代最近法），在雙目和RGB-D相機中，我們可以直接獲取圖像的深度信息，也就是說特征點在相機坐標(biāo)系下的z我們是知道的，這時候其實就相當(dāng)于直接求兩個相機坐標(biāo)系的轉(zhuǎn)換R和t。

3D-2D：PnP，當(dāng)我們知道一組點在世界坐標(biāo)系下的坐標(biāo)和它們在相機中的投影位置時，可以利用這種方法直接求得當(dāng)前時刻的相機位姿。

5 后端優(yōu)化

視覺里程計講得很多了，但是從視覺里程計中計算得到的相機位姿會有一定的誤差，一次兩次還好，一旦多了累積誤差可是很嚴(yán)重的。

這時就需要后端對前端的結(jié)果進(jìn)行優(yōu)化，從而得到最優(yōu)的位姿。

和前端分為兩種主流方法一樣，后端也有兩種解決方法：

濾波器方法，以擴展卡爾曼濾波（EKF）為代表，認(rèn)為某一時刻的狀態(tài)只和上一時刻的狀態(tài)有關(guān)。

非線性優(yōu)化方法，以高斯牛頓法和列文伯格-馬夸爾特法為代表，認(rèn)為某一時刻的狀態(tài)和之前所有狀態(tài)有關(guān)。

我們主要用的是非線性優(yōu)化方法，對相機位姿和路標(biāo)點構(gòu)建最小二乘問題，并利用圖優(yōu)化的方法求解，也就是常說的Bundle Adjustment。

當(dāng)然，因為BA處理的數(shù)據(jù)量很大，在整個SLAM過程中還會采取別的方法控制優(yōu)化的數(shù)據(jù)，比如滑動窗口法。

簡單地說，就是在保持處理的幀數(shù)不變的情況下，將舊的數(shù)據(jù)刪除，加入新的數(shù)據(jù)。

假設(shè)每次只優(yōu)化10幀，那么當(dāng)接收到第11幀圖像時，移除第1幀圖像的數(shù)據(jù)，并將第11幀圖像加入優(yōu)化問題中。

6回環(huán)檢測

回環(huán)檢測（Loop Closure）是一個挺特殊的模塊，主要讓機器人能識別出曾經(jīng)去過的地方。

隨著時間推進(jìn)，SLAM的誤差會不斷地累積，時間久了后，使得軌跡出現(xiàn)嚴(yán)重的漂移。

如果有了回環(huán)檢測，機器人就會檢測到自己曾經(jīng)到過這個地方，利用這個信息和歷史數(shù)據(jù)比對，從而修正累積誤差，得到全局一致的狀態(tài)估計。

為什么說回環(huán)檢測挺特殊的呢？因為這個模塊乍一看還挺像機器學(xué)習(xí)甚至是目前深度神經(jīng)網(wǎng)絡(luò)應(yīng)用的領(lǐng)域。

判斷“兩張圖像是否為同一個地方”會出現(xiàn)4種結(jié)果：

事實是，機器人判斷為是；

事實是，機器人判斷為否；

事實否，機器人判斷為是；

事實否，機器人判斷為否。

最好的結(jié)果當(dāng)然是第1種和第4種，因為機器人的判斷和事實符合，可現(xiàn)實總是不完美的。

而衡量回環(huán)檢測效果的指標(biāo)也有兩種——準(zhǔn)確率（Precision）和召回率（Recall）。

用西瓜書里的話解釋，準(zhǔn)確率關(guān)心的是“挑出的西瓜中有多少比例是好瓜”，召回率關(guān)心的是“所有好瓜中有多少比例被挑了出來”。

西瓜書都搬出來了，這還不是機器學(xué)習(xí)問題嗎？

當(dāng)然，在SLAM中還是更注重召回率的，希望更多“現(xiàn)實是回環(huán)”被機器人“判斷為回環(huán)”。

傳統(tǒng)主流的視覺SLAM中回環(huán)檢測采用的是詞袋模型，當(dāng)然按照目前深度學(xué)習(xí)的勢頭來說，也不妨試試神經(jīng)網(wǎng)絡(luò)，但是實時性就得另當(dāng)別論了。

而且，回環(huán)檢測還可以用來進(jìn)行重定位，解決跟蹤失敗的問題，保證在跟蹤失敗的情況下可以快速重新得到當(dāng)前的精確位姿。

7地圖構(gòu)建

最后一個模塊是地圖構(gòu)建，前面也說過了，我們根據(jù)不同的傳感器類型和應(yīng)用需求可以建立不同的地圖。

還是用高博《視覺SLAM十四講》中的圖，簡潔明了地說明了不同應(yīng)用場景對應(yīng)不同類型的地圖。

如果只是單純的定位，只需要知道機器人在什么位置即可，這時候稀疏路標(biāo)地圖就足夠了。

在導(dǎo)航、避障的情況下，機器人必須知道什么地方可以走而什么地方不能走，這才能規(guī)劃出運動路徑。于是就需要稠密地圖，稀疏路標(biāo)點的地圖壓根就不能判斷那是什么東西。

重建也很明了，既然是重建，那就得是帶有輪廓紋理等詳細(xì)信息的稠密地圖了。

如果是人機交互的話呢，機器人得知道什么是桌子、杯子在哪里等語義信息，這時候光是知道物體什么模樣可不行了，得上語義地圖才是。

我們在做導(dǎo)航時，有一種常用的地圖，那就是八叉樹地圖（Octomap）。

它是把三維空間分為許多方塊，方塊再分為八個同樣大小的小方塊，小方塊再繼續(xù)往下分。。整個三維空間就用八叉樹數(shù)據(jù)結(jié)構(gòu)來表示。

當(dāng)方塊中所有子方塊都被占據(jù)或者都沒被占據(jù)的時候，這個方塊或者說八叉樹中的這個節(jié)點就沒必要往下展開了。

相對與點云地圖來說，這樣會大大減少了地圖的存儲空間！下面就是一張八叉樹地圖。

8 結(jié)語

近兩年，SLAM有要火的趨勢，但是這也不妨礙它毫不平易近人的特質(zhì)。

審核編輯：彭菁