視覺SLAM筆記--第7篇: 單目相機位姿估計流程

單目相機位姿估計流程

• 參考書籍
• 整體流程
• • 1. 坐标系變換
  • 2. 對極限制
  • 3. 單目初始化
  • 4. 三角測量
  • 5. 求解空間3D坐标
  • 6. 最小化重投影誤差

參考書籍

參考書籍：《視覺SLAM十四講》 高翔博士

整體流程

1. 坐标系變換

已知：比對的像素點對 ( u , v ) ， ( u ′ , v ′ ) (u,v)，(u^{'},v^{'}) (u,v)，(u′,v′)，相機内參矩陣 K K K

求解：相機歸一化坐标 ( X ′ , Y ′ , 1 ) (X^{'},Y^{'},1) (X′,Y′,1)

通過相機與像素間的坐标變換關系，可以得到下面公式：

[ u v 1 ] = [ f x 0 c x 0 f y c y 0 0 1 ] [ X ′ Y ′ 1 ] \left[ \begin{matrix} u\\ v \\ 1 \end{matrix} \right]= \left[ \begin{matrix} f_{x} & 0 & c_{x}\\ 0 & f_{y} & c_{y}\\ 0 & 0 & 1 \end{matrix} \right]\left[ \begin{matrix} X^{'}\\ Y^{'}\\ 1 \end{matrix} \right] ⎣⎡​uv1​⎦⎤​=⎣⎡​fx​00​0fy​0​cx​cy​1​⎦⎤​⎣⎡​X′Y′1​⎦⎤​表示像素坐标與相機歸一化平面坐标的變換關系。

2. 對極限制

利用對極限制關系，具體推導可參見部落格，利用求得的相機歸一化坐标 x 1 ， x 2 x_{1}，x_{2} x1​，x2​，可以求解基礎矩陣和本質矩陣：

x 2 E x 1 = 0 x_{2}Ex_{1}=0 x2​Ex1​=0分解 E E E便可以得到變換矩陣 R , t R,t R,t，注意，由于E(本質矩陣)本身具有尺度等價性,它分解得到的 R 、 t R、t R、t也有一個尺度等價性。

3. 單目初始化

在單目視覺中，我們對兩張圖像的t進行歸一化，相當于固定了一個尺度。雖然我們不知道它的實際長度是多少，但是我們可以以這時的 t t t為單目1，計算相機運動和特征點的3D位置，這一步稱為單目SLAM的初始化。

通常把 t t t 進行歸一化，讓它的長度等于1.

初始化的兩張圖檔必須要有一定程度的平移，而後的軌迹和地圖都将會以這一步的平移為機關。單目初始化不能隻有純旋轉，必須要有一定程度的平移，如果沒有平移，單目将無法初始化。

4. 三角測量

按照對極幾何中的定義，如果設 x 1 , x 2 x_1,x_{2} x1​,x2​為兩個特征點的歸一化坐标，于是有下列關系式：

s 1 x 1 = s 2 x 2 R + t s_{1}x_{1}=s_{2}x_{2}R+t s1​x1​=s2​x2​R+t已知：坐标系 o 到 o 1 o到o_{1} o到o1​的變換矩陣 R 、 t R、t R、t歸一化坐标 x 1 , x 2 x_{1},x_{2} x1​,x2​，現在要求的就是兩個特征點的深度 s 1 , s 2 s_{1},s_{2} s1​,s2​

左乘 [ x 1 ] × [x_{1}]_{\times} [x1​]×​可得 s 1 [ x 1 ] × x 1 = 0 s_{1}[x_{1}]_{\times}x_{1}=0 s1​[x1​]×​x1​=0：

s 2 [ x 1 ] × x 2 R + [ x 1 ] × t = 0 s_{2}[x_{1}]_{\times}x_{2}R+[x_{1}]_{\times}t =0 s2​[x1​]×​x2​R+[x1​]×​t=0可以解出 s 2 s_{2} s2​, 将其帶入原來的式子可解出 s 1 s_{1} s1​.

但是，由于噪聲的存在，我們求出來的R,t不一定能夠使得上式精确等于0，是以在實際情況中，更常見的做法是求最小二乘解而不是零解。

5. 求解空間3D坐标

已知：像素點坐标 ( u , v ) (u,v) (u,v)，相機内參矩陣 K K K，像素點的空間深度 s s s

求解：相機坐标 ( X , Y , Z ) (X,Y,Z) (X,Y,Z)

通過相機與像素間的坐标變換關系，可以得到下面公式：

s [ u v 1 ] = [ f x 0 c x 0 f y c y 0 0 1 ] [ X Y Z ] s\left[ \begin{matrix} u\\ v \\ 1 \end{matrix} \right]= \left[ \begin{matrix} f_{x} & 0 & c_{x}\\ 0 & f_{y} & c_{y}\\ 0 & 0 & 1 \end{matrix} \right]\left[ \begin{matrix} X\\ Y\\ Z \end{matrix} \right] s⎣⎡​uv1​⎦⎤​=⎣⎡​fx​00​0fy​0​cx​cy​1​⎦⎤​⎣⎡​XYZ​⎦⎤​

利用計算得到的相鄰兩幀相機變換關系 R ， t R，t R，t 以及相機坐标 P c = ( X , Y , Z ) P_{c}=(X,Y,Z) Pc​=(X,Y,Z)，可以求解世界坐标 P w P_{w} Pw​

P c = R ∗ P w + t P_{c}=R*P_{w}+t Pc​=R∗Pw​+t注意：這是以第一幀相機原點作為世界坐标系的原點進行求得，後面由于相機在不斷移動，是以相機間的變換矩陣在不斷改變，通過變換矩陣累乘的方式将相機坐标變化為世界坐标。

重點：

如果要計算第 n n n 個相機下，其相機坐标到世界坐标的變換，需要用以下的公式計算：

已知：第一幀相機到第二幀相機的變換矩陣 T 12 T_{12} T12​（世界坐标到相機坐标系變換就很），第二幀相機到第三幀相機的變換矩陣 T 23 T_{23} T23​，以此類推，第 n − 1 n-1 n−1幀相機到第 n n n幀相機的變換矩陣 T ( n − 1 ) n T_{(n-1)n} T(n−1)n​， 第 n n n幀相機下的相機坐标 P c ( n ) P^{(n)}_{c} Pc(n)​

求解：第 n n n幀相機下的世界坐标 P w ( n ) P^{(n)}_{w} Pw(n)​

P w ( n ) = T 12 ∗ T 23 ∗ . . . . . . ∗ T ( n − 1 ) n ∗ P c ( n ) P^{(n)}_{w}=T_{12}*T_{23}*......*T_{(n-1)n}*P^{(n)}_{c} Pw(n)​=T12​∗T23​∗......∗T(n−1)n​∗Pc(n)​其中 T 12 ∗ T 23 ∗ . . . . . . ∗ T ( n − 1 ) n ∗ P c ( n ) T_{12}*T_{23}*......*T_{(n-1)n}*P^{(n)}_{c} T12​∗T23​∗......∗T(n−1)n​∗Pc(n)​就是相機到世界坐标系的歐式變換矩陣，是重投影誤差的初始值。

6. 最小化重投影誤差

已知：3D空間坐标 P i P_{i} Pi​，及其投影像素坐标 u i u_{i} ui​，相機的初始位姿 T T T(相機到世界坐标系的變換矩陣)，可以由第(5)步得到。

求解目标函數：

T ∗ = arg min ⁡ T 1 2 ∣ ∣ u i − 1 s i K T P i ∣ ∣ 2 T^{*}=\argmin_{T} \frac{1}{2}||u_{i}-\frac{1}{s_{i}}KTP_{i}||^{2} T∗=Targmin​21​∣∣ui​−si​1​KTPi​∣∣2根據高斯牛頓法，可得：

e ( x + Δ x ) = e ( x ) + J T Δ x e(x+\Delta{x})=e(x)+J^{T}\Delta{x} e(x+Δx)=e(x)+JTΔx其中 J T J^{T} JT表示目标函數(誤差)關于 T T T的一階導數，雅可比矩陣，根據鍊式法則可以将其分解如下：

其中 P ′ P^{'} P′表示相機坐标，是以可以将求解的世界坐标轉換為對相機坐标求導。

可以得雅可比矩陣：

通過高斯牛頓或者LM法不斷優化重投影誤差，便可以得到較為準确的位姿。