第七講: 視覺里程計1

第七講: 視覺里程計1

文章目錄

• 第七講: 視覺里程計1
• 1. 特徵點法
• 1.1. 特徵點
• 1.2. ORB特徵
• 1.3. 特徵匹配
• 2. 實踐: 特徵提取和匹配
• 3. D-2D:對極幾何
• 3.1. 對極約束
• 3.2. 本質矩陣
• 3.3. 單應矩陣
• 4. 實踐:對極約束求解相機運動
• 5. 三角測量
• 6. 實踐:三角測量
• 6.1. 三角測量代碼
• 6.2. 討論
• 7. D-2D:PnP
• 7.1. 直接線性優化(6對匹配點)
• 7.2. P3P(3對匹配點)
• 7.3. Bundle adjustment
• 8. 實踐:求解UPnP
• 8.1. 使用ePnP求解
• 8.2. 使用BA優化
• 9. D-3D:ICP
• 9.1. SVD
• 9.2. 非線性優化
• 10. 實踐:求解ICP
• 10.1. SVD方法
• 10.2. 非線性優化方法

1. 特徵點法

slam分爲視覺前端和視覺後端.前端也稱爲視覺里程計(VO). 它根據相鄰圖像的信息粗略估計相機的運動,給後端提供較好的初始值.

本講學習如果提取匹配特徵點,然後估計兩幀之間的相機運動和場景結構.

1.1. 特徵點

圖像本身是由亮度和色彩組成的矩陣.

特徵點:

• 樸素角點: 簡單,受環境,相機旋轉等影響.
• 人工設計的特徵點: 可重複性,可區別性,高效率性,本地性.

特徵點是由關鍵點和描述子組成:

• 關鍵點: 圖像中的位置
• 描述子: 附加信息,爲了更好的區別其他點.

描述子通常是一個向量,按照某種人爲設計的方式,描述了該關鍵點周圍像素的信息.設計原則: 外觀相似的特徵應該有相似的描述子.

ORB(Oriented FAST and Rotated BRIEF) 是目前非常具有代表性的實時圖像特徵.

ORB是質量與性能較好的折中.

1.2. ORB特徵

ORB由關鍵點和描述子組成.

關鍵點稱爲: Oriented FAST.是一種改進的FAST角點.

描述子稱爲: BRIEF(Binary Robust Independent Elementary Feature).

提取ORB特徵點的步驟爲:

• FAST角點提取: 找出圖像中的角點. 相較於原版FAST,ORB中計算了特徵點的主方向,爲後續的BRIEF描述子增加了旋轉不變性.
• BRIEF描述子: 對前一步中的特徵點的周圍進行描述.

FAST 關鍵點:

FAST是一種角點,主要檢測局部像素灰度變化明顯的地方.特點是速度快.

$FAST特徵點: \begin{cases} 優點: 僅比較像素間亮度的差異速度快 \\ 缺點: \begin{cases} 特徵點很多且不確定-解決方法:指定要提取的角點數N,選取前N個具有最大響應值的點作爲最終角點的 合 \\ 不具有方向信息-解決方法: 添加旋轉描述,其特徵的旋轉由灰度實心法實現 \\ 存在尺度問題-解決方法: 添加尺度描述,尺度不變性由構建圖像金字塔,並在金字塔每層檢驗角點來實現. \end{cases} \end{cases}$FAST特徵點:⎩⎪⎪⎪⎨⎪⎪⎪⎧​優點:僅比較像素間亮度的差異速度快缺點:⎩⎪⎨⎪⎧​特徵點很多且不確定−解決方法:指定要提取的角點數N,選取前N個具有最大響應值的點作爲最終角點的合不具有方向信息−解決方法:添加旋轉描述,其特徵的旋轉由灰度實心法實現存在尺度問題−解決方法:添加尺度描述,尺度不變性由構建圖像金字塔,並在金字塔每層檢驗角點來實現.​​

質心:圖像塊灰度值作爲質心.

操作步驟如下:

1. 在一個小的圖像塊B中,定義圖像塊的矩陣爲:

   $m_{pq} = \sum_{x,y \in B}x^py^qI(x,y),\qquad p,q=\{0,1\}$mpq​=x,y∈B∑​xpyqI(x,y),p,q={0,1}

2. 通過矩可以找到圖像塊的質心

   $C = (\frac{m_{10}}{m_{00}},\frac{m_{01}}{m_{00}})$C=(m00​m10​​,m00​m01​​)

3. 連接圖形塊的幾何中心O與質心C, 得到一個方向向量 $\overrightarrow{OC}$OC ,於是特徵點的方向可以定義爲:

   $\theta = \arctan(\frac{m_{01}}{m_{10}})$θ=arctan(m10​m01​​)

通過以上信息,FAST便具有了尺度與旋轉的描述.提升了健壯性. 在ORB中把這種改進後的FAST稱爲:Oriented FAST.

BRIEF描述子:

提取關鍵點後對每個點計算其描述子.

BRIEF是一種二進制描述子. 例如,取關鍵點附近的p和q,如果p比q大,則取1,否則取0,如果去了128對這樣的點,那麼描述子可以用一個128位的二進制數來表示.

與原始的BRIEF相比,ORB描述子具有良好的旋轉不變性.

1.3. 特徵匹配

特徵匹配解決了SLAM中數據關聯問題,即確定當前看到的路標與之前看到的路標之間的對於關係.

通過對圖像與圖像之間或者圖像與地圖之間的描述進行準確匹配,我們可以爲後續姿態估計,優化等操作減輕大量負擔.

如何兩個圖片中特徵點集合的對應關係呢?

• 暴力匹配: 對每個特徵點都測量描述子的距離,然後排序,取得最近的一個作爲匹配點.

描述子的距離,
對於浮點型的描述子使用歐氏距離
對於二進制類型的描述子使用漢明距離

• 快速近似最鄰近匹配: 更適合與匹配點的情況.它已經集成到了opencv當中.

2. 實踐: 特徵提取和匹配

目前主流的幾種特徵提速方法在OpenCV中已經集成.

以下爲openCV中圖像特徵提取,計算和匹配的過程.

代碼見:code\第七講

我們希望根據匹配的點對來估計相機的運動.

1. 當相機爲單目時,我們只知道2D的像素座標,因而問題是根據兩組2D點估計運動.該問題用對極幾何來解決.
2. 當相機爲雙目,RGB等,即有距離信息. 那麼就根據3D點估計運動. 常用ICP來解決.
3. 如果有3D點及在其他相機的投影位置,也能估計相機的運動. 該問題是通過PnP求解.

3. D-2D:對極幾何

3.1. 對極約束

在第一幀的座標系下,設P的空間位置爲;
$P = [X,Y,Z]^T$P=[X,Y,Z]T

根據第五講針孔相機模型,我們知道兩個像素點 $p_1,p_2$p1​,p2​的像素位置爲:
$s_1p_1 = KP , \quad s_2p_2 = K(RP+t)$s1​p1​=KP,s2​p2​=K(RP+t)

K爲相機的內參矩陣. $R,t$R,t爲相機的運動,也可以寫成李代數的形式.

如果使用齊次座標,那麼上式也可以寫成如下形式;
$p_1 = KP , \quad p_2 = K(RP+t)$p1​=KP,p2​=K(RP+t)

取:
$x_1 = K^{-1}p_1, \quad x_2 = K^{-1}p_2$x1​=K−1p1​,x2​=K−1p2​

$x_1,x_2$x1​,x2​是兩個像素點 歸一化平面的座標.帶入上式得:
$x_2 = Rx_1 + t$x2​=Rx1​+t

兩邊同時左乘 $t^\wedge$t∧(^表示將向量變成矩陣),相當於兩側同時與t做外積:
$t^\wedge x_2 = t^\wedge Rx_1$t∧x2​=t∧Rx1​

然後同時左乘 $x_2^T$x2T​
$x_2^Tt^\wedge x_2 = x_2^T t^\wedge Rx_1$x2T​t∧x2​=x2T​t∧Rx1​

因爲 $t^\wedge x_2 = 0$t∧x2​=0,所以上式化簡爲:
$x_2^Tt^\wedge R x_1 = 0$x2T​t∧Rx1​=0

重新帶入 $p_1,p_2$p1​,p2​得:
$p_2^TK^{-T} t^\wedge RK^{-1} p_1 = 0$p2T​K−Tt∧RK−1p1​=0

以上兩個式子都成爲 對極約束.

它的幾何意義是 $O_1,P,O_2$O1​,P,O2​三者共面. 対極約束中同時包含了 平移和旋轉.

把中間部分記做兩個矩陣:

• F : 基礎矩陣
• E : 本質矩陣

於是進一步簡化対極約束;
$E = r^\wedge R , \quad F = K^{-T}EK^{-1}, \quad x_2^TEx_1 = p_2^TFp_1 = 0$E=r∧R,F=K−TEK−1,x2T​Ex1​=p2T​Fp1​=0

$対極約束簡潔的給出了兩個匹配點的空間位置關係.$対極約束簡潔的給出了兩個匹配點的空間位置關系. 於是相機的位姿估計可以分爲以下步驟:

1. 根據匹配點的像素位置求出E 或者 F
2. 根據E或者F 求出 $R,t$R,t

由於E和F只相差了相機內參,所以往往使用更簡潔的EK

3.2. 本質矩陣

本質矩陣 $E=t^\wedge R$E=t∧R. 它是一個 $3\times 3$3×3矩陣. 從E的構造方式看,有以下幾點需要注意:

1. 本質矩陣由対極約束定義.由於対極約束是等式爲0的,所以對E乘以任意非零常數, 対極約束仍然滿足. 我們把這件事稱爲E在不同尺度下是等價的.
2. 根據 $E=t^\wedge R$E=t∧R ,課證明本質矩陣E的奇異值必定是 $[\sigma,\sigma,0]^T$[σ,σ,0]T的形式. 這稱爲 $本質矩陣的內在性質.$本質矩陣的內在性質.
3. 平移加旋轉共有6個自由度,所以 $t^\wedge R$t∧R共有6個自由度.但是由於 尺度等價性, 實際上只有5個自由度.

使用8點法求解E.

$\begin{bmatrix} u_1^1u_2^1 & u_1^1v_2^1 & u_1^1 & v_1^1u_2^1 & v_1^1v_2^1 & v_1^1 & u_2^1 & v_2^1 &1 \\ u_1^2u_2^2 & u_1^2v_2^2 & u_1^2 & v_1^2u_2^2 & v_1^2v_2^2 & v_1^2 & u_2^2 & v_2^2 &1 \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \\ u_1^8u_2^8 & u_1^8v_2^8 & u_1^8 & v_1^8u_2^8 & v_1^8v_2^8 & v_1^8 & u_2^8 & v_2^8 &1 \end{bmatrix} \begin{bmatrix} e_1 \\ e_2 \\ e_3 \\ e_4 \\ e_5 \\ e_6 \\ e_7 \\ e_8 \end{bmatrix} = 0$⎣⎢⎢⎢⎡​u11​u21​u12​u22​⋮u18​u28​​u11​v21​u12​v22​⋮u18​v28​​u11​u12​⋮u18​​v11​u21​v12​u22​⋮v18​u28​​v11​v21​v12​v22​⋮v18​v28​​v11​v12​⋮v18​​u21​u22​⋮u28​​v21​v22​⋮v28​​11⋮1​⎦⎥⎥⎥⎤​⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡​e1​e2​e3​e4​e5​e6​e7​e8​​⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤​=0

一聲個方程構成了線性方程組. 它的係數矩陣由特徵點位置構成,大小爲 $8\times 9$8×9. e位於該矩陣的零空間中. 如果係數矩陣的秩爲8,那麼零空間的維數爲1,也就是e構成一條線.

如果係數矩陣的秩爲8,那麼上述方程可以解得E的值.

接下來根據估計的本質矩陣E,恢復出相機運動R,t.這個過程由奇異值分解(SVD)得到.
$E = U \Sigma V^T$E=UΣVT

$U,V$U,V爲正交矩陣, $\Sigma$Σ爲奇異值矩陣.
根據E的內在性質, $\Sigma = diag(\sigma,\sigma,0)$Σ=diag(σ,σ,0)

對於任意E,存在兩個可能的 $t,R$t,R與之對應:
$t_1^\wedge = UR_Z(\frac{\pi}{2})\Sigma U^T, \quad R_1 = UR_Z^T(\frac{\pi}{2})V^Tt \newline t_2^\wedge = UR_Z(-\frac{\pi}{2})\Sigma U^T, \quad R_2 = UR_Z^T(-\frac{\pi}{2})V^Tt$t1∧​=URZ​(2π​)ΣUT,R1​=URZT​(2π​)VTtt2∧​=URZ​(−2π​)ΣUT,R2​=URZT​(−2π​)VTt

$R_Z(\frac{\pi}{2})$RZ​(2π​)表示旋轉90度的旋轉矩陣. 同時,由於 $-E 等價於 E$−E等價於E,所以任意一個t取負號,會得到同樣的結果. 因此,從E分解得到 $t,R$t,R,一個存在4種結果.

如下圖;

最後還有一個問題,線性方程組求解出E後,它可能不滿足E的內在性質:奇異值不爲 $(\sigma, \sigma, 0)$(σ,σ,0) 的形式.
對8點大求得的E進行SVD分解後,會得到奇異值矩陣 $\Sigma = diag(\sigma,\sigma,0)$Σ=diag(σ,σ,0).取( $\sigma _1 \geqslant \sigma _2 \geqslant \sigma _3$σ1​⩾σ2​⩾σ3​):

$E = U diag(\frac{\sigma_1+\sigma_2}{2},\frac{\sigma_1+\sigma_2}{2},0)V^T$E=Udiag(2σ1​+σ2​​,2σ1​+σ2​​,0)VT

最簡單的方法是將奇異值取 $diag(1,1,0)$diag(1,1,0)

3.3. 單應矩陣

單應矩陣H:它描述了兩個平面之間的映射關係.

單應矩陣通常掃描處於共平面上的一些點在兩張圖像之間的變換關係.

經過計算我們可以得到一個直接描述圖像座標 $p_1$p1​到 $p_2$p2​之間的變換,把中間部分記做H:
$p_2 = H p_1$p2​=Hp1​

把上式展開:
$\begin{bmatrix} u_2 \\ v_2 \\ 1 \end{bmatrix}= \begin{bmatrix} h_1 & h_2 & h_3 \\ h_4 & h_5 & h_6 \\ h_7 & h_8 & h_9 \end{bmatrix} \begin{bmatrix} u_1 \\ v_1 \\ 1 \end{bmatrix}$⎣⎡​u2​v2​1​⎦⎤​=⎣⎡​h1​h4​h7​​h2​h5​h8​​h3​h6​h9​​⎦⎤​⎣⎡​u1​v1​1​⎦⎤​

在實際中通常乘以一個非零因子,使得 $h_9 = 1$h9​=1,於是有:
$u_2 = \frac{h_1u_1+h_2v_2+h_3}{h_7u_1+h_8v_1+h_9} \newline \quad\newline v_2 = \frac{h_4u_1+h_5v_1+h_6}{h_7u_1+h_8v_1+h_9}$u2​=h7​u1​+h8​v1​+h9​h1​u1​+h2​v2​+h3​​v2​=h7​u1​+h8​v1​+h9​h4​u1​+h5​v1​+h6​​

於是自由度爲8的單應矩陣可以通過4對匹配特徵點算出.即求解以下方程:
$(\begin{array}{cccccccc}{u}_1^1& v_1^1& 1& 0& 0& 0& -u_1^1{u}_2^1& -v_1^1{u}_2^1\\ 0& 0& 0& u_1^1& v_1^1& 1& -u_1^1{u}_2^1& -v_1^1{u}_2^1\\ u_1^2& v_1^2& 1& 0& 0& 0& -u_1^2{u}_2^2& -v_1^2{u}_2^2\\ 0& 0& 0& u_1^2& v_1^2& 1& -u_1^2{u}_2^2& -v_1^2\end{array}$+h5​v1​+h6​​

於是自由度爲8的單應矩陣可以通過4對匹配特徵點算出.即求解以下方程:
$(\begin{array}{cccccccc}{u}_1^1& v_1^1& 1& 0& 0& 0& -u_1^1{u}_2^1& -v_1^1{u}_2^1\\ 0& 0& 0& u_1^1& v_1^1& 1& -u_1^1{u}_2^1& -v_1^1{u}_2^1\\ u_1^2& v_1^2& 1& 0& 0& 0& -u_1^2{u}_2^2& -v_1^2{u}_2^2\\ 0& 0& 0& u_1^2& v_1^2& 1& -u_1^2{u}_2^2& -v_1^2{u}_2^2\\ u_1^3& v_1^3& 1& 0& 0& 0& -u_1^3{u}_2^3& -v_1^3{u}_2^3\\ 0& 0& 0& u_1^3& v_1^3& 1& -u_1^3{u}_2^3& -v_1^3{u}_2^3\\ u_1^4& v_1^4& 1& 0& 0& 0& -u_1^4{u}_2^4& -v_1^4{u}_2^4\\ 0& 0& 0& u_1^4& v_1^4& 1& -u_1^4{u}_2^4& -v_1^4\end{array}$