【SLAM】視覺SLAM：一直在入門，從未到精通
點(diǎn)擊上方“小白學(xué)視覺”，選擇加"星標(biāo)"或“置頂”
重磅干貨，第一時(shí)間送達(dá)
上周的組會(huì)上，我給研一的萌新們講解什么是SLAM，為了能讓他們在沒有任何基礎(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í)間的積累斗膽簡單談?wù)剬σ曈XSLAM的認(rèn)識，如有不當(dāng)，還請指教。
?
SLAM的英文全名叫做Simultaneous Localization and Mapping，中文名是同時(shí)定位與建圖，從字面上來看就是同時(shí)解決定位和地圖構(gòu)建問題。
定位主要是解決“在什么地方”的問題，比如你目前在哪國哪省哪市哪區(qū)哪路哪棟幾號幾樓哪個(gè)房間哪個(gè)角落。
建圖主要是解決“周圍環(huán)境是什么樣”的問題，你可以回憶一下百度高德地圖，甚至一些景點(diǎn)的手繪地圖。
下面就以掃地機(jī)器人作為例子再稍微詳細(xì)地講解一下。
對于掃地機(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ī)）觀測環(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）和慣性測量單元（Visual-inertial SLAM）。
接下來我們介紹的是視覺SLAM的方法，也就是只采用相機(jī)作為傳感器的SLAM。
當(dāng)然，用相機(jī)作為傳感器的話，還是有不同的方法，因?yàn)橄鄼C(jī)也有不同的種類，常見的有單目相機(jī)、雙目相機(jī)和RGB-D相機(jī)。
可能你會(huì)有疑問了，為什么不用GPS定位？為什么不用現(xiàn)成的地圖？下面就來一一解答。
對于定位來說，我要反問一下，沒有GPS怎么破？比如在一些建筑物內(nèi)、隧道或者偏遠(yuǎn)地方，我們是無法獲取GPS的。這種情況下機(jī)器人或者無人車是不是得自己定位了。
再者，GPS的定位精度不夠高，最多也就達(dá)到幾米的精度，要是在室內(nèi)定位的話，一套房就這么巴掌大的地方，幾米的誤差也許就讓你的掃地機(jī)器人誤以為是在你鄰居家打掃了。。
對于建圖來說，我還是要反問一下，沒有現(xiàn)成地圖怎么破？比如你的家、公司或者工廠，我們很難直接拿到現(xiàn)成的地圖，家家戶戶的圖紙數(shù)據(jù)量得多大，而且每一戶裝修也不一樣，家私的擺放位置更是無從得知。
而且，不同場景需求下的地圖也是不一樣的，后面我們會(huì)提到有各式各樣的地圖，不存在解決各種問題的地圖。
所以，在沒有GPS和現(xiàn)成地圖的場景下，同時(shí)考慮到定位的精度和地圖的需求，SLAM對于機(jī)器人來說簡直是雪中送炭。
下面就用高博《視覺SLAM十四講》里的框圖來講解視覺SLAM大致是怎么實(shí)現(xiàn)的。
首先通過傳感器（這里利用的是相機(jī)）獲取環(huán)境中的數(shù)據(jù)信息，也就是一幀一幀的圖像，在前端視覺里程計(jì)中通過這些圖像信息計(jì)算出相機(jī)的位置（準(zhǔn)確來說是位姿，后面會(huì)細(xì)說）。
同時(shí)進(jìn)行閉環(huán)檢測，判斷機(jī)器人是否到達(dá)先前經(jīng)過的地方。然后利用后端非線性優(yōu)化，對前端得出的相機(jī)位姿進(jìn)行優(yōu)化，得到全局最優(yōu)的狀態(tài)。
最后根據(jù)每一時(shí)刻的相機(jī)位姿和空間中目標(biāo)的信息，根據(jù)需求建立相應(yīng)的地圖。
接下來就來詳細(xì)說說其中每一個(gè)模塊都是怎么操作的。
視覺里程計(jì)的英文名稱是Visual Odometry，簡稱VO，主要是研究怎么通過相鄰兩幀圖像計(jì)算兩幀之間相機(jī)的運(yùn)動(dòng)。
這里面涉及了不少知識，其中包括圖像處理中的特征提取和特征匹配、三維視覺中的剛體運(yùn)動(dòng)和對極幾何、數(shù)學(xué)中的李群李代數(shù)等等。。
不要慌！天空飄來五個(gè)字，那都不是事！
在視覺SLAM中，主流的方法根據(jù)前端的不同分為特征點(diǎn)法和直接法，下面介紹的是利用特征點(diǎn)法的視覺SLAM。
你瞅瞅，下面的兩幀圖像之間相機(jī)進(jìn)行了怎樣的運(yùn)動(dòng)？
對于我們?nèi)搜壑庇^判斷來說，從前一張圖像到后一張圖像應(yīng)該是往右上的方向稍微旋轉(zhuǎn)了一下。
但是，對于機(jī)器人來說它可沒那么“直觀”。
首先它要對這兩張圖像進(jìn)行特征提取，也就是找到圖像中特別的地方，比如角點(diǎn)、邊緣點(diǎn)等。然后，對這些特征點(diǎn)在兩張圖像之間進(jìn)行特征匹配。
把匹配對中誤匹配的篩選掉之后，就能得到較為準(zhǔn)確的匹配了，如下圖。
有了這些匹配點(diǎn)對之后，就能利用它們瘋狂計(jì)算相機(jī)的位姿了。對了，這里得講講位姿究竟是什么東西？
位姿其實(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：對極約束，在單目相機(jī)中，我們只能獲取二維圖像，利用兩幀圖像的匹配點(diǎn)關(guān)系通過對極幾何的關(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)和它們在相機(jī)中的投影位置時(shí)，可以利用這種方法直接求得當(dāng)前時(shí)刻的相機(jī)位姿。
視覺里程計(jì)講得很多了，但是從視覺里程計(jì)中計(jì)算得到的相機(jī)位姿會(huì)有一定的誤差，一次兩次還好，一旦多了累積誤差可是很嚴(yán)重的。
這時(shí)就需要后端對前端的結(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)化方法，對相機(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)窗口法。
簡單地說，就是在保持處理的幀數(shù)不變的情況下，將舊的數(shù)據(jù)刪除，加入新的數(shù)據(jù)。
假設(shè)每次只優(yōu)化10幀，那么當(dāng)接收到第11幀圖像時(shí)，移除第1幀圖像的數(shù)據(jù)，并將第11幀圖像加入優(yōu)化問題中。
回環(huán)檢測（Loop Closure）是一個(gè)挺特殊的模塊，主要讓機(jī)器人能識別出曾經(jīng)去過的地方。
隨著時(shí)間推進(jìn)，SLAM的誤差會(huì)不斷地累積，時(shí)間久了后，使得軌跡出現(xiàn)嚴(yán)重的漂移。
如果有了回環(huán)檢測，機(jī)器人就會(huì)檢測到自己曾經(jīng)到過這個(gè)地方，利用這個(gè)信息和歷史數(shù)據(jù)比對，從而修正累積誤差，得到全局一致的狀態(tài)估計(jì)。
為什么說回環(huán)檢測挺特殊的呢？因?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)檢測效果的指標(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)檢測采用的是詞袋模型，當(dāng)然按照目前深度學(xué)習(xí)的勢頭來說，也不妨試試神經(jīng)網(wǎng)絡(luò)，但是實(shí)時(shí)性就得另當(dāng)別論了。
而且，回環(huán)檢測還可以用來進(jìn)行重定位，解決跟蹤失敗的問題，保證在跟蹤失敗的情況下可以快速重新得到當(dāng)前的精確位姿。
最后一個(gè)模塊是地圖構(gòu)建，前面也說過了，我們根據(jù)不同的傳感器類型和應(yīng)用需求可以建立不同的地圖。
還是用高博《視覺SLAM十四講》中的圖，簡潔明了地說明了不同應(yīng)用場景對應(yīng)不同類型的地圖。
如果只是單純的定位，只需要知道機(jī)器人在什么位置即可，這時(shí)候稀疏路標(biāo)地圖就足夠了。
在導(dǎo)航、避障的情況下，機(jī)器人必須知道什么地方可以走而什么地方不能走，這才能規(guī)劃出運(yùn)動(dòng)路徑。于是就需要稠密地圖，稀疏路標(biāo)點(diǎn)的地圖壓根就不能判斷那是什么東西。
重建也很明了，既然是重建，那就得是帶有輪廓紋理等詳細(xì)信息的稠密地圖了。
如果是人機(jī)交互的話呢，機(jī)器人得知道什么是桌子、杯子在哪里等語義信息，這時(shí)候光是知道物體什么模樣可不行了，得上語義地圖才是。
我們在做導(dǎo)航時(shí)，有一種常用的地圖，那就是八叉樹地圖（Octomap）。
它是把三維空間分為許多方塊，方塊再分為八個(gè)同樣大小的小方塊，小方塊再繼續(xù)往下分。。整個(gè)三維空間就用八叉樹數(shù)據(jù)結(jié)構(gòu)來表示。
當(dāng)方塊中所有子方塊都被占據(jù)或者都沒被占據(jù)的時(shí)候，這個(gè)方塊或者說八叉樹中的這個(gè)節(jié)點(diǎn)就沒必要往下展開了。
相對與點(diǎn)云地圖來說，這樣會(huì)大大減少了地圖的存儲(chǔ)空間！下面就是一張八叉樹地圖。
近兩年，SLAM貌似有要火的趨勢，但是這也不妨礙它毫不平易近人的特質(zhì)。
如果要說這四個(gè)多月以來接觸視覺SLAM什么感受的話，吾之拙見實(shí)難表達(dá)其萬一，所以還是用三張圖結(jié)束此文吧。
入門前：
入門中：
入門后：
當(dāng)然，這是開個(gè)小玩笑。最后，愿大家頭發(fā)濃密，歡迎入坑！
下載1：OpenCV-Contrib擴(kuò)展模塊中文版教程 在「小白學(xué)視覺」公眾號后臺(tái)回復(fù)：擴(kuò)展模塊中文教程，即可下載全網(wǎng)第一份OpenCV擴(kuò)展模塊教程中文版，涵蓋擴(kuò)展模塊安裝、SFM算法、立體視覺、目標(biāo)跟蹤、生物視覺、超分辨率處理等二十多章內(nèi)容。 下載2：Python視覺實(shí)戰(zhàn)項(xiàng)目52講 在「小白學(xué)視覺」公眾號后臺(tái)回復(fù)：Python視覺實(shí)戰(zhàn)項(xiàng)目，即可下載包括圖像分割、口罩檢測、車道線檢測、車輛計(jì)數(shù)、添加眼線、車牌識別、字符識別、情緒檢測、文本內(nèi)容提取、面部識別等31個(gè)視覺實(shí)戰(zhàn)項(xiàng)目，助力快速學(xué)校計(jì)算機(jī)視覺。 下載3：OpenCV實(shí)戰(zhàn)項(xiàng)目20講 在「小白學(xué)視覺」公眾號后臺(tái)回復(fù)：OpenCV實(shí)戰(zhàn)項(xiàng)目20講，即可下載含有20個(gè)基于OpenCV實(shí)現(xiàn)20個(gè)實(shí)戰(zhàn)項(xiàng)目，實(shí)現(xiàn)OpenCV學(xué)習(xí)進(jìn)階。 交流群
歡迎加入公眾號讀者群一起和同行交流，目前有SLAM、三維視覺、傳感器、自動(dòng)駕駛、計(jì)算攝影、檢測、分割、識別、醫(yī)學(xué)影像、GAN、算法競賽等微信群（以后會(huì)逐漸細(xì)分），請掃描下面微信號加群，備注：”昵稱+學(xué)校/公司+研究方向“，例如：”張三?+?上海交大?+?視覺SLAM“。請按照格式備注，否則不予通過。添加成功后會(huì)根據(jù)研究方向邀請進(jìn)入相關(guān)微信群。請勿在群內(nèi)發(fā)送廣告，否則會(huì)請出群，謝謝理解~