天天看點

視覺SLAM簡介(限于初學者)

經典的運動重建(SFM,Structure From Motion),也叫做同步定位和制圖(simultaneous localization and mapping,SLAM),基本上是假設場景是靜态情況下通過錄影機的運動來擷取圖像序列并得到場景3-D結構的估計,也是計算機視覺的重要任務;在機器人領域,這個任務還會估計現場攝像頭的姿态和位置,即定位任務;下面介紹一下視覺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,反之依然。

視覺SLAM簡介(限于初學者)

(a)對應一條光線的外極線 (b)對應的外極線集合和外極平面

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

視覺SLAM簡介(限于初學者)

那麼

視覺SLAM簡介(限于初學者)

而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為例,給定一組比對特征點(n7)

視覺SLAM簡介(限于初學者)

定義F矩陣元素fij(i=1~3,j=1~3),那麼線性齊次方程組為

視覺SLAM簡介(限于初學者)

簡記為

Qf = 0

做SVD得到Q = USVT,而最終F的解是V最右的奇異向量(singular vector);

另外,因為E矩陣完全由攝像頭外參數(R和t共6個未知參數,但有一個不确定平移尺度量)決定,是以存在5點算法求解E矩陣。

可以分解E得到攝像頭外參數,其步驟如下:

1)同樣對E矩陣做奇異值分解(SVD):

視覺SLAM簡介(限于初學者)

2)那麼第一個攝像頭投影矩陣簡單設為P1=[I|0],而第二個攝像頭矩陣P2有四種可能情況,如圖(a)-(d)所示:

視覺SLAM簡介(限于初學者)

其中

視覺SLAM簡介(限于初學者)
視覺SLAM簡介(限于初學者)
視覺SLAM簡介(限于初學者)

從E矩陣分解得到R和t

根據視圖方向與攝像頭中心到3-D點的方向之間夾角可以發現,四個可能中隻有情況(a)是合理的解。

确定兩個視角的姿态之後,比對的特征點x,x’可以重建其3-D坐标X,即三角化(triangulation)理論;首先存在一個線性解:設兩個攝像頭投影矩陣為P和P’,相應的它們列向量為pi,pi’,i=1~3,則有方程組:

AX = 0

其中

視覺SLAM簡介(限于初學者)

但一些誤差幹擾的存在,上述線性解是不存在的;是以需要一個非線性的解,這裡采用F矩陣定義的外極限制方程xTFx’= 0,得到最小化誤差函數為

視覺SLAM簡介(限于初學者)

下面采用外極線l, l’的來限制定義誤差,如圖所示,将目标函數重寫為

視覺SLAM簡介(限于初學者)
視覺SLAM簡介(限于初學者)

外極線定義的誤差

另外,在已知重建的3-D點集,如何和新視角的2-D圖像特征點比對呢?這個問題解法稱為PnP(Perspective n Points),算法如下(如圖所示):

  1. 首先,3D點表示為4個控制點的權重和;
  2. 控制點坐标是求解的(12)未知數;
  3. 3D點投影到圖像上以控制點坐标建立線性方程;
  4. 控制點坐标表達為零特征向量(null eigenvectors)線性組合;上面組合的權重(i)是新未知數(<4);

5)增加剛體(rigidity)限制以得到i二次方程;

6)根據i數目(無論線性化,或重新線性化)求解。

(注:有時候3-D-2-D比對比3-D之間比對的精度高)

視覺SLAM簡介(限于初學者)

PnP求解的示意圖

這裡需要補充兩個概念,一是魯棒估計的随機樣本共識法(RANSAC,RANdom SAmple Consensus),另一個是全局優化的集束修正法(BA,bundle adjustment)。

RANSAC的目的是在包含異常點(outlier)的資料集上魯棒地拟合一個模型,如圖所示:

  1. 随機選擇(最小)資料點子集并執行個體化(instantiate)模型;
  2. 基于此模型,将所有資料點分類為内點(inlier)或異常點;
  3. 疊代重複1-2步;
  4. 選擇最大的内地集,以此重新估計最終模型。
視覺SLAM簡介(限于初學者)

RANSAC示意圖

BA的目的是優化全局參數估計,對SLAM來說,結構重建的3-D點X和其對應的2-D特征點x,還有估計的視角變換參數(甚至包括攝像頭内參數)P,位于一個重投影(reprojection)誤差函數D最小化架構下(如圖所示),即

視覺SLAM簡介(限于初學者)

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

視覺SLAM簡介(限于初學者)

BA示意圖

在以上基礎之上,再重新審視SLAM過程:

首先要提到概念VO,即視覺裡程計(visual odometry);VO是SLAM的一部分,VO主要是估計視角之間的變換,或者運動參數,它不需要輸出制圖(mapping)的結果,而且BA就是motion-only的模式。

SLAM方法分兩種途徑,一種是遞推濾波器方法,另一種是批處理估計方法:

濾波器方法,比如卡爾曼濾波遞推估計,實際上建立一個狀态空間的觀測模型和狀态轉換(運動)模型;觀察模型描述當攝像頭姿态和地标位置(來自于地圖)已知時觀測(地标)的機率; 運動模型是系統狀态(攝像頭姿态)轉換的機率分布,即馬爾可夫過程; 那麼在遞歸貝葉斯估計中,同時更新系統狀态和建立的地圖,其中融合來自不同視角的觀測來完成制圖,而估計系統狀态可計算攝像頭的姿态,即定位問題。