簡單明瞭,一文入門視覺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 迭代的步進量計算變成如下方程組
原文連結:
來自 “ ITPUB部落格 ” ,連結:http://blog.itpub.net/69946223/viewspace-2654841/,如需轉載,請註明出處,否則將追究法律責任。
相關文章
- SLAM入門之視覺里程計(1):特徵點的匹配SLAM視覺特徵
- 手把手教你製作簡單明瞭的視覺化地圖視覺化地圖
- git簡單明瞭Git
- 視覺SLAM綜述視覺SLAM
- 視覺SLAM漫淡視覺SLAM
- 視覺SLAM技術綜述視覺SLAM
- 2.2 視覺SLAM 實踐:Eigen視覺SLAM
- 簡明 MongoDB 入門教程MongoDB
- 視覺SLAM實戰(一):RGB-D SLAM V2視覺SLAM
- 一文帶您快速入門視覺化分析平臺 Kibana視覺化
- 視覺 SLAM 十四講-基礎數學篇視覺SLAM
- 視覺SLAM漫談 (三): 研究點介紹視覺SLAM
- 【入門】Hello Rust!自學簡明扼要Rust
- Azkaban 簡單入門
- postgresql 簡單入門SQL
- SprintBoot簡單入門boot
- Vue簡單入門Vue
- Kafka簡單入門Kafka
- Mysql 簡單入門MySql
- git簡單入門Git
- Espresso 簡單入門Espresso
- Groovy 簡單入門
- 簡單明瞭的體會構建者模式模式
- [Echarts視覺化] 一.入門篇之簡單繪製中國地圖和貴州地區Echarts視覺化地圖
- 有趣的機器學習:最簡明入門指南機器學習
- 小程式 – 簡單入門
- PWA超簡單入門
- SpringSecurity簡單入門SpringGse
- Quartz - Quartz簡單入門quartz
- AVFoundation 簡單入門二
- rxjs簡單入門JS
- canvas簡單入門(2)Canvas
- ViewModels 簡單入門View
- GitHub簡單入門教程Github
- akka入門-簡單示例
- Rust Rocket簡單入門Rust
- 視覺檢測-VisionPro工具入門視覺
- 新手入門SLAM必收藏的資料SLAM