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的應用場合而定。大體上講，可以分為度量地圖與拓撲地圖兩種。