簡單明瞭，一文入門視覺SLAM

作者 | 黃浴轉載自知乎

【導讀】SLAM是“Simultaneous Localization And Mapping”的縮寫，可譯為同步定位與建圖。最早，SLAM 主要用在機器人領域，是為了在沒有任何先驗知識的情況下，根據感測器資料實時構建周圍環境地圖，同時根據這個地圖推測自身的定位。因此本文以簡單清晰的文字為大家介紹了視覺 V-SLAM。

簡單的V-SLAM介紹，就當入門：）。

首先，從雙目立體幾何（stereo geometry）原理開始來定義 外極（epipolar）約束：兩個攝像頭光心分別是 c0 和 c1，3-D 空間點 p 在兩個影像平面的投影點分別是 x0 和 x1，那麼直線 c0c1 和兩個影像平面的交點即 外極點（epipole）e0 和 e1，pc0c1 平面稱為 外極平面（epipolar plane），它和兩個影像平面的交線 l0 和 l1 即 外極線（epipolar line）；可以看到兩個攝像頭座標系之間的轉換滿足（R，t），同時說明攝像頭 c0 的影像點 x0 在攝像頭 c1 的影像對應點 x1 一定落在其外極線 l1，反之依然；

（a）對應一條光線的外極線 （b）對應的外極線集合和外極平面 外極線約束

這裡需要定義一個 本質矩陣（essential matrix）E 和一個 基礎矩陣（fundamental matrix）F：E = [t]×R，其中 [t]× 是反對稱矩陣，即定義向量

那麼

而 F = A−TEA−1，其中 A 是攝像頭內引數矩陣；對矩陣 E 和矩陣 F 來說，均滿足外極約束方程 x1TEx0 =0，x1TFx0 =0

前者是攝像頭已標定情況下影像特徵點的對應關係，後者是攝像頭未標定情況下影像特徵點的對應關係；

其次，得到外極線約束（以F矩陣為例，E矩陣同樣）如下

l1=Fx0，l0=x1F，

以及，外極點約束（以F矩陣為例，E矩陣同樣）如下

Fe0=0，FTe1=0；

根據上面影像特徵點的外極約束方程，有  8 點演算法求解 F 和 E，以 F 為例，給定一組匹配特徵點（n > 7）

定義F矩陣元素fij（i=1～3，j=1～3），那麼線性齊次方程組為

簡記為

Qf = 0

做SVD得到Q = USVT，而最終F的解是V最右的奇異向量（singular vector）；

另外，因為E矩陣完全由攝像頭外引數（R和t共6個未知引數，但有一個不確定平移尺度量）決定，所以存在  5 點演算法求解  E 矩陣；

可以 分解E得到攝像頭外引數，其步驟如下：

i. 同樣對E矩陣做奇異值分解（SVD）：

ii. 那麼第一個攝像頭投影矩陣簡單設為

而第二個攝像頭矩陣P2有四種可能情況，如圖（a）-（d）所示：

其中

從E矩陣分解得到R和t

根據檢視方向與攝像頭中心到3-D點的方向之間夾角可以發現，四個可能中只有情況（a）是合理的解；

確定兩個視角的姿態之後，匹配的特徵點 x，x’可以重建其 3-D 座標  X，即 三角化（triangulation）理論；首先存在一個線性解：設兩個攝像頭投影矩陣為 P 和 P’，相應的它們列向量為  pi，pi’，i=1～3，則有方程組： AX = 0

其中

但一些誤差干擾的存在，上述線性解是不存在的；所以需要一個非線性的解，這裡採用  F 矩陣定義的外極約束方程  xT Fx’= 0，得到最小化誤差函式為

下面採用外極線 l, l’的來約束定義誤差，如圖所示，將目標函式重寫為

外極線定義的誤差

另外，在已知重建的 3-D 點集，如何和新視角的 2-D 影像特徵點匹配呢？這個問題解法稱為 PnP（Perspective n Points），演算法如下：

• i. 首先，3D 點表示為 4 個控制點的加權和；
• ii. 控制點座標是求解的（12）未知數；
• iii. 3D點投影到影像上以控制點座標建立線性方程；
• iv. 控制點座標表達為零特徵向量（null eigenvectors）線性組合；
• v. 上面組合的權重（bi）是新未知數（<4）；
• vi. 增加剛體（rigidity）約束以得到bi二次方程；
• vii. 根據bi數目（無論線性化，或重新線性化）求解。
• （注：有時候3-D-2-D匹配比3-D之間匹配的精度高）

PnP求解的示意圖

這裡需要補充兩個概念，一是魯棒估計的 隨機樣本共識法（ RANSAC，RANdom SAmple Consensus），另一個是全域性優化的 集束脩正法（ BA，bundle adjustment）：

i. RANSAC的目的是在包含異常點（outlier）的資料集上魯棒地擬合一個模型，如圖 2-12 所示：

• 1. 隨機選擇（最小）資料點子集並例項化（instantiate）模型;
• 2. 基於此模型，將所有資料點分類為內點（inlier）或異常點；
• 3. 迭代重複 1-2 步；
• 4. 選擇最大的內地集，以此重新估計最終模型。

RANSAC示意圖

ii. BA的目的是優化全域性引數估計，對 SLAM 來說，結構重建的 3-D 點  X 和其對應的 2-D 特徵點  x ，還有估計的視角變換引數（甚至包括攝像頭內引數）  P ，位於一個重投影（reprojection）誤差函式 D 最小化框架下（如圖所示），即

這裡集束（Bundle）指2-D點和3-D點之間的光線集，而修正（Adjustment）是指全域性優化過程；其解法是非線性迭代的梯度下降法，如Gauss-Newton 方法和其修正 Levenberg-Marquardt 方法，因為問題自身的特性，這裡的雅可比矩陣是非常稀疏的；另外，只取重建 3-D 點修正的話，稱為 structure only BA，而只取視角變換修正的話，稱為 motion-only BA；

在以上基礎之上，再重新審視SLAM過程：

i. 首先要提到概念 VO，即視覺里程計（visual odometry）；VO 是 SLAM 的一部分，VO 主要是估計視角之間的變換，或者運動引數，它不需要輸出製圖（mapping）的結果，而且 BA 就是 motion-only 的模式；

ii. SLAM 方法分兩種途徑，一種是遞推濾波器方法，另一種是批處理估計方法；

a)  濾波器方法，比如卡爾曼濾波遞推估計，實際上建立一個狀態空間的觀測模型和狀態轉換（運動）模型；觀察模型描述當攝像頭姿態和地標位置（來自於地圖）已知時觀測（地標）的概率; 運動模型是系統狀態（攝像頭姿態）轉換的概率分佈，即馬爾可夫過程; 那麼在遞迴貝葉斯估計中，同時更新系統狀態和建立的地圖，其中融合來自不同視角的觀測來完成製圖，而估計系統狀態可計算攝像頭的姿態，即定位問題;

b)  批處理估計方法，也稱“ 關鍵幀”方法，其步驟是：

• 1）首先通過選擇的頭兩（關鍵）幀，採用雙目幾何的特徵點匹配得到初始的3-D點雲重建；
• 2）正常模式：假設 3D 地圖可用，並且估計出攝像頭增量運動，跟蹤特徵點並使用 PnP 進行攝像頭姿態估計；
• 3）恢復模式：假設 3D 地圖可用，但跟蹤失敗故沒有增量運動，可相對於先前重建的地圖重新定位（re-localize）攝像頭姿勢；
• 4）關鍵幀 BA：保持一個“關鍵幀”子集，狀態向量是所有關鍵幀的 3D 地標和對應攝像頭姿勢，BA 可以在與跟蹤模組並列的執行緒中調整狀態估計；
• （注意：關鍵幀的選擇策略是演算法效能很重要的一個因素）

SLAM 中的 閉環（loop closure）問題：當攝像頭又回到原來位置，稱為閉環，需要檢測閉環，並以此在 BA 框架下優化結構和運動估計；閉環檢測和重定位是類似的，可以基於影像特徵的匹配實現，俗稱“基於影像的重定位（image-based re-localization），當關鍵幀子集較大的時候，需要對特徵匹配進行壓縮和加速，比如詞包（bag of words）法和K維-樹（KD-tree）資料結構等等；

SLAM 中的感測器可以是單目、雙目、深度感測器（RGB-D）甚至鐳射雷達，也可以和 IMU 融合，稱為 VINS（visual inertial navigation system）。

附錄：G-N 和 L-M 的非線性最小二乘演算法

假設有觀測向量 zi’，其預測模型為 zi = zi(x)，其中x為模型引數；那麼最小二乘（LS）法就是最小化如下代價函式：平方誤差加權和（weighted Sum of Squared Error，SSE)

其中 Wi 是一個任意對稱正定（symmetric positive definite，SPD) 矩陣，特徵誤差函式為

雅可比矩陣  J 和黑森（Hessian）矩陣  H 的計算為

那麼 H 近似為

H ≈ J⊤ W J

作為梯度下降法，其 G-N 迭代的步進量即 z → z + delta z，由下面方程組計算

對於上面 G-N 的迭代步進量計算，可能左邊的矩陣不可逆。為此，一種改進的方法是在對角線元素中加入阻尼因子（Damped factor），即 L-M 迭代的步進量計算變成如下方程組

原文連結：

https://zhuanlan.zhihu.com/p/67848295

https://www.toutiao.com/a6728204634000720392/

來自 “ ITPUB部落格 ” ，連結：http://blog.itpub.net/69946223/viewspace-2654841/，如需轉載，請註明出處，否則將追究法律責任。