經(jīng)典視覺SLAM框架

引言：通過前面的推送我們已經(jīng)對SLAM有了個大體的認識。（初識視覺SLAM）下面來看經(jīng)典的視覺SLAM框架，了解一下視覺SLAM究竟由哪幾個模塊組成。
本文選自《視覺SLAM十四講：從理論到實踐》。

　　整體視覺SLAM流程圖。
　　　　　　　　　　　　　　

　　整個視覺SLAM流程包括以下步驟。

1. 傳感器信息讀取。在視覺SLAM中主要為相機圖像信息的讀取和預(yù)處理。如果是在機器人中，還可能有碼盤、慣性傳感器等信息的讀取和同步。

2. 視覺里程計（Visual Odometry，VO）。視覺里程計的任務(wù)是估算相鄰圖像間相機的運動，以及局部地圖的樣子。VO又稱為前端（Front End）。

3. 后端優(yōu)化（Optimization）。后端接受不同時刻視覺里程計測量的相機位姿，以及回環(huán)檢測的信息，對它們進行優(yōu)化，得到全局一致的軌跡和地圖。由于接在VO之后，又稱為后端（Back End）。

4. 回環(huán)檢測（Loop Closing）?；丨h(huán)檢測判斷機器人是否到達過先前的位置。如果檢測到回環(huán)，它會把信息提供給后端進行處理。

5. 建圖（Mapping）。它根據(jù)估計的軌跡，建立與任務(wù)要求對應(yīng)的地圖。

經(jīng)典的視覺SLAM框架是過去十幾年的研究成果。這個框架本身及其所包含的算法已經(jīng)基本定型，并且已經(jīng)在許多視覺程序庫和機器人程序庫中提供。依靠這些算法，我們能夠構(gòu)建一個視覺SLAM系統(tǒng)，使之在正常的工作環(huán)境里實時定位與建圖。因此，我們說，如果把工作環(huán)境限定在靜態(tài)、剛體，光照變化不明顯、沒有人為干擾的場景，那么，這個SLAM系統(tǒng)是相當(dāng)成熟的了。

　　讀者可能還沒有理解上面幾個模塊的概念，下面就來詳細介紹各個模塊具體的任務(wù)。這里您只需對各模塊有一個直觀的、定性的理解即可。

視覺里程計

　　視覺里程計關(guān)心的是相鄰圖像之間的相機運動，最簡單的情況當(dāng)然是兩張圖像之間的運動關(guān)系。例如，當(dāng)看到下圖時，我們會自然地反應(yīng)出右圖應(yīng)該是左圖向左旋轉(zhuǎn)一定角度的結(jié)果（在視頻情況下感覺會更加自然）。我們不妨思考一下：自己是怎么知道“向左旋轉(zhuǎn)”這件事情的呢？人類早已習(xí)慣于用眼睛探索世界，估計自己的位置，但又往往難以用理性的語言描述我們的直覺。看到時，我們會自然地認為，這個場景中離我們近的是吧臺，遠處是墻壁和黑板。當(dāng)相機向左轉(zhuǎn)動時，吧臺離我們近的部分出現(xiàn)在視野中，而右側(cè)遠處的柜子則移出了視野。通過這些信息，我們判斷相機應(yīng)該是向左旋轉(zhuǎn)了。

　　　　　　　　
　　　　　　　　　　　　　　　　　　　相機拍攝到的圖片與人眼反應(yīng)的運動方向。

　　但是，如果進一步問：能否確定旋轉(zhuǎn)了多少度，平移了多少厘米？我們就很難給出一個確切的答案了。因為我們的直覺對這些具體的數(shù)字并不敏感。但是，在計算機中，又必須精確地測量這段運動信息。所以我們要問：計算機是如何通過圖像確定相機的運動的呢？
　　前面也提過，在計算機視覺領(lǐng)域，人類在直覺上看來十分自然的事情，在計算機視覺中卻非常困難。圖像在計算機里只是一個數(shù)值矩陣。這個矩陣?yán)锉磉_著什么東西，計算機毫無概念（這也正是現(xiàn)在機器學(xué)習(xí)要解決的問題）。而在視覺SLAM中，我們只能看到一個個像素，知道它們是某些空間點在相機的成像平面上投影的結(jié)果。所以，為了定量地估計相機運動，必須先了解相機與空間點的幾何關(guān)系。
　　要講清這個幾何關(guān)系以及VO的實現(xiàn)方法，需要鋪墊一些背景知識。在這里我們先讓讀者對VO有個直觀的概念?，F(xiàn)在只需知道，VO能夠通過相鄰幀間的圖像估計相機運動，并恢復(fù)場景的空間結(jié)構(gòu)。稱它為“里程計”是因為它和實際的里程計一樣，只計算相鄰時刻的運動，而和再往前的過去的信息沒有關(guān)聯(lián)。在這一點上，VO就像一種只有短時間記憶的物種。
　　現(xiàn)在，假定我們已有了一個視覺里程計，估計了兩張圖像間的相機運動。那么，只要把相鄰時刻的運動“串”起來，就構(gòu)成了機器人的運動軌跡，從而解決了定位問題。另一方面，我們根據(jù)每個時刻的相機位置，計算出各像素對應(yīng)的空間點的位置，就得到了地圖。這么說來，有了VO，是不是就解決了SLAM問題呢？
　　視覺里程計確實是SLAM的關(guān)鍵，我們也會花大量的篇幅來介紹它。然而，僅通過視覺里程計來估計軌跡，將不可避免地出現(xiàn)累積漂移（Accumulating Drift）。這是由于視覺里程計（在最簡單的情況下）只估計兩個圖像間的運動造成的。我們知道，每次估計都帶有一定的誤差，而由于里程計的工作方式，先前時刻的誤差將會傳遞到下一時刻，導(dǎo)致經(jīng)過一段時間之后，估計的軌跡將不再準(zhǔn)確。比方說，機器人先向左轉(zhuǎn)，再向右轉(zhuǎn)。由于誤差，我們把第一個估計成了。那我們就會尷尬地發(fā)現(xiàn)，向右轉(zhuǎn)之后機器人的估計位置并沒有回到原點。更糟糕的是，即使之后的估計再準(zhǔn)確，與真實值相比，都會帶上這的誤差。

　　　　　　　　　　　　　　
　　　　　　　　　　　　　　　　　　　　　　　　累積誤差與回環(huán)檢測的校正結(jié)果。

　　這也就是所謂的漂移（Drift）。它將導(dǎo)致我們無法建立一致的地圖。你會發(fā)現(xiàn)原本直的走廊變成了斜的，而原本的直角變成了歪的——這實在是一件很難令人忍受的事情！為了解決漂移問題，我們還需要兩種技術(shù)：后端優(yōu)化（更多時候稱為后端（Back End）。由于主要使用的是優(yōu)化方法，故稱為后端優(yōu)化。）和回環(huán)檢測。回環(huán)檢測負責(zé)把“機器人回到原始位置”的事情檢測出來，而后端優(yōu)化則根據(jù)該信息，校正整個軌跡的形狀。

后端優(yōu)化

　　籠統(tǒng)地說，后端優(yōu)化主要指處理SLAM過程中噪聲的問題。雖然我們很希望所有的數(shù)據(jù)都是準(zhǔn)確的，然而現(xiàn)實中，再精確的傳感器也帶有一定的噪聲。便宜的傳感器測量誤差較大，昂貴的可能會小一些，有的傳感器還會受磁場、溫度的影響。所以，除了解決“如何從圖像估計出相機運動”之外，我們還要關(guān)心這個估計帶有多大的噪聲，這些噪聲是如何從上一時刻傳遞到下一時刻的，而我們又對當(dāng)前的估計有多大的自信。后端優(yōu)化要考慮的問題，就是如何從這些帶有噪聲的數(shù)據(jù)中估計整個系統(tǒng)的狀態(tài)，以及這個狀態(tài)估計的不確定性有多大——這稱為最大后驗概率估計（Maximum-a-Posteriori，MAP）。這里的狀態(tài)既包括機器人自身的軌跡，也包含地圖。
　　相對地，視覺里程計部分有時被稱為“前端”。在SLAM框架中，前端給后端提供待優(yōu)化的數(shù)據(jù)，以及這些數(shù)據(jù)的初始值。而后端負責(zé)整體的優(yōu)化過程，它往往面對的只有數(shù)據(jù)，不必關(guān)心這些數(shù)據(jù)到底來自什么傳感器。在視覺SLAM中，前端和計算機視覺研究領(lǐng)域更為相關(guān)，比如圖像的特征提取與匹配等，后端則主要是濾波與非線性優(yōu)化算法。
　　從歷史意義上來說，現(xiàn)在我們稱為后端優(yōu)化的部分，很長一段時間直接被稱為“SLAM研究”。早期的SLAM問題是一個狀態(tài)估計問題——正是后端優(yōu)化要解決的東西。在最早提出SLAM的一系列論文中，當(dāng)時的人們稱它為“空間狀態(tài)不確定性的估計”（Spatial Uncertainty）。雖然有一些晦澀，但也確實反映出了SLAM問題的本質(zhì)：對運動主體自身和周圍環(huán)境空間不確定性的估計。為了解決SLAM問題，我們需要狀態(tài)估計理論，把定位和建圖的不確定性表達出來，然后采用濾波器或非線性優(yōu)化，估計狀態(tài)的均值和不確定性（方差）。狀態(tài)估計與非線性優(yōu)化的具體內(nèi)容將在第6講、第10講和第11講介紹。讓我們暫時跳過它的原理說明，繼續(xù)往下介紹。

回環(huán)檢測

　　回環(huán)檢測，又稱閉環(huán)檢測（Loop Closure Detection），主要解決位置估計隨時間漂移的問題。怎么解決呢？假設(shè)實際情況下機器人經(jīng)過一段時間的運動后回到了原點，但是由于漂移，它的位置估計值卻沒有回到原點。怎么辦呢？我們想，如果有某種手段，讓機器人知道“回到了原點”這件事，或者把“原點”識別出來，我們再把位置估計值“拉”過去，就可以消除漂移了。這就是所謂的回環(huán)檢測。
　　回環(huán)檢測與“定位”和“建圖”二者都有密切的關(guān)系。事實上，我們認為，地圖存在的主要意義是讓機器人知曉自己到過的地方。為了實現(xiàn)回環(huán)檢測，我們需要讓機器人具有識別到過的場景的能力。它的實現(xiàn)手段有很多。例如像前面說的那樣，我們可以在機器人下方設(shè)置一個標(biāo)志物（如一張二維碼圖片）。它只要看到了這個標(biāo)志，就知道自己回到了原點。但是，該標(biāo)志物實質(zhì)上是一種環(huán)境中的傳感器，對應(yīng)用環(huán)境做了限制（萬一不能貼二維碼怎么辦？）。我們更希望機器人能使用攜帶的傳感器——也就是圖像本身，來完成這一任務(wù)。例如，可以判斷圖像間的相似性來完成回環(huán)檢測。這一點和人是相似的。當(dāng)我們看到兩張相似的圖片時，容易辨認它們來自同一個地方。如果回環(huán)檢測成功，可以顯著地減小累積誤差。所以，視覺回環(huán)檢測實質(zhì)上是一種計算圖像數(shù)據(jù)相似性的算法。由于圖像的信息非常豐富，使得正確檢測回環(huán)的難度降低了不少。
　　在檢測到回環(huán)之后，我們會把“A與B是同一個點”這樣的信息告訴后端優(yōu)化算法。然后，后端根據(jù)這些新的信息，把軌跡和地圖調(diào)整到符合回環(huán)檢測結(jié)果的樣子。這樣，如果我們有充分而且正確的回環(huán)檢測，就可以消除累積誤差，得到全局一致的軌跡和地圖。

建圖

建圖（Mapping）是指構(gòu)建地圖的過程。地圖是對環(huán)境的描述，但這個描述并不是固定的，需要視SLAM的應(yīng)用而定。

　　　　　　　　　　　　
　　　　　　　　　　　　　　　　　　　　　　　形形×××的地圖

　　對于家用掃地機器人來說，這種主要在低矮平面里運動的機器人，只需要一個二維的地圖，標(biāo)記哪里可以通過，哪里存在障礙物，就夠它在一定范圍內(nèi)導(dǎo)航了。而對于一個相機，它有6自由度的運動，我們至少需要一張三維的地圖。有些時候，我們想要一個漂亮的重建結(jié)果，不僅是一組空間點，還需要帶紋理的三角面片。另一些時候，我們又不關(guān)心地圖的樣子，只需要知道“A點到B點可通過，而B點到C點不行”這樣的事情。甚至，有時不需要地圖，或者地圖可以由其他人提供，例如，行駛的車輛往往可以得到已繪制好的當(dāng)?shù)氐貓D。
　　對于地圖，我們有太多的想法和需求。因此，相比于前面提到的視覺里程計、回環(huán)檢測和后端優(yōu)化，建圖并沒有一個固定的形式和算法。一組空間點的集合也可以稱為地圖，一個漂亮的3D模型亦是地圖，一個標(biāo)記著城市、村莊、鐵路、河道的圖片還是地圖。地圖的形式隨SLAM的應(yīng)用場合而定。大體上講，可以分為度量地圖與拓撲地圖兩種。

度量地圖（Metric Map）

　　度量地圖強調(diào)精確地表示地圖中物體的位置關(guān)系，通常用稀疏（Sparse）與稠密（Dense）對其分類。稀疏地圖進行了一定程度的抽象，并不需要表達所有的物體。例如，我們選擇一部分具有代表意義的東西，稱之為路標(biāo)（Landmark），那么一張稀疏地圖就是由路標(biāo)組成的地圖，而不是路標(biāo)的部分就可以忽略掉。相對地，稠密地圖著重于建模所有看到的東西。對于定位來說，稀疏路標(biāo)地圖就足夠了。而用于導(dǎo)航時，則往往需要稠密的地圖（否則撞上兩個路標(biāo)之間的墻怎么辦？）。稠密地圖通常按照某種分辨率，由許多個小塊組成。對于二維度量地圖是許多個小格子（Grid），而對于三維度量地圖則是許多小方塊（Voxel）。一般地，一個小塊含有占據(jù)、空閑、未知三種狀態(tài)，以表達該格內(nèi)是否有物體。當(dāng)查詢某個空間位置時，地圖能夠給出該位置是否可以通過的信息。這樣的地圖可以用于各種導(dǎo)航算法，如A*、D*等，為機器人研究者所重視。但是我們也看到，這種地圖需要存儲每一個格點的狀態(tài)，會耗費大量的存儲空間，而且多數(shù)情況下地圖的許多細節(jié)部分是無用的。另一方面，大規(guī)模度量地圖有時會出現(xiàn)一致性問題。很小的一點轉(zhuǎn)向誤差，可能會導(dǎo)致兩間屋子的墻出現(xiàn)重疊，使地圖失效。

拓撲地圖（Topological Map）

　　相比于度量地圖的精確性，拓撲地圖則更強調(diào)地圖元素之間的關(guān)系。拓撲地圖是一個圖（Graph），由節(jié)點和邊組成，只考慮節(jié)點間的連通性，例如A、B點是連通的，而不考慮如何從A點到達B點。它放松了地圖對精確位置的需要，去掉了地圖的細節(jié)問題，是一種更為緊湊的表達方式。然而，拓撲地圖不擅長表達具有復(fù)雜結(jié)構(gòu)的地圖。如何對地圖進行分割形成結(jié)點與邊，又如何使用拓撲地圖進行導(dǎo)航與路徑規(guī)劃，仍是有待研究的問題。
　　本文選自《視覺SLAM十四講：從理論到實踐》，點此鏈接可在博文視點官網(wǎng)查看此書。
　　　　　　　　　　　　　　　　　　　　
　　想及時獲得更多精彩文章，可在微信中搜索“博文視點”或者掃描下方二維碼并關(guān)注。