經典視覺SLAM框架

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

　　整體視覺SLAM流程圖。
　　　　　　　　　　　　　　

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

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

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

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

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

5. 建圖（Mapping）。它根據估計的軌跡，建立與任務要求對應的地圖。

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

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

視覺里程計

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

　　　　　　　　
　　　　　　　　　　　　　　　　　　　相機拍攝到的圖片與人眼反應的運動方向。

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

　　　　　　　　　　　　　　
　　　　　　　　　　　　　　　　　　　　　　　　累積誤差與回環檢測的校正結果。

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

后端優化

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

回環檢測

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

建圖

建圖（Mapping）是指構建地圖的過程。地圖是對環境的描述，但這個描述并不是固定的，需要視SLAM的應用而定。

　　　　　　　　　　　　
　　　　　　　　　　　　　　　　　　　　　　　形形×××的地圖

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

度量地圖（Metric Map）

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

拓撲地圖（Topological Map）

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