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

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

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

下面就以我這段時(shí)間的積累斗膽簡(jiǎn)單談?wù)剬?duì)視覺SLAM的認(rèn)識(shí)，如有不當(dāng)，還請(qǐng)指教。

1 什么是SLAM？

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

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

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

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

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

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

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

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

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

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

2 為什么用SLAM？

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

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

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

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

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

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

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

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

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

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

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

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

4 前端視覺里程計(jì)

視覺里程計(jì)的英文名稱是Visual Odometry，簡(jiǎn)稱VO，主要是研究怎么通過相鄰兩幀圖像計(jì)算兩幀之間相機(jī)的運(yùn)動(dòng)。

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

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

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

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

但是，對(duì)于機(jī)器人來說它可沒那么“直觀”。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

5 后端優(yōu)化

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

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

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

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

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

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

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

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

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

6回環(huán)檢測(cè)

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

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

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

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

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

事實(shí)是，機(jī)器人判斷為是；

事實(shí)是，機(jī)器人判斷為否；

事實(shí)否，機(jī)器人判斷為是；

事實(shí)否，機(jī)器人判斷為否。

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

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

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

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

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

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

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

7地圖構(gòu)建

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

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

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

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

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

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

我們?cè)谧鰧?dǎo)航時(shí)，有一種常用的地圖，那就是八叉樹地圖（Octomap）。

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

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

相對(duì)與點(diǎn)云地圖來說，這樣會(huì)大大減少了地圖的存儲(chǔ)空間！下面就是一張八叉樹地圖。

8 結(jié)語

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

審核編輯：彭菁