1. 視覺SLAM 系統概述
SLAM 是Simultaneous Localization and Mapping 的縮寫，中文譯作“同時定位與地圖構建” 。它是指搭載特定傳感器的主體，在沒有環境先驗信息的情況下，於運動過程中建立環境的模型，同時估計自己的運動。如果這裏的傳感器主要爲相機，那就稱爲“視覺SLAM”。
視覺SLAM流程分爲以下幾步
- 傳感器信息讀取。在視覺SLAM 中主要爲相機圖像信息的讀取和預處理。
- 視覺里程計。視覺里程計任務是估算相鄰圖像間相機的運動，以及局部地圖的樣子。
- 後端優化。後端接受不同時刻視覺里程計測量的相機位姿，以及迴環檢測的信息，對它們進行優化，得到全局一致的軌跡和地圖。
- 迴環檢測。迴環檢測判斷機器人是否曾經到達過先前的位置。如果檢測到迴環，它會把信息提供給後端進行處理。
- 建圖。它根據估計的軌跡，建立與任務要求對應的地圖。
2. 前端視覺里程計
視覺里程計根據相鄰圖像的信息，估計出粗略的相機運動，給後端提供較好的初始值。視覺里程計的算法主要分爲兩個大類：特徵點法和直接法。基於特徵點法的前端，長久以來（直到現在）被認爲是視覺里程計的主流方法。它運行穩定，對光照、動態物體不敏感，是目前比較成熟的解決方案。
核心問題：如何根據圖像估計相機運動。
2.1. 特徵點法
特徵點：由關鍵點和描述子兩部分組成。關鍵點是指該特徵點在圖像裏的位置，有些特徵點還具有朝向、大小等信息。描述子通常是一個向量，按照某種人爲設計的方式，描述了該關鍵點周圍像素的信息。描述子是按照“外觀相似的特徵應該有相似的描述子”的原則設計的。
特徵匹配：視覺SLAM 中極爲關鍵的一步，特徵匹配解決了SLAM 中的數據關聯問題，即確定當前看到的路標與之前看到的路標之間的對應關係。通過對圖像與圖像，或者圖像與地圖之間的描述子進行準確的匹配，我們可以爲後續的姿態估計，優化等操作減輕大量負擔。匹配方法：暴力匹配等。
當相機爲單目時，我們只知道2D 的像素座標，因而問題是根據兩組2D 點估計運動。該問題用對極幾何來解決。
當相機爲雙目、RGB-D 時，或者我們通過某種方法得到了距離信息，那問題就是根據兩組3D 點估計運動。該問題通常用ICP 來解決。
如果我們有3D 點和它們在相機的投影位置，也能估計相機的運動。該問題通過PnP求解。
2.1.1. 2D-2D: 對極幾何
假設我們從兩張圖像中，得到了一對配對好的特徵點，如果我們有若干對這樣的匹配點，就可以通過這些二維圖像點的對應關係，恢復出在兩幀之間攝像機的運動。
八點法+奇異值分解即可求得旋轉矩陣R和位移向量t。
2.1.2. 三角測量
在得到運動之後，下一步我們需要用相機的運動估計特徵點的空間位置。在單目SLAM 中，僅通過單張圖像無法獲得像素的深度信息，我們需要通過三角測量（Triangulation）（或三角化）的方法來估計地圖點的深度。
2.1.3. 3D-2D: PnP
PnP（Perspective-n-Point）是求解3D 到2D 點對運動的方法。它描述了當我們知道n 個3D 空間點以及它們的投影位置時，如何估計相機所在的位姿
如果兩張圖像中，其中一張特徵點的3D 位置已知，那麼最少只需三個點對（需要至少一個額外點驗證結果）就可以估計相機運動
在雙目或RGB-D 的視覺里程計中，我們可以直接使用PnP 估計相機運動。而在單目視覺里程計中，必須先進行初始化，然後才能使用PnP
PnP 問題有很多種求解方法，例如用三對點估計位姿的P3P，直接線性變換（DLT），非線性優化構建最小二乘問題並迭代求解
2.1.4. 3D-3D: ICP
假設一組配對好的3D 點（比如對兩個RGB-D 圖像進行了匹配）：
現在，找一個歐氏變換R; t，使得：
這個問題可以用迭代最近點（Iterative Closest Point, ICP）求解
ICP 的求解也分爲兩種方式：利用線性代數的求解（主要是SVD），以及利用非線性優化方式的求解（類似於Bundle Adjustment）。
2.1.5. 特徵點法的缺陷
特徵點法存在的問題：
- 關鍵點的提取與描述子的計算非常耗時
- 使用特徵點時，忽略了除特徵點以外的所有信息。一張圖像有幾十萬個像素，而特徵點只有幾百個。只使用特徵點丟棄了大部分可能有用的圖像信息。
- 相機有時會運動到特徵缺失的地方，往往這些地方沒有明顯的紋理信息。
克服對策： - 只計算關鍵點，不計算描述子。同時，使用光流法（Optical Flow）來跟蹤特徵點的運動。這樣可以迴避計算和匹配描述子帶來的時間，而光流本身的計算時間要小於特徵點的計算與匹配。
- 只計算關鍵點，不計算描述子。同時，使用直接法（Direct Method）來計算特徵點在下一時刻圖像的位置。這同樣可以跳過描述子的計算過程，而且直接法的計算更加簡單。
2.2. 光流法
光流法仍然使用特徵點，只是把匹配描述子替換成了光流跟蹤，估計相機運動時仍使用對極幾何、PnP 或ICP 算法。
光流是一種描述像素隨着時間，在圖像之間運動的方法，計算部分像素運動的稱爲稀疏光流，計算所有像素的稱爲稠密光流。
LK光流是光流法的一種，它對觀測量做了“灰度不變”假設和“某個窗口內的像素具有相同的運動”假設。因而能夠從前後兩幅圖片中追蹤到同一個點的位置移動。
在實際應用中，LK光流的作用就是跟蹤特徵點。與對每一幀提取特徵點相比，使用LK光流只需要提取一次特徵點，後續視頻幀只需要跟蹤就可以了，節約了許多特徵提取時間。
2.3. 直接法
在直接法中，根據圖像的像素灰度信息同時估計相機的運動和點的投影，不要求提取到的點必須爲角點。
直接法的思路是根據當前相機的位姿估計值，來尋找p2 的位置。但若相機位姿不夠好，p2 的外觀和p1 會有明顯差別。於是，爲了減小這個差別，我們優化相機的位姿，來尋找與p1 更相似的p2。光度誤差（Photometric Error），也就是P 的兩個像的亮度誤差：
優化目標爲該誤差的二範數
能夠做這種優化的理由，仍是基於灰度不變假設。在直接法中，假設一個空間點在各個視角下，成像的灰度是不變的。有許多個（比如N 個）空間點Pi，那麼，整個相機位姿估計問題變爲：
然後使用G-N 或L-M 計算增量，迭代求解。
3. 後端優化
前端視覺里程計能給出一個短時間內的軌跡和地圖，但由於不可避免的誤差累積，這個地圖在長時間內是不準確的。所以，在視覺里程計的基礎上，我們還希望構建一個尺度、規模更大的優化問題，以考慮長時間內的最優軌跡和地圖。
3.1. 線性系統和卡爾曼濾波KF
3.2. 非線性系統和擴展的卡爾曼濾波EKF
把卡爾曼濾波器的結果拓展到非線性系統中來，稱爲擴展卡爾曼濾波器（ExtendedKalman Filter，EKF）。通常的做法是，在某個點附近考慮運動方程以及觀測方程的一階泰勒展開，只保留一階項，即線性的部分，然後按照線性系統進行推導。
先定義一個卡爾曼增益Kk：
在卡爾曼增益的基礎上，後驗概率的形式爲：
3.3. 光束法平差BA
所謂的Bundle Adjustment，是指從視覺重建中提煉出最優的3D 模型和相機參數（內參數和外參數）。從每一個特徵點反射出來的幾束光線（bundles of light rays），在我們把相機姿態和特徵點空間位置做出最優的調整(adjustment) 之後，最後收束到相機光心的這個過程，簡稱爲BA。
左側的p 是全局座標系下的三維座標點，右側的us， vs 是該點在圖像平面上的最終像素座標。
系統的觀測方程爲：
z=h(T,p)
其中，T爲相機的位姿變換矩陣,其對應的李代數爲ξ。
則以最小二乘的角度考慮，可得此次觀測的誤差：
e=z-h(T,p)
然後，把其他時刻的觀測量也考慮進來，我們可以給誤差添加一個下標。設zij 爲在位姿ξi 處觀察路標pj 產生的數據，那麼整體的代價函數（Cost Function）爲：
對這個最小二乘進行求解，相當於對位姿和路標同時作了調整，也就是所謂的BA。
3.4. 位姿圖優化
構建一個只有軌跡的圖優化，而位姿節點之間的邊，可以由兩個關鍵幀之間通過特徵匹配之後得到的運動估計來給定初始值。一旦初始估計完成，我們就不再優化那些路標點的位置，而只關心所有的相機位姿之間的聯繫了。通過這種方式，我們省去了大量的特徵點優化的計算，只保留了關鍵幀的軌跡，從而構建了所謂的位姿圖
位姿圖優化中的節點表示相機位姿，邊表示兩個節點之間相對運動的估計。
邊可表示爲
或按李羣的寫法：
然後構建誤差eij：
所有的位姿頂點和位姿——位姿邊構成了一個圖優化，本質上是一個最小二乘問題，優化變量爲各個頂點的位姿，邊來自於位姿觀測約束。記ε 爲所有邊的集合，那麼總體目標函數爲：
我們依然可以用Gauss-Newton、Levenberg-Marquardt 等方法求解此問題，除了用李代數表示優化位姿以外，別的都是相似的。
4. 迴環檢測
4.1. 概述
4.1.1. 迴環檢測的意義
前端提供特徵點的提取和軌跡、地圖的初值，而後端負責對這所有的數據進行優化。然而，如果像VO 那樣僅考慮相鄰時間上的關聯，那麼，之前產生的誤差將不可避免地累計到下一個時刻，使得整個SLAM 會出現累積誤差。長期估計的結果將不可靠，或者說，我們無法構建全局一致的軌跡和地圖。
迴環檢測模塊，能夠給出除了相鄰幀之外的，一些時隔更加久遠的約束。迴環檢測的關鍵，就是如何有效地檢測出相機經過同一個地方這件事。如果能夠成功地檢測這件事，就可以爲後端的Pose Graph 提供更多的有效數據，使之得到更好的估計，特別是得到一個全局一致（Global Consistent）的估計。
4.1.2. 核心問題：
如何計算圖像間的相似性
4.1.3. 準確率和召回率
準確率：算法提取的所有迴環中，確實是真實迴環的概率。
召回率：在所有真實迴環中，被正確檢測出來的概率。
爲了評價算法的好壞，我們會測試它在各種配置下的P 和R 值，然後做出一條Precision-Recall 曲線。當用召回率爲橫軸，用準確率爲縱軸時，我們會關心整條曲線偏向右上方的程度、100% 準確率下的召回率，或者50% 召回率時候的準確率，作爲評價算法的指標。
值得一提的是，在SLAM 中，我們對準確率要求更高，而對召回率則相對寬容一些。由於假陽性的（檢測結果是而實際不是的）迴環將在後端的Pose Graph 中添加根本錯誤的邊，有些時候會導致優化算法給出完全錯誤的結果。而相比之下，召回率低一些，則頂多有部分的迴環沒有被檢測到，地圖可能受一些累積誤差的影響——然而僅需一兩次迴環就可以完全消除它們了。所以說在選擇迴環檢測算法時，我們更傾向於把參數設置地更嚴格一些，或者在檢測之後再加上回環驗證的步驟。
4.2. 詞袋模型
詞袋，也就是Bag-of-Words（BoW），目的是用“圖像上有哪幾種特徵”來描述一個圖像。
字典中的單詞，假設爲w1、w2、w3。然後，對於任意圖像A，根據它們含有的單詞，可記爲：
字典是固定的，所以只要用[1 1 0]T 這個向量就可以表達A 的意義。通過字典和單詞，只需一個向量就可以描述整張圖像了。
同理，用[2 0 1]T 可以描述圖像B。如果只考慮“是否出現”而不考慮數量的話，也可以是[1 0 1]T ，這時候這個向量就是二值的。於是，根據這兩個向量，設計一定的計算方式，就能確定圖像間的相似性了。當然如果對兩個向量求差仍然有一些不同的做法，比如說對於a，b∈ RW，可以計算：
其中範數取L1 範數，即各元素絕對值之和。請注意在兩個向量完全一樣時，我們將得到1；完全相反時（a 爲0 的地方b 爲1）得到0。這樣就定義了兩個描述向量的相似性，也就定義了圖像之間的相似程度。
4.3. 字典創建
4.3.1. K均值算法
字典由很多單詞組成，而每一個單詞代表了一個概念。一個單詞與一個單獨的特徵點不同，它不是從單個圖像上提取出來的，而是某一類特徵的組合。所以，字典生成問題類似於一個聚類問題。當我們有N 個特徵點，想要歸成k 個類，那麼用K-means 來做，主要有以下幾個步驟：
4.3.2. k叉樹字典
使用一種k叉樹來表達字典。它的思路很簡單，類似於層次聚類，是K-means的直接擴展。假定我們有N 個特徵點，希望構建一個深度爲d，每次分叉爲k 的樹，那麼做法如下：
實際上，最終我們仍在葉子層構建了單詞，而樹結構中的中間節點僅供快速查找時使用。這樣一個k 分支，深度爲d 的樹，可以容納kd個單詞。另一方面，在查找某個給定特徵對應的單詞時，只需將它與每個中間結點的聚類中心比較（一共d 次），即可找到最後的單詞，保證了對數級別的查找效率。
4.4. 相似度計算
考慮權重以後，對於某個圖像A，它的特徵點可對應到許多個單詞，組成它的Bag-of-Words：
對於給定的VA和VB，通過某些方式即可比較其相似度。如L1範數：
5. 建圖
所謂地圖，即所有路標點的集合。一旦我們確定了路標點的位置，那就可以說我們完成了建圖。SLAM 作爲一種底層技術，往往是用來爲上層應用提供信息的。應用層面對於“定位”的需求是相似的，他們希望SLAM 提供相機或搭載相機的主體的空間位姿信息。而對於地圖，則存在着許多不同的需求。
稀疏地圖只建模感興趣的部分，也就是前面說了很久的特徵點（路標點）。
稠密地圖是指，建模所有看到過的部分。
在稠密重建，我們需要知道每一個像素點（或大部分像素點）的距離，那麼大致上有以下幾種解決方案：
- 使用單目相機，利用移動相機之後進行三角化，測量像素的距離。
- 使用雙目相機，利用左右目的視差計算像素的距離（多目原理相同）。
- 使用RGB-D 相機直接獲得像素距離。
前兩種方式稱爲立體視覺（Stereo Vision），其中移動單目的又稱爲移動視角的立體視覺（Moving View Stereo）。相比於RGB-D 直接測量的深度，單目和雙目對深度的獲取往往是“費力不討好”的——我們需要花費大量的計算，最後得到一些不怎麼可靠的¬深度估計。當然，RGB-D 也有一些量程、應用範圍和光照的限制，不過相比於單目和雙目的結果，使用RGB-D 進行稠密重建往往是更常見的選擇。而單目雙目的好處，是在目前RGB-D還無法很好應用的室外、大場景場合中，仍能通過立體視覺估計深度信息。
5.1. 單目稠密建圖
在稠密深度圖估計中，我們無法把每個像素都當作特徵點，計算描述子。因此，稠密深度估計問題中，匹配就成爲很重要的一環：如何確定第一張圖的某像素，出現在其他圖裏的位置呢？這需要用到極線搜索和塊匹配技術。然後，當我們知道了某個像素在各個圖中的位置，就能像特徵點那樣，利用三角測量確定它的深度。
左邊的相機觀測到了某個像素p1。由於這是一個單目相機，我們無從知道它的深度，所以假設這個深度可能在某個區域之內，不妨說是某最小值到無窮遠之間：(dmin，+∞)。因此，該像素對應的空間點就分佈在某條線段（本例中是射線）上。在另一個視角（右側相機）看來，這條線段的投影也形成圖像平面上的一條線，我們知道這稱爲極線。
在p1 周圍取一個大小爲w * w 的小塊，然後在極線上也取很多同樣大小的小塊進行比較，就可以一定程度上提高區分性。這就是所謂的塊匹配。
然後計算小塊與小塊間的差異，存在很多計算方法，如
它計算的是兩個小塊的相關性，接近0表示兩個圖像不相似，而接近1表示相似。
5.2. RGB-D稠密建圖
除了使用單目和雙目進行稠密重建之外，在適用範圍內，RGB-D 相機是一種更好的選擇。在RGB-D 相機中可以完全通過傳感器中硬件測量得到深度，無需消耗大量的計算資源來估計它們。並且，RGB-D 的結構光或飛時原理，保證了深度數據對紋理的無關性。即使面對純色的物體，只要它能夠反射光，我們就能測量到它的深度。這亦是RGB-D 傳感器的一大優勢。
利用RGB-D 進行稠密建圖是相對容易的。不過，根據地圖形式不同，也存在着若干種不同的主流建圖方式。最直觀最簡單的方法，就是根據估算的相機位姿，將RGB-D 數據轉化爲點雲（Point Cloud），然後進行拼接，最後得到一個由離散的點組成的點雲地圖（Point Cloud Map）。在此基礎上，如果我們對外觀有進一步的要求，希望估計物體的表面，可以使用三角網格（Mesh），面片（Surfel）進行建圖。另一方面，如果希望知道地圖的障礙物信息並在地圖上導航，亦可通過體素（Voxel）建立佔據網格地圖（Occupancy Map）。