《視覺SLAM十四講》詳細筆記
《視覺SLAM十四講》筆記摘抄
- ch02 初識SLAM
- ch03 三維空間剛體運動
- ch04 李群與李代數
- ch05 相機與影象
- ch06 非線性優化
- ch07 視覺里程計01
ch02 初識SLAM
經典視覺SLAM框架
視覺SLAM流程包括以下步驟:
-
感測器資訊讀取: 在視覺SLAM中主要為相機影象資訊的讀取和預處理.如果是在機器人中,還可能有碼盤、慣性感測器等資訊的讀取和同步.
-
視覺里程計(Visual Odometry,VO): 視覺里程計的任務是估算相鄰影象間相機的運動,以及區域性地圖的樣子.VO又稱為前端(Front End).
視覺里程計不可避免地會出現累積漂移(Accumulating Drift)問題.
-
後端優化 (Optimization): 後端接受不同時刻視覺里程計測量的相機位姿,以及迴環檢測的資訊,對它們進行優化,得到全域性一致的軌跡和地圖.由於接在VO之後,又稱為後端(Back End).
在視覺 SLAM中,前端和計算機視覺研究領域更為相關,比如影象的特徵提取與匹配等,後端則主要是濾波與非線性優化演算法.
-
迴環檢測 (Loop Closing): 迴環檢測判斷機器人是否到達過先前的位置.如果檢測到迴環,它會把資訊提供給後端進行處理.
-
建圖 (Mapping): 它根據估計的軌跡,建立與任務要求對應的地圖.
地圖的形式包括度量地圖(精確表示地圖物體的位置關係)與拓撲地圖(更強調地圖元素之間的關
系)兩種.
SLAM問題的數學表述
“小蘿蔔攜帶著感測器在環境中運動”,由如下兩件事情描述:
-
什麼是運動 ?我們要考慮從 k − 1 k-1 k−1時刻到 k k k時刻,小蘿蔔的位置 x x x是如何變化的.
運動方程:
x k = f ( x k − 1 , u k , w k ) x_k = f(x_{k-1}, u_k, w_k) xk=f(xk−1,uk,wk)
- x k , x k − 1 x_k, x_{k-1} xk,xk−1表示小蘿蔔在 k k k和 k − 1 k-1 k−1時刻的位置
- u k u_k uk表示運動感測器的讀數(有時也叫輸入)
- w k w_k wk表示噪聲
-
什麼是觀測 ?假設小蘿蔔在 k k k時刻於 x k x_k xk處探測到了某一個路標 y j y_j yj,我們要考慮這件事情是如何用數學語言來描述的.
觀測方程:
z k , j = h ( y j , x k , v k , j ) z_{k,j} = h(y_j, x_k, v_{k,j}) zk,j=h(yj,xk,vk,j)
- z k , j z_{k,j} zk,j表示小蘿蔔在 x k x_k xk位置上看到路標點 y j y_j yj,產生的觀測資料
- y j y_j yj表示第 j j j個路標點
- v k , j v_{k,j} vk,j表示噪聲
這兩個方程描述了最基本的SLAM問題:當知道運動測量的讀數 u u u ,以及感測器的讀數 z z z 時,如何求解定位問題(估計 x x x )和建圖問題(估計 y y y)?這時,我們就把SLAM問題建模成了一個狀態估計問題:如何通過帶有噪聲的測量資料,估計內部的、隱藏著的狀態變數?
ch03 三維空間剛體運動
旋轉矩陣
點和向量,座標系
-
向量 a a a線上性空間的基 [ e 1 , e 2 , e 3 ] [e_1, e_2, e_3] [e1,e2,e3]下的座標為 [ a 1 , a 2 , a 3 ] T [a_1, a_2, a_3]^T [a1,a2,a3]T.
a = [ e 1 , e 2 , e 3 ] [ a 1 a 2 a 3 ] = a 1 e 1 + a 2 e 2 + a 3 e 3 a = [e_1, e_2, e_3] \left[
a1a2a3
-
\right] = a_1e_1 + a_2e_2 + a_3e_3 a=[e1,e2,e3]⎣⎡a1a2a3⎦⎤=a1e1+a2e2+a3e3
-
向量的內積與外積
-
向量的內積: 描述向量間的投影關係
a ⋅ b = a T b = ∑ i = 1 3 a i b i = ∣ a ∣ ∣ b ∣ cos 〈 a , b 〉 a \cdot b = a^T b = \sum_{i=1}^3 a_ib_i = |a|\,|b| \cos \langle a,b \rangle a⋅b=aTb=i=1∑3aibi=∣a∣∣b∣cos〈a,b〉 -
向量的外積: 描述向量的旋轉
a × b = [ i j k a 1 a 2 a 3 b 1 b 2 b 3 ] = [ a 2 b 3 − a 3 b 2 a 3 b 1 − a 1 b 3 a 1 b 2 − a 2 b 1 ] = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] b ≜ a ∧ b a \times b = \left[ia1b1ja2b2ka3b3
-
- \right] = \left[
a2b3−a3b2a3b1−a1b3a1b2−a2b1
\right] = \left[0a3−a2−a30a1a2−a10
\right] b \triangleq a ^\wedge b a×b=⎣⎡ia1b1ja2b2ka3b3⎦⎤=⎣⎡a2b3−a3b2a3b1−a1b3a1b2−a2b1⎦⎤=⎣⎡0a3−a2−a30a1a2−a10⎦⎤b≜a∧b
其中 a ∧ a^\wedge a∧表示 a a a的反對稱矩陣
a ∧ = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] a ^\wedge = \left[0a3−a2−a30a1a2−a10
-
-
\right] a∧=⎣⎡0a3−a2−a30a1a2−a10⎦⎤
-
座標系間的歐氏變換
-
歐式變換:
在歐式變換前後的兩個座標系下,同一個向量的模長和方向不發生改變,是為歐式變換.
一個歐式變換由一個旋轉和一個平移組成.
-
旋轉矩陣 R R R:
-
旋轉矩陣 R R R的推導:
設單位正交基 [ e 1 , e 2 , e 3 ] [e_1, e_2, e_3] [e1,e2,e3]經過一次旋轉變成了 [ e 1 ′ , e 2 ′ , e 3 ′ ] [e_1', e_2', e_3'] [e1′,e2′,e3′],對於同一個向量 a a a,在兩個座標系下的座標分別為 [ a 1 , a 2 , a 3 ] T [a_1, a_2, a_3]^T [a1,a2,a3]T和 [ a 1 ′ , a 2 ′ , a 3 ′ ] T [a_1', a_2', a_3']^T [a1′,a2′,a3′]T.根據座標的定義:
[ e 1 , e 2 , e 3 ] [ a 1 a 2 a 3 ] = [ e 1 ′ , e 2 ′ , e 3 ′ ] [ a 1 ′ a 2 ′ a 3 ′ ] [e_1, e_2, e_3] \left[a1a2a3
-
a′1a′2a′3
\right] [e1,e2,e3]⎣⎡a1a2a3⎦⎤=[e1′,e2′,e3′]⎣⎡a1′a2′a3′⎦⎤
等式左右兩邊同時左乘 [ e 1 T , e 2 T , e 3 T ] T [e_1^T, e_2^T, e_3^T]^T [e1T,e2T,e3T]T,得到
[ a 1 a 2 a 3 ] = [ e 1 T e 1 ′ e 1 T e 2 ′ e 1 T e 3 ′ e 2 T e 1 ′ e 2 T e 2 ′ e 2 T e 3 ′ e 3 T e 1 ′ e 3 T e 2 ′ e 3 T e 3 ′ ] [ a 1 ′ a 2 ′ a 3 ′ ] ≜ R a ′ \left[a1a2a3
\right] = \left[eT1e′1eT2e′1eT3e′1eT1e′2eT2e′2eT3e′2eT1e′3eT2e′3eT3e′3
\right] \left[a′1a′2a′3
-
-
\right] \triangleq R a' ⎣⎡a1a2a3⎦⎤=⎣⎡e1Te1′e2Te1′e3Te1′e1Te2′e2Te2′e3Te2′e1Te3′e2Te3′e3Te3′⎦⎤⎣⎡a1′a2′a3′⎦⎤≜Ra′
矩陣 R R R描述了旋轉,稱為旋轉矩陣.
-
旋轉矩陣 R R R的性質
- 旋轉矩陣是行列式為1的正交矩陣,任何行列式為1的正交矩陣也是一個旋轉矩陣.所有旋轉矩陣構成特殊正交群 S O SO SO:
S O ( n ) = { R ∈ R n × n ∣ R R T = I , det ( R ) = 1 } SO(n) = \{ R \in \mathbb{R}^{n \times n} | RR^T = I, \det(R)=1 \} SO(n)={R∈Rn×n∣RRT=I,det(R)=1}
- 旋轉矩陣是正交矩陣(其轉置等於其逆),旋轉矩陣的逆 R − 1 R^{-1} R−1(即轉置 R T R^T RT)描述了一個相反的旋轉.
-
-
歐式變換的向量表示:
世界座標系中的向量 a a a,經過一次旋轉(用旋轉矩陣 R R R描述)和一次平移(用平移向量 t t t描述)後,得到了 a ′ a' a′:
a ′ = R a + t a' = Ra + t a′=Ra+t
變換矩陣與齊次座標
-
變換矩陣 T T T:
在三維向量的末尾新增1,構成的四維向量稱為齊次座標.將旋轉和平移寫入變換矩陣 T T T中,得到:
[ a ′ 1 ] = [ R t 0 1 ] [ a 1 ] ≜ T [ a 1 ] \left[
a′1
R0t1
\right] \left[a1
\right] \triangleq T \left[a1
-
-
\right] [a′1]=[R0t1][a1]≜T[a1]
齊次座標的意義在於將歐式變換表示為線性關係. -
變換矩陣 T T T的性質:
-
變換矩陣 T T T構成特殊歐式群 S E SE SE
S E ( 3 ) = { T = [ R t 0 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } SE(3) = \left\{ T = \left[R0t1
-
-
\right] \in \mathbb{R}^{4\times4} | R \in SO(3), t \in \mathbb{R}^3 \right\} SE(3)={T=[R0t1]∈R4×4∣R∈SO(3),t∈R3}
-
變換矩陣的逆表示一個反向的歐式變換
T − 1 = [ R T − R T t 0 1 ] T^{-1} = \left[RT0−RTt1
-
-
\right] T−1=[RT0−RTt1]
-
齊次座標(Homogeneous Coordinate)的優勢
若點 p = ( x , y ) p=(x,y) p=(x,y)在直線 l = ( a , b , c ) l=(a,b,c) l=(a,b,c)上,則有:
a x + b y + c = [ a , b , c ] T ⋅ [ x , y , 1 ] = l T ⋅ p ′ = 0 ax+by+c = [a,b,c]^T \cdot [x,y,1] = l^T \cdot p' = 0 ax+by+c=[a,b,c]T⋅[x,y,1]=lT⋅p′=0
若點 p = ( x , y , z ) p=(x,y,z) p=(x,y,z)在平面 A = ( a , b , c , d ) A=(a,b,c,d) A=(a,b,c,d)上,則有:
a x + b y + c z + d = [ a , b , c , d ] T ⋅ [ x , y , z , 1 ] = A T ⋅ p ′ = 0 ax+by+cz+d = [a,b,c,d]^T \cdot [x,y,z,1] = A^T \cdot p' = 0 ax+by+cz+d=[a,b,c,d]T⋅[x,y,z,1]=AT⋅p′=0
在齊次座標下,
- 可以用兩個點 p p p, q q q的齊次座標叉乘結果表示它們的共線 l l l.
- 可以用兩條直線 l l l, m m m的齊次座標叉乘結果表示它們的交點 x x x.
這裡利用叉乘的性質: 叉乘結果與兩個運算向量都垂直:
-
性質1的證明:
l T ⋅ p = ( p × q ) ⋅ p = 0 l T ⋅ q = ( p × q ) ⋅ q = 0 l^T \cdot p = (p \times q) \cdot p = 0 \\ l^T \cdot q = (p \times q) \cdot q = 0 lT⋅p=(p×q)⋅p=0lT⋅q=(p×q)⋅q=0 -
性質2的證明:
l T ⋅ p = l T ⋅ ( l × m ) = 0 m T ⋅ p = m T ⋅ ( l × m ) = 0 l^T \cdot p = l^T \cdot (l \times m) = 0 \\ m^T \cdot p = m^T \cdot (l \times m) = 0 lT⋅p=lT⋅(l×m)=0mT⋅p=mT⋅(l×m)=0
- 點 ( x , y , z ) (x,y,z) (x,y,z)的齊次座標為 ( x , y , z , 1 ) (x,y,z,1) (x,y,z,1)
- 向量 ( x , y , z ) (x,y,z) (x,y,z)的齊次座標為 ( x , y , z , 0 ) (x,y,z,0) (x,y,z,0)
對於平行直線 l = ( a , b , c ) l=(a,b,c) l=(a,b,c)和 m = ( a , b , d ) m=(a,b,d) m=(a,b,d),求取其交點的齊次座標 x = l × m = ( k b , − k a , 0 ) x=l \times m=(kb, -ka, 0) x=l×m=(kb,−ka,0),將其轉為非齊次座標,得到 x = ( k b / 0 , − k a / 0 ) = ( inf , − inf ) x = (kb/0, -ka/0) = (\inf, -\inf) x=(kb/0,−ka/0)=(inf,−inf),這表示無窮遠點.
使用齊次座標,可以將加法運算轉化為乘法運算.
| 變換形式 | 圖形示意 | 數學變換 | MATLAB函式 |
|---|---|---|---|
| 位移(Translation) |  | [ x ′ y ′ 1 ] = [ 1 0 t x 0 1 t y 0 0 1 ] ∗ [ x y 1 ] \left[ x′y′1 |
\right] =\left[
100010txty1
\right] * \left[
xy1
| \right] ⎣⎡x′y′1⎦⎤=⎣⎡100010txty1⎦⎤∗⎣⎡xy1⎦⎤ | imtranslate() | |
| 縮放(Scale) |  | [ x ′ y ′ 1 ] = [ s x 0 0 0 s y 0 0 0 1 ] ∗ [ x y 1 ] \left[ x′y′1 |
\right] =\left[
sx000sy0001
\right] * \left[
xy1
| \right] ⎣⎡x′y′1⎦⎤=⎣⎡sx000sy0001⎦⎤∗⎣⎡xy1⎦⎤ | imresize() | |
| 錯切(Shear) |  | [ x ′ y ′ 1 ] = [ 1 h x 0 h y 1 0 0 0 1 ] ∗ [ x y 1 ] \left[ x′y′1 |
\right] =\left[
1hy0hx10001
\right] * \left[
xy1
| \right] ⎣⎡x′y′1⎦⎤=⎣⎡1hy0hx10001⎦⎤∗⎣⎡xy1⎦⎤ | ||
| 旋轉(Rotate) |  | [ x ′ y ′ 1 ] = [ cos θ sin θ 0 − sin θ cos θ 0 0 0 1 ] ∗ [ x y 1 ] \left[ x′y′1 |
\right] =\left[
cosθ−sinθ0sinθcosθ0001
\right] * \left[
xy1
| \right] ⎣⎡x′y′1⎦⎤=⎣⎡cosθ−sinθ0sinθcosθ0001⎦⎤∗⎣⎡xy1⎦⎤ | imrotate() |
旋轉向量和尤拉角
旋轉向量
-
旋轉矩陣的缺點:
- 旋轉矩陣有9個量,但一次旋轉只有3個自由度,這種表達方式是冗餘的.
- 旋轉矩陣自帶約束(必須是行列式為1的正交矩陣),這些約束會給估計和優化帶來困難.
-
旋轉向量: 任意旋轉都可以用一個旋轉軸和一個旋轉角 來刻畫.於是,我們可以使用一個向量,其方向表示旋轉軸而長度表示旋轉角.這種向量稱為旋轉向量(或軸角,Axis-Angle).
假設有一個旋轉軸為 n n n,角度為 θ \theta θ的旋轉,其對應的旋轉向量為 θ n \theta n θn.
-
旋轉向量和旋轉矩陣之間的轉換:
設旋轉向量 R R R表示一個繞單位向量 n n n,角度為 θ θ θ的旋轉.
-
旋轉向量到旋轉矩陣:
R = cos θ I + ( 1 − cos θ ) n n T + sin θ n ∧ R = \cos\theta I + (1-\cos\theta) n n^T + \sin\theta \, n^\wedge R=cosθI+(1−cosθ)nnT+sinθn∧ -
旋轉矩陣到旋轉向量:
- 旋轉角 θ = arccos ( t r ( R ) − 1 2 ) \theta = \arccos \left( \frac{tr(R)-1}{2} \right) θ=arccos(2tr(R)−1)
- 旋轉軸 n n n是矩陣 R R R特徵值1對應的特徵向量
-
尤拉角
-
尤拉角將一次旋轉分解成3個分離的轉角.常用的一種ZYX轉角將任意旋轉分解成以下3個軸上的轉角:
- 繞物體的 Z Z Z軸旋轉,得到偏航角yaw
- 繞旋轉之後的 Y Y Y軸旋轉,得到俯仰角pitch
- 繞旋轉之後的 X X X軸旋轉,得到滾轉角roll
-
尤拉角的一個重大缺點是萬向鎖問題(奇異性問題): 在俯仰角為$\pm$90° 時,第一次旋轉與第三次旋轉將使用同一個軸,使得系統丟失了一個自由度(由3次旋轉變成了2次旋轉).
四元數
為什麼需要四元數: 對於三維旋轉,找不到不帶奇異性的三維向量描述方式.因此引入四元數.
四元數是一種擴充套件的複數,既是緊湊的,也沒有奇異性.
四元數的定義
-
四元數的定義
一個四元數 q q q擁有一個實部和三個虛部
q = q 0 + q 1 i + q 2 j + q 3 k q = q_0 + q_1 i + q_2 j + q_3 k q=q0+q1i+q2j+q3k其中 i i i, j j j, k k k,為四元數的3個虛部,它們滿足以下關係式(自己和自己的運算像複數,自己和別人的運算像叉乘):
{ i 2 = j 2 = k 2 = − 1 i j = k , j i = − k j k = i , k j = − i k i = j , i k = − j \left\{i2=j2=k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ik=−j
-
\right. ⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧i2=j2=k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ik=−j
也可以用一個標量和一個向量來表達四元數:
q = [ s , v ] , s = q 0 ∈ R v = [ q 1 , q 2 , q 3 ] T ∈ R 3 q = [s, v], \quad s=q_0\in\mathbb{R} \quad v=[q_1, q_2, q_3]^T \in \mathbb{R}^3 q=[s,v],s=q0∈Rv=[q1,q2,q3]T∈R3s s s為四元數的實部, v v v為四元數的虛部.有實四元數和虛四元數的概念.
-
四元數與旋轉角度的關係:
- 在二維情況下,任意一個旋轉都可以用單位複數來描述,乘 i i i就是繞 i i i軸旋轉90°.
- 在三維情況下,任意一個旋轉都可以用單位四元數來描述,乘 i i i就是繞 i i i軸旋轉180°.
-
單位四元數和旋轉向量之間的轉換:
設單位四元數 q q q表示一個繞單位向量 n = [ n x , n y , n z ] T n =[n_x,n_y,n_z]^T n=[nx,ny,nz]T,角度為 θ θ θ的旋轉.
- 從旋轉向量到單位四元數:
q = [ cos ( θ 2 ) , n sin ( θ 2 ) ] T = [ cos ( θ 2 ) , n x sin ( θ 2 ) , n y sin ( θ 2 ) , n z sin ( θ 2 ) ] T q = \left[ \cos(\frac{\theta}{2}), n\sin(\frac{\theta}{2}) \right]^T= \left[ \cos(\frac{\theta}{2}), n_x\sin(\frac{\theta}{2}), n_y\sin(\frac{\theta}{2}), n_z\sin(\frac{\theta}{2}) \right]^T q=[cos(2θ),nsin(2θ)]T=[cos(2θ),nxsin(2θ),nysin(2θ),nzsin(2θ)]T
- 從單位四元數到旋轉向量:
{ θ = 2 arccos q 0 [ n x , n y , n z ] = [ q 1 , q 2 , q 3 ] T / sin θ 2 \left\{θ=2arccosq0[nx,ny,nz]=[q1,q2,q3]T/sinθ2
-
- \right. ⎩⎨⎧θ=2arccosq0[nx,ny,nz]=[q1,q2,q3]T/sin2θ
用單位四元數表示旋轉
給定一個空間三維點 p = [ x , y , z ] ∈ R 3 p=[x,y,z ]\in \R^3 p=[x,y,z]∈R3,以及一個由軸角 n n n, θ θ θ指定的旋轉,三維點 p p p經過旋轉後變為 p ′ p′ p′.如何使用單位四元數 q q q表達旋轉?
-
把三維空間點用一個虛四元數 p p p表示:
p = [ 0 , x , y , z ] = [ 0 , v ] p = [0, x, y, z] = [0, v] p=[0,x,y,z]=[0,v] -
把旋轉用單位四元數 q q q表示:
q = [ cos θ 2 , n sin θ 2 ] q = [\cos{\frac{\theta}{2}}, n\sin{\frac{\theta}{2}} ] q=[cos2θ,nsin2θ] -
旋轉後的點 p ′ p' p′可表示為:
p ′ = q p q − 1 p' = qpq^{-1} p′=qpq−1
這樣得到的點 p ′ p' p′仍為一個純虛四元數,其虛部的3個分量表示旋轉後3D點的座標.
只有單位四元數才能表示旋轉,因此在程式中建立四元數後,要記得呼叫
normalize()以將其單位化
ch04 李群與李代數
李群與李代數基礎
旋轉矩陣構成特殊正交群 S O ( 3 ) SO(3) SO(3),變換矩陣構成了特殊歐氏群 S E ( 3 ) SE(3) SE(3).
S O ( 3 ) = { R ∈ R 3 × 3 ∣ R R T = I , det ( R ) = 1 } S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 }
SO(3)SE(3)={R∈R3×3|RRT=I,det(R)=1}={T=[R0Tt1]∈R4×4|R∈SO(3),t∈R3}
SO(3)SE(3)={R∈R3×3∣RRT=I,det(R)=1}={T=[R0Tt1]∈R4×4∣R∈SO(3),t∈R3}
群的定義
-
群(Group)是一種集合加上一種運算的代數結構.把集合記作 A A A,運算記作 ⋅ \cdot ⋅ ,那麼群可以記作 G = ( A , ⋅ ) G =(A,\cdot) G=(A,⋅).群要求這個運算滿足如下條件(封結么逆):
- 封閉性: ∀ a 1 , a 2 ∈ A , a 1 ⋅ a 2 ∈ A \forall{a_1, a_2} \in A, \quad a_1 \cdot a_2 \in A ∀a1,a2∈A,a1⋅a2∈A.
- 結合律: ∀ a 1 , a 2 , a 3 ∈ A , ( a 1 ⋅ a 2 ) ⋅ a 3 = a 1 ⋅ ( a 2 ⋅ a 3 ) \forall{a_1, a_2, a_3} \in A, \quad (a_1 \cdot a_2) \cdot a_3 = a_1 \cdot (a_2 \cdot a_3 ) ∀a1,a2,a3∈A,(a1⋅a2)⋅a3=a1⋅(a2⋅a3)
- 么元: ∃ a 0 ∈ A , s . t . ∀ a ∈ A , a 0 ⋅ a = a ⋅ a 0 = a \exists{a_0} \in A, \quad \mathrm{s.t.} \quad \forall a \in A, \quad a_0\cdot a = a\cdot a_0 = a ∃a0∈A,s.t.∀a∈A,a0⋅a=a⋅a0=a
- 逆: ∀ a ∈ A , ∃ a − 1 ∈ A , s . t . a ⋅ a − 1 = a 0 \forall a \in A, \quad \exists{a^{-1}} \in A, \quad \mathrm{s.t.} a\cdot a^{-1}=a_0 ∀a∈A,∃a−1∈A,s.t.a⋅a−1=a0
-
李群是指具有連續(光滑)性質的群. S O ( 3 ) SO(3) SO(3)和 S E ( 3 ) SE(3) SE(3)都是李群
李代數的定義
每個李群都有與之對應的李代數,李代數描述了李群的區域性性質.
通用的李代數的定義如下:
李代數由一個集合 V V V,一個數域 F F F和一個二元運算 [ , ] [, ] [,]組成.如果它們滿足以下幾條性質,則稱 ( V , F , [ , ] ) (V, F, [, ]) (V,F,[,])為一個李代數,記作 g \mathfrak{g} g.
-
封閉性: ∀ X , Y ∈ V , [ X , Y ] ∈ V \forall{X, Y} \in V, [X,Y] \in V ∀X,Y∈V,[X,Y]∈V.
-
雙線性: $\forall X,Y,Z \in V, a,b \in F $有:
[ a X + b Y , Z ] = a [ X , Z ] + b [ Y , Z ] , [ Z , a X + b Y ] = a [ Z , X ] + b [ Z , Y ] [aX+bY,Z]=a[X,Z]+b[Y,Z], \quad [Z, aX+bY]=a[Z,X]+b[Z,Y] [aX+bY,Z]=a[X,Z]+b[Y,Z],[Z,aX+bY]=a[Z,X]+b[Z,Y] -
自反性: ∀ X , ∈ V , [ X , X ] = 0 \forall{X,} \in V, [X,X]=0 ∀X,∈V,[X,X]=0.
-
雅可比等價 ∀ X , Y , Z ∈ V , [ X , [ Y , Z ] ] + [ Z , [ X , Y ] ] + [ Y , [ Z , X ] ] = 0 \forall X,Y,Z \in V, \quad [X, [Y,Z ]]+[Z, [X,Y ]]+[Y, [Z,X ]]=0 ∀X,Y,Z∈V,[X,[Y,Z]]+[Z,[X,Y]]+[Y,[Z,X]]=0.
其中的二元運算 [ , ] [,] [,]被稱為李括號.例如三維向量空間 R 3 \mathbb{R^3} R3上定義的叉積 × \times ×是一種李括號.
李代數 s o ( 3 ) \mathfrak{so}(3) so(3)
-
李群 S O ( 3 ) SO(3) SO(3)對應的李代數 s o ( 3 ) \mathfrak{so}(3) so(3)是定義在 R 3 \mathbb{R^3} R3上的向量,記作 ϕ \phi ϕ.
s o ( 3 ) = { ϕ ∈ R 3 , Φ = ϕ ∧ = [ 0 − ϕ 3 ϕ 2 ϕ 3 0 − ϕ 1 − ϕ 2 ϕ 1 0 ] ∈ R 3 × 3 } \mathfrak{so}(3) = \left\{ \phi \in \mathbb{R^3}, \Phi=\phi^\wedge = \left[
0ϕ3−ϕ2−ϕ30ϕ1ϕ2−ϕ10
-
\right] \in \mathbb{R^{3 \times 3}} \right\} so(3)=⎩⎨⎧ϕ∈R3,Φ=ϕ∧=⎣⎡0ϕ3−ϕ2−ϕ30ϕ1ϕ2−ϕ10⎦⎤∈R3×3⎭⎬⎫
-
李代數 s o ( 3 ) \mathfrak{so}(3) so(3)的李括號為
[ ϕ 1 , ϕ 2 ] = ( Φ 1 Φ 2 − Φ 2 Φ 1 ) ∨ [\phi_1, \phi_2] = (\Phi_1 \Phi_2 - \Phi_2 \Phi_1) ^\vee [ϕ1,ϕ2]=(Φ1Φ2−Φ2Φ1)∨
其中 ∨ ^\vee ∨是 ∧ ^\wedge ∧的逆運算,表示將反對稱矩陣還原為向量 -
s o ( 3 ) \mathfrak{so}(3) so(3)和 S O ( 3 ) SO(3) SO(3)間的對映關係為
李 群 R = exp ( ϕ ∧ ) = exp ( Φ ) 李 代 數 ϕ = ln ( R ) ∨
李群R李代數ϕ=exp(ϕ∧)=exp(Φ)=ln(R)∨
-
李群R李代數ϕ=exp(ϕ∧)=exp(Φ)=ln(R)∨
李代數 s e ( 3 ) \mathfrak{se}(3) se(3)
-
類似地,李群 S E ( 3 ) SE(3) SE(3)的李代數 s e ( 3 ) \mathfrak{se}(3) se(3)是定義在 R 6 \mathbb{R^6} R6上上的向量.記作 ξ \xi ξ:
s e ( 3 ) = { ξ = [ ρ ϕ ] ∈ R 6 , ρ ∈ R 3 , ϕ ∈ s o ( 3 ) , ξ ∧ = [ ϕ ∧ ρ 0 T 0 ] ∈ R 4 × 4 } \mathfrak{se}(3) = \left\{ \xi = \left[
ρϕ
\right] \in \mathbb{R^6}, \rho \in \mathbb{R^3}, \phi \in \mathfrak{so}(3), \xi^\wedge = \left[
ϕ∧0Tρ0
-
\right] \in \mathbb{R^{4\times 4}} \right\} se(3)={ξ=[ρϕ]∈R6,ρ∈R3,ϕ∈so(3),ξ∧=[ϕ∧0Tρ0]∈R4×4}
s e ( 3 ) \mathfrak{se}(3) se(3)中的每個元素 ξ \xi ξ,是一個六維向量.前三維 ρ \rho ρ表示平移;後三維 ϕ \phi ϕ表示旋轉,本質上是 s o ( 3 ) \mathfrak{so}(3) so(3)元素. -
在這裡同樣使用 ∧ ^\wedge ∧符號將六維向量擴充套件成為四維矩陣,但不再表示反對稱
ξ ∧ = [ ϕ ∧ ρ 0 T 0 ] ∈ R 4 × 4 \xi^\wedge = \left[
ϕ∧0Tρ0
-
\right] \in \mathbb{R^{4 \times 4}} ξ∧=[ϕ∧0Tρ0]∈R4×4
-
李代數 s e ( 3 ) \mathfrak{se}(3) se(3)的李括號和 s o ( 3 ) \mathfrak{so}(3) so(3)類似:
[ ξ 1 , ξ 2 ] = ( ξ 1 ∧ ξ 2 ∧ − ξ 2 ∧ ξ 1 ∧ ) ∨ [\xi_1, \xi_2] = (\xi^\wedge_1 \xi^\wedge_2 - \xi^\wedge_2 \xi^\wedge_1) ^\vee [ξ1,ξ2]=(ξ1∧ξ2∧−ξ2∧ξ1∧)∨ -
s e ( 3 ) \mathfrak{se}(3) se(3)和 S E ( 3 ) SE(3) SE(3)間對映關係為
李 群 T = exp ( ξ ∧ ) 李 代 數 ξ = ln ( T ) ∨李群T李代數ξ=exp(ξ∧)=ln(T)∨
-
李群T李代數ξ=exp(ξ∧)=ln(T)∨
李群與李代數的轉換關係:指數對映和對數對映
S O ( 3 ) SO(3) SO(3)和 s o ( 3 ) \mathfrak{so}(3) so(3)間的轉換關係
-
將三維向量 ϕ \phi ϕ分解為其模長 θ \theta θ和方向向量 α \alpha α,即 ϕ = θ α \phi=\theta\alpha ϕ=θα.則從 s o ( 3 ) \mathfrak{so}(3) so(3)到 S O ( 3 ) SO(3) SO(3)的指數對映可表示為:
R = exp ( ϕ ) = exp ( θ α ∧ ) = cos θ I + ( 1 − cos θ ) α α T + sin θ α ∧ R = \exp(\phi) = \exp(\theta \alpha ^\wedge) = \cos \theta I + (1-\cos\theta) \alpha \alpha^T + \sin \theta \alpha ^\wedge R=exp(ϕ)=exp(θα∧)=cosθI+(1−cosθ)ααT+sinθα∧
上式即為旋轉向量到旋轉矩陣的羅德里格斯公式,可見** s o ( 3 ) \mathfrak{so}(3) so(3)本質上是旋轉向量組成的空間**.
-
從 S O ( 3 ) SO(3) SO(3)到 s o ( 3 ) \mathfrak{so}(3) so(3)的對數對映可表示為:
ϕ = ln ( R ) ∨ \phi = \ln(R)^\vee ϕ=ln(R)∨實際計算時可以通過跡的性質分別求出轉角 θ \theta θ和轉軸 α \alpha α
θ = arccos t r ( R ) − 1 2 , R α = α \theta = \arccos \frac{tr(R)-1}{2}, \qquad R \alpha = \alpha θ=arccos2tr(R)−1,Rα=α
S E ( 3 ) SE(3) SE(3)和 s e ( 3 ) \mathfrak{se}(3) se(3)間的轉換關係
-
從 s e ( 3 ) \mathfrak{se}(3) se(3)到 S E ( 3 ) SE(3) SE(3)的指數對映可表示為:
T = exp ( ξ ∧ ) = [ R J ρ 0 T 1 ] T = \exp(\xi ^\wedge) = \left[
R0TJρ1
-
\right] T=exp(ξ∧)=[R0TJρ1]
其中
J = sin θ θ I + ( 1 − sin θ θ ) α α T + 1 − cos θ θ α ∧ J = \frac{\sin\theta}{\theta} I + (1-\frac{\sin\theta}{\theta}) \alpha \alpha^T + \frac{1- \cos\theta}{\theta} \alpha^\wedge J=θsinθI+(1−θsinθ)ααT+θ1−cosθα∧可以看到,平移部分經過指數對映之後,發生了一次以 J J J為係數矩陣的線性變換.
-
從 S E ( 3 ) SE(3) SE(3)到 s e ( 3 ) \mathfrak{se}(3) se(3)的對數對映可表示為:
ξ = ln ( T ) ∨ \xi = \ln(T)^\vee ξ=ln(T)∨實際計算時 ϕ \phi ϕ可以由 S O ( 3 ) SO(3) SO(3)到 s o ( 3 ) \mathfrak{so}(3) so(3)的對映得到, ρ \rho ρ可以由 t = J ρ t=J\rho t=Jρ計算得到.
李代數求導: 引入李代數的一大動機就是方便求導優化
李群乘法與李代數加法的關係
-
BCH公式及其近似形式
-
很遺憾地,李群乘積和李代數加法並不等價,即:
R 1 R 2 = exp ( ϕ 1 ∧ ) exp ( ϕ 1 ∧ ) ≠ exp ( ( ϕ 1 + ϕ 2 ) ∧ ) R_1 R_2 = \exp(\phi_1^\wedge) \exp(\phi_1^\wedge) \ne \exp((\phi_1 + \phi_2)^\wedge) R1R2=exp(ϕ1∧)exp(ϕ1∧)=exp((ϕ1+ϕ2)∧)李群乘積與李代數運算的對應關係由BCH公式給出:
ln ( exp ( A ) exp ( B ) ) = A + B + 1 2 [ A , B ] + 1 12 [ A , [ A , B ] ] − 1 12 [ B , [ A , B ] ] + . . . \ln(\exp(A) \exp(B)) = A+B +\frac{1}{2} [A,B] +\frac{1}{12} [A, [A,B]] -\frac{1}{12} [B, [A,B]] + ... ln(exp(A)exp(B))=A+B+21[A,B]+121[A,[A,B]]−121[B,[A,B]]+...
上式中 [ , ] [,] [,]表示李括號運算.
-
當 ϕ 1 \phi_1 ϕ1或 ϕ 2 \phi_2 ϕ2為小量時,可以對BCH公式進行線性近似,得到李群乘積對應的李代數的表示式:
R 1 ⋅ R 2 對 應 的 李 代 數 = ln ( exp ( ϕ 1 ∧ ) exp ( ϕ 1 ∧ ) ) ∨ ≈ { J l ( ϕ 2 ) − 1 ϕ 1 + ϕ 2 當 ϕ 1 為小量時 J r ( ϕ 1 ) − 1 ϕ 2 + ϕ 1 當 ϕ 2 為小量時 R_1 \cdot R_2 對應的李代數 = \ln (\exp(\phi_1^\wedge) \exp(\phi_1^\wedge))^\vee \approx \left\{Jl(ϕ2)−1ϕ1+ϕ2當ϕ1為小量時Jr(ϕ1)−1ϕ2+ϕ1當ϕ2為小量時
-
-
-
\right. R1⋅R2對應的李代數=ln(exp(ϕ1∧)exp(ϕ1∧))∨≈{Jl(ϕ2)−1ϕ1+ϕ2當ϕ1為小量時Jr(ϕ1)−1ϕ2+ϕ1當ϕ2為小量時
其中左乘雅可比矩陣 J l J_l Jl即為從 S E ( 3 ) SE(3) SE(3)到 s e ( 3 ) \mathfrak{se}(3) se(3)對數對映中的雅可比矩陣
J l = sin θ θ I + ( 1 − sin θ θ ) α α T + 1 − cos θ θ α ∧ J_l = \frac{\sin\theta}{\theta} I + (1-\frac{\sin\theta}{\theta}) \alpha \alpha^T + \frac{1- \cos\theta}{\theta} \alpha^\wedge Jl=θsinθI+(1−θsinθ)ααT+θ1−cosθα∧其逆為
J l − 1 = θ 2 cot θ 2 I + ( 1 − θ 2 cot θ 2 ) α α T + θ 2 α ∧ J_l^{-1} = \frac{\theta}{2} \cot{\frac{\theta}{2}} I + (1-\frac{\theta}{2} \cot{\frac{\theta}{2}}) \alpha \alpha^T + \frac{\theta}{2} \alpha^\wedge Jl−1=2θcot2θI+(1−2θcot2θ)ααT+2θα∧右乘雅可比矩陣只需對自變數取負號即可
J r ( ϕ ) = J l ( − ϕ ) J_r(\phi) = J_l(-\phi) Jr(ϕ)=Jl(−ϕ)
-
-
李群 S O ( 3 ) SO(3) SO(3)乘法與李代數 s o ( 3 ) \mathfrak{so}(3) so(3)加法的關係:
-
對旋轉 R R R(李代數為 ϕ \phi ϕ)左乘一個微小旋轉 Δ R \Delta R ΔR(李代數為 Δ ϕ \Delta \phi Δϕ),得到的旋轉李群 Δ R ⋅ R \Delta R\cdot R ΔR⋅R對應的李代數為:
Δ R ⋅ R 對 應 的 李 代 數 = ln ( exp ( Δ ϕ ∧ ) exp ( ϕ ∧ ) ) = ϕ + J l − 1 ( ϕ ) Δ ϕ \Delta R \cdot R 對應的李代數 = \ln \left( \exp(\Delta \phi^\wedge) \exp(\phi^\wedge) \right) = \phi + J_l^{-1}(\phi)\Delta \phi ΔR⋅R對應的李代數=ln(exp(Δϕ∧)exp(ϕ∧))=ϕ+Jl−1(ϕ)Δϕ -
反之,李代數加法 ( ϕ + Δ ϕ ) (\phi+\Delta \phi) (ϕ+Δϕ)對應的李群元素可表示為:
( ϕ + Δ ϕ ) 對 應 的 李 群 = exp ( ( ϕ + Δ ϕ ) ∧ ) = exp ( ( J l Δ ϕ ) ∧ ) exp ( ϕ ∧ ) = exp ( ϕ ∧ ) exp ( ( J r Δ ϕ ) ∧ ) (\phi+\Delta \phi)對應的李群 = \exp((\phi+\Delta \phi)^\wedge) = \exp((J_l \Delta \phi)^\wedge) \exp(\phi^\wedge)= \exp(\phi^\wedge) \exp((J_r \Delta \phi)^\wedge) (ϕ+Δϕ)對應的李群=exp((ϕ+Δϕ)∧)=exp((JlΔϕ)∧)exp(ϕ∧)=exp(ϕ∧)exp((JrΔϕ)∧)
-
-
同理,李群 S E ( 3 ) SE(3) SE(3)乘法與李代數 s e ( 3 ) \mathfrak{se}(3) se(3)加法的關係:
exp ( Δ ξ ∧ ) exp ( ξ ∧ ) ≈ exp ( ( J l − 1 Δ ξ + ξ ) ∧ ) exp ( ξ ∧ ) exp ( Δ ξ ∧ ) ≈ exp ( ( J r − 1 Δ ξ + ξ ) ∧ ) \exp(\Delta \xi^\wedge) \exp(\xi^\wedge) \approx \exp\left( (J_l^{-1}\Delta \xi + \xi)^\wedge \right) \\ \exp(\xi^\wedge) \exp(\Delta \xi^\wedge) \approx \exp\left( (J_r^{-1}\Delta \xi + \xi)^\wedge \right) exp(Δξ∧)exp(ξ∧)≈exp((Jl−1Δξ+ξ)∧)exp(ξ∧)exp(Δξ∧)≈exp((Jr−1Δξ+ξ)∧)
S O ( 3 ) SO(3) SO(3)上的李代數求導
對空間點 p p p進行旋轉,得到 R p Rp Rp,旋轉之後點的座標對旋轉的導數可表示為:
∂ ( R p ) ∂ R \frac{\partial(Rp)}{\partial R} ∂R∂(Rp)
對於上式的求導,有兩種方式:
- 用李代數 ϕ \phi ϕ表示姿態 R R R,然後根據李代數加法對 ϕ \phi ϕ求導.
- 用李代數 φ \varphi φ表示微小擾動 ∂ R \partial R ∂R,然後根據李群左乘對 φ \varphi φ求導.
其中擾動模型表示式簡單,更為實用.
用李代數 ϕ \phi ϕ表示姿態 R R R,求導得到
∂ ( R p ) ∂ R = ∂ ( exp ( ϕ ∧ ) p ) ∂ ϕ = − ( R p ) ∧ J l \frac{\partial(Rp)}{\partial R} = \frac{\partial( \exp(\phi^\wedge) p)}{\partial \phi} = -(Rp) ^\wedge J_l ∂R∂(Rp)=∂ϕ∂(exp(ϕ∧)p)=−(Rp)∧Jl
另一種求導方式是對 R R R進行一次左乘擾動 ∂ R \partial R ∂R,設左乘擾動 ∂ R \partial R ∂R對應的李代數為 φ \varphi φ,對 φ \varphi φ求導,得到
∂ ( R p ) ∂ R = exp ( ( ϕ + φ ) ∧ ) p − exp ( ϕ ∧ ) p φ = − ( R p ) ∧ \frac{\partial(Rp)}{\partial R} = \frac{ \exp((\phi+\varphi)^\wedge)p - \exp(\phi^\wedge)p }{\varphi} =-(Rp) ^\wedge ∂R∂(Rp)=φexp((ϕ+φ)∧)p−exp(ϕ∧)p=−(Rp)∧
S E ( 3 ) SE(3) SE(3)上的李代數求導
類似地,空間點 p p p經過變換 T T T得到 T p Tp Tp,給 T T T左乘一個擾動 Δ T = exp ( δ ξ ∧ ) \Delta T = \exp (\delta \xi ^\wedge) ΔT=exp(δξ∧),則有
∂ ( R p ) δ ξ = [ I − ( R p + t ) ∧ 0 T 0 T ] = ( T P ) ⊙ \frac{\partial(Rp)}{\delta \xi} = \left[
I0T−(Rp+t)∧0T
\right]= (TP) ^ \odot δξ∂(Rp)=[I0T−(Rp+t)∧0T]=(TP)⊙
ch05 相機與影象
針孔相機模型
O − x − y − z O-x-y-z O−x−y−z為相機座標系,現實空間點 P P P的相機座標為 [ X , Y , Z ] T [X,Y,Z]^T [X,Y,Z]T,投影到 O ′ − x ′ − y ′ O'-x'-y' O′−x′−y′平面上的點 P ′ P' P′,座標為 [ X ′ , Y ′ , Z ′ ] T [X',Y',Z']^T [X′,Y′,Z′]T.
-
將成像平面對稱到相機前方,根據幾何相似關係 Z f = X X ′ = Y Y ′ \frac{Z}{f} = \frac{X}{X'} = \frac{Y}{Y'} fZ=X′X=Y′Y,整理得到投影點 P ′ P' P′在投影平面上的座標 P ′ = [ X ′ , Y ′ ] P'=[X',Y'] P′=[X′,Y′]:
{ X ′ = f X Z Y ′ = f Y Z \left\{
X′=fXZY′=fYZ
-
\right. ⎩⎪⎪⎨⎪⎪⎧X′=fZXY′=fZY
-
轉換得到投影點 P ′ P' P′在畫素平面上的畫素座標 P u , v = [ u , v ] T P_{u,v} = [u, v]^T Pu,v=[u,v]T
{ u = α X ′ + c x = f x X Z + c x v = β Y ′ + c y = f x X Z + c x \left\{u=αX′+cxv=βY′+cy=fxXZ+cx=fxXZ+cx
-
\right. ⎩⎪⎪⎨⎪⎪⎧u=αX′+cxv=βY′+cy=fxZX+cx=fxZX+cx
上式中 u u u, v v v, c x c_x cx, c y c_y cy, f x f_x fx, f y f_y fy的單位為畫素, α \alpha α, β \beta β的單位為畫素/米.
-
將上式寫成矩陣形式,得到**現實空間點相機座標 P P P和投影點畫素座標 P u v P_{uv} Puv**之間的關係:
Z P u v = Z [ u v 1 ] = [ f x 0 c x 0 f y c y 0 0 1 ] [ X Y Z ] ≜ K P Z P_{uv} = Z \left[uv1
- \right] = \left[
fx000fy0cxcy1
\right] \left[XYZ
-
\right] \triangleq KP ZPuv=Z⎣⎡uv1⎦⎤=⎣⎡fx000fy0cxcy1⎦⎤⎣⎡XYZ⎦⎤≜KP
其中矩陣 K K K稱為相機的內參數矩陣.
-
上式中的 P P P為現實空間點在相機座標系下的相機座標,將其轉為世界座標 P W P_W PW,有
Z P u v = K ( R P W + t ) = K T P W ZP_{uv} = K(RP_W+t)= KTP_W ZPuv=K(RPW+t)=KTPW
因此 R R R, t t t(或 T T T)又稱為相機的外引數.
-
將最後一維進行歸一化處理,得到點 P P P在歸一化平面的歸一化座標 P c = [ X / Z , Y / Z , 1 ] T P_c=[X/Z, Y/Z, 1]^T Pc=[X/Z,Y/Z,1]T
P c = P Z = K − 1 P u v P_c = \frac{P}{Z}={K^{-1} P_{uv}} Pc=ZP=K−1Puv
引數矩陣有內參數 K K K和外引數 R R R, t t t,其中:
-
內參數矩陣 K K K體現了歸一化相機座標到畫素座標的變換.
之所以是歸一化座標,這體現了投影性質:在某一條直線上的空間點,最終會投影到同一畫素點上.
-
外引數矩陣 R R R, t t t(或 T T T)體現了世界座標到相機座標的變換.
畸變模型
畸變包含兩種: 徑向畸變和切向畸變.
-
徑向畸變: 由透鏡形狀引起,主要包括桶形畸變和枕形畸變.
可以看成座標點沿著長度方向發生了變化,也就是其距離原點的長度發生了變化.
x d i s t o r t e d = x ( 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 ) y d i s t o r t e d = y ( 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 ) x_{distorted} = x(1+ k_1r^2 + k_2r^4 + k_3r6) \\ y_{distorted} = y(1+ k_1r^2 + k_2r^4 + k_3r6) xdistorted=x(1+k1r2+k2r4+k3r6)ydistorted=y(1+k1r2+k2r4+k3r6) -
切向畸變: 由透鏡和成像平面不嚴格平行引起.
可以看成座標點沿著切線方向發生了變化，也就是水平夾角發生了變化.
x d i s t o r t e d = x + 2 p 1 x y + p 2 ( r 2 + 2 x 2 ) y d i s t o r t e d = y + p 1 ( r 2 + 2 y 2 ) + 2 p 2 x y x_{distorted} = x + 2p_1xy + p_2(r^2+2x^2) \\ y_{distorted} = y + p_1(r^2+2y^2) + 2p_2xy xdistorted=x+2p1xy+p2(r2+2x2)ydistorted=y+p1(r2+2y2)+2p2xy
單目相機的成像過程
單目相機的成像過程：
- 世界座標系下有一個固定的原點 P P P,其世界座標 P W P_W PW
- 由於相機在運動,它的運動由 R R R, t t t或變換矩陣 T ∈ S E ( 3 ) T\in SE(3) T∈SE(3)描述.原點 P P P的相機座標 P c ~ = R P W + t \tilde{P_c}=RP_W+t Pc~=RPW+t
- 這時 P c ~ \tilde{P_c} Pc~的分量為 X X X, Y Y Y, Z Z Z,把它們投影到歸一化平面 Z = 1 Z =1 Z=1上,得到 P P P的歸一化相機座標 P c = P c ~ Z = [ X Z , Y Z , 1 ] T P_c =\frac{\tilde{P_c}}{Z}=[\frac{X}{Z},\frac{Y}{Z}, 1] ^T Pc=ZPc~=[ZX,ZY,1]T
- 有畸變時,根據畸變引數計算 P c P_c Pc發生畸變後的歸一化相機座標
- P P P的歸一化相機座標 P c P_c Pc經過內參 K K K後,對應到它的畫素座標 P u v = K P c P_{uv}=KP_c Puv=KPc
在討論相機成像模型時,我們一共談到了四種座標: 世界座標、相機座標、歸一化相機座標和畫素座標.請讀者釐清它們的關係,它反映了整個成像的過程.
ch06 非線性優化
狀態估計問題
最大後驗與最大似然
SLAM模型由狀態方程和運動方程構成:
{ x k = f ( x k − 1 , u k , w k ) z k , j = h ( y j , x k , v k , j ) \left\{xkzk,j=f(xk−1,uk,wk)=h(yj,xk,vk,j)
\right. {xkzk,j=f(xk−1,uk,wk)=h(yj,xk,vk,j)
通常假設兩個噪聲項 w k w_k wk, v k , j v_{k,j} vk,j滿足零均值的高斯分佈:
w k ∼ N ( 0 , R k ) , v k , j ∼ N ( 0 , Q k , j ) w_k \sim \mathcal{N}(0, R_k) ,\; v_{k,j} \sim \mathcal{N}(0, Q_{k,j}) wk∼N(0,Rk),vk,j∼N(0,Qk,j)對機器人的估計,本質上就是已知輸入資料 u u u和觀測資料 z z z的條件下,求機器人位姿 x x x和路標點 y y y的條件概率分佈:
P ( x , y ∣ z , u ) P(x,y|z,u) P(x,y∣z,u)利用貝葉斯法則,有:
P ( x , y ∣ z , u ) = P ( z , u ∣ x , y ) P ( x , y ) P ( z , u ) ∝ P ( z , u ∣ x , y ) P ( x , y ) P(x,y|z,u) = \frac{P(z,u|x,y) P(x,y)}{P(z,u)} \propto P(z,u|x,y) P(x,y) P(x,y∣z,u)=P(z,u)P(z,u∣x,y)P(x,y)∝P(z,u∣x,y)P(x,y)其中 P ( x , y ∣ z , u ) P(x,y|z,u) P(x,y∣z,u)為後驗概率, P ( z , u ∣ x , y ) P(z,u|x,y) P(z,u∣x,y)為似然, P ( x , y ) P(x,y) P(x,y)為先驗,上式可表述為 後 驗 概 率 ∝ 似 然 ⋅ 先 驗 後驗概率 \propto 似然 \cdot 先驗 後驗概率∝似然⋅先驗.直接求後驗分佈是困難的,但是求一個狀態最優估計,使得在該狀態下後驗概率最大化則是可行的:
( x , y ) M A P ∗ = arg max P ( x , y ∣ z , u ) = arg max P ( z , u ∣ x , y ) P ( x , y ) (x,y)^*_{MAP} = \arg \max P(x,y | z,u) = \arg \max P(z,u|x,y) P(x,y) (x,y)MAP∗=argmaxP(x,y∣z,u)=argmaxP(z,u∣x,y)P(x,y)求解最大後驗概率相當於最大化似然和先驗的乘積.因為 x x x, y y y未知,即不知道先驗,則可以求最大似然估計:
( x , y ) M L E ∗ = arg max P ( z , u ∣ x , y ) (x,y)^*_{MLE} = \arg \max P(z,u|x,y) (x,y)MLE∗=argmaxP(z,u∣x,y)最大似然估計的直觀意義為:在什麼樣的狀態下，最可能產生現在觀測到的資料.
最小二乘
對於某一次觀測
z k , j = h ( y j , x k ) + v k , j z_{k,j} = h(y_j, x_k) + v_{k,j} zk,j=h(yj,xk)+vk,j由於假設噪聲 v k , j ∼ N ( 0 , Q k , j ) v_{k,j} \sim \mathcal{N}(0, Q_{k,j}) vk,j∼N(0,Qk,j),則觀測資料 z j , k z_{j,k} zj,k的似然為
P ( z j , k ∣ x k , y j ) = N ( h ( y j , x k ) , Q k , j ) P(z_{j,k}|x_k,y_j) = \mathcal{N} (h(y_j, x_k), Q_{k,j}) P(zj,k∣xk,yj)=N(h(yj,xk),Qk,j)將上式代入高斯分佈表示式中,並取負對數,得到
( x k , y j ) ∗ = arg max N ( h ( y j , x k ) , Q k , j ) = arg min ( ( z k , j − h ( x k , y j ) ) T Q k , j − 1 ( z k , j − h ( x k , y j ) ) )(xk,yj)∗=argmaxN(h(yj,xk),Qk,j)=argmin((zk,j−h(xk,yj))TQ−1k,j(zk,j−h(xk,yj)))
(xk,yj)∗=argmaxN(h(yj,xk),Qk,j)=argmin((zk,j−h(xk,yj))TQk,j−1(zk,j−h(xk,yj)))
上式等價於最小化噪聲項(即誤差)的一個二次型,其中 Q k , j − 1 Q_{k,j}^{-1} Qk,j−1稱為資訊矩陣,即高斯分佈協方差矩陣的逆.
因為觀測 z z z和輸入 u u u是獨立的,因此可對 z z z和 u u u的聯合似然進行因式分解:
P ( x , y ∣ z , u ) = ∏ k P ( u k ∣ x k − 1 , x k ) ∏ k , j P ( z j , k ∣ x k , y j ) P(x,y|z,u) = \prod_k P(u_k|x_{k-1},x_k) \prod_{k,j} P(z_{j,k}|x_k,y_j) P(x,y∣z,u)=k∏P(uk∣xk−1,xk)k,j∏P(zj,k∣xk,yj)定義輸入和觀測資料與模型之間的誤差:
e u , k = x k − f ( x k − 1 , u k ) e z , j , k = z k , j − h ( x k , y j )eu,kez,j,k=xk−f(xk−1,uk)=zk,j−h(xk,yj)
eu,kez,j,k=xk−f(xk−1,uk)=zk,j−h(xk,yj)
定義
J ( x , y ) = ∑ k e u , k T R k − 1 e u , k + ∑ k ∑ j e z , k , j T Q k , j − 1 e z , k , j J(x,y) = \sum_k e_{u,k}^T R_k^{-1}e_{u,k} + \sum_k \sum_j e_{z,k,j}^T Q_{k,j}^{-1}e_{z,k,j} J(x,y)=k∑eu,kTRk−1eu,k+k∑j∑ez,k,jTQk,j−1ez,k,j則有
( x k , y j ) ∗ = arg min J ( x , y ) (x_k,y_j)^* = \arg \min J(x,y) (xk,yj)∗=argminJ(x,y)非線性最小二乘
對於非線性最小二乘問題:
min x F ( x ) = 1 2 ∣ ∣ f ( x ) ∣ ∣ 2 2 \min_{x} F(x) = \frac{1}{2} ||f(x)||_2^2 xminF(x)=21∣∣f(x)∣∣22求解該問題的具體步驟如下:
- 給定某個初始值 x 0 x_0 x0
- 對於第 k k k次迭代,尋找一個增量 Δ x k \Delta x_k Δxk,使得 ∣ ∣ F ( x k + Δ x k ) ∣ ∣ 2 2 ||F(x_k +\Delta x_k)||_2^2 ∣∣F(xk+Δxk)∣∣22達到極小值
- 若 Δ x k \Delta x_k Δxk足夠小,則停止
- 否則,令 x k + 1 = x k + Δ x k x_{k +1} =x_k +\Delta x_k xk+1=xk+Δxk,返回第2步
這樣,最小二乘問題被轉化為一個不斷尋找下降增量 Δ x k \Delta x_k Δxk的問題.,具體有以下方法
一階和二階梯度法
將目標函式 F ( x ) F(x) F(x)在 x k x_k xk附近進行泰勒展開
F ( x k + Δ x k ) ≈ F ( x k ) + J ( x k ) T Δ x k + 1 2 Δ x k T H ( x k ) x k F(x_k +\Delta x_k) \approx F(x_k) + J(x_k)^T \Delta x_k + \frac{1}{2} \Delta x_k^T H(x_k) x_k F(xk+Δxk)≈F(xk)+J(xk)TΔxk+21ΔxkTH(xk)xk其中 J ( x ) J(x) J(x)是 F ( x ) F(x) F(x)關於 x x x的一階導數矩陣, H ( x ) H(x) H(x)是 F ( x ) F(x) F(x)關於 x x x的二階導數矩陣.
-
若 Δ x k \Delta x_k Δxk取一階導數,則
Δ x k ∗ = − J ( x k ) \Delta x_k^* = -J(x_k) Δxk∗=−J(xk) -
若 Δ x k \Delta x_k Δxk取二階導數,則
Δ x k ∗ = arg min ( F ( x k ) + J ( x k ) T Δ x k + 1 2 Δ x k T H ( x k ) x k ) \Delta x_k^* = \arg \min \left( F(x_k) + J(x_k)^T \Delta x_k + \frac{1}{2} \Delta x_k^T H(x_k) x_k \right) Δxk∗=argmin(F(xk)+J(xk)TΔxk+21ΔxkTH(xk)xk)令上式對 Δ x k \Delta x_k Δxk導數等於0,則 Δ x k ∗ \Delta x_k^* Δxk∗可以取 H Δ x k = − J H \Delta x_k = -J HΔxk=−J的解.
高斯牛頓法
將 f ( x k ) f(x_k) f(xk)而非 F ( x k ) F(x_k) F(xk)在 x k x_k xk附近進行泰勒展開
f ( x k + Δ x k ) ≈ f ( x k ) + J ( x k ) T Δ x k f(x_k+\Delta x_k) \approx f(x_k) + J(x_k)^T \Delta x_k f(xk+Δxk)≈f(xk)+J(xk)TΔxk則
Δ x k ∗ = arg min Δ x k 1 2 ∣ ∣ f ( x k ) + J ( x k ) T Δ x k ∣ ∣ 2 \Delta x_k^* = \arg \min_{\Delta x_k} \frac{1}{2} ||f(x_k)+J(x_k)^T \Delta x_k||^2 Δxk∗=argΔxkmin21∣∣f(xk)+J(xk)TΔxk∣∣2令上式對 Δ x \Delta x Δx的導數為0,得到高斯牛頓方程
J ( x k ) f ( x k ) + J ( x k ) J T ( x k ) Δ x k = 0 J(x_k) f(x_k) + J(x_k) J^T(x_k) \Delta x_k = 0 J(xk)f(xk)+J(xk)JT(xk)Δxk=0令 H ( x ) = J ( x ) J T ( x ) H(x)=J(x)J^T(x) H(x)=J(x)JT(x), g ( x ) = − J ( x ) f ( x ) g(x)=-J(x)f(x) g(x)=−J(x)f(x),則 Δ x k ∗ \Delta x_k^* Δxk∗可以取 H Δ x k = g H \Delta x_k = g HΔxk=g的解.
列文伯格-馬夸爾特方法
泰勒展開只能在展開點附近才有較好的近似效果,因此應給 Δ x \Delta x Δx新增一個範圍,稱為信賴區域.
定義一個指標 ρ \rho ρ刻畫這個近似的好壞程度,其分子為實際函式下降的值,分母是近似模型下降的值:
ρ = f ( x + Δ x ) − f ( x ) J ( x ) T Δ x \rho = \frac {f(x+\Delta x)-f(x)} {J(x)^T \Delta x} ρ=J(x)TΔxf(x+Δx)−f(x)通過調整 ρ \rho ρ來確定信賴區域:
- 若 ρ \rho ρ接近1,則近似是最好的.
- 若 ρ \rho ρ太小,說明實際下降的值遠小於近似下降的值,則認為近似比較差,需要縮小近似範圍.
- 若 ρ \rho ρ太大,說明實際下降的比預計的更大,我們可以放大近似範圍.
改良版的非線性優化框架如下:
-
給定初始值 x 0 x_0 x0,以及初始優化半徑 μ \mu μ
-
對於第 k k k次迭代,求解:
min Δ x k 1 2 ∣ ∣ f ( x k ) + J ( x k ) T Δ x k ∣ ∣ 2 s.t. ∣ ∣ D Δ x k ∣ ∣ 2 ≤ μ \min_{\Delta x_k} \frac{1}{2} ||f(x_k)+J(x_k)^T \Delta x_k||^2 \quad \text{s.t.} ||D\Delta x_k||^2 \leq \mu Δxkmin21∣∣f(xk)+J(xk)TΔxk∣∣2s.t.∣∣DΔxk∣∣2≤μ其中, μ \mu μ是信賴區域的半徑, D D D為係數矩陣
-
計算 ρ \rho ρ
-
若 ρ > 3 4 \rho > \frac34 ρ>43則 μ = 2 μ \mu =2\mu μ=2μ
-
若 ρ < 1 4 \rho < \frac14 ρ<41則 μ = 0.5 μ \mu =0.5\mu μ=0.5μ
-
若 ρ \rho ρ大於某閾值,則認為近似可行.令 x k + 1 = x k + Δ x k x_{k +1}=x_k +\Delta x_k xk+1=xk+Δxk
-
判斷演算法是否收斂.如不收斂則返回第2步,否則結束.
第2步中 Δ x k \Delta x_k Δxk的求解要使用拉格朗日乘數法:
L ( Δ x k , λ ) = 1 2 ∣ ∣ f ( x k ) + J ( x k ) T Δ x k ∣ ∣ 2 + λ 2 ( ∣ ∣ D Δ x k ∣ ∣ 2 − μ ) \mathcal{L}(\Delta x_k, \lambda) = \frac{1}{2} ||f(x_k)+J(x_k)^T \Delta x_k||^2+ \frac{\lambda}{2} (||D\Delta x_k||^2 - \mu) L(Δxk,λ)=21∣∣f(xk)+J(xk)TΔxk∣∣2+2λ(∣∣DΔxk∣∣2−μ)令上式對 Δ x k \Delta x_k Δxk導數為0,得到
( H + λ D T D ) Δ x k = g (H+\lambda D^T D) \Delta x_k = g (H+λDTD)Δxk=g考慮簡化形式,即 D = I D=I D=I,則相當於求解
( H + λ I ) Δ x k = g (H+\lambda I) \Delta x_k = g (H+λI)Δxk=g- 當 λ \lambda λ較小時, H H H佔主要地位,這說明二次近似模型在該範圍內是比較好的,列文伯格-馬夸爾特方法更接近於高斯牛頓法.
- 當 λ \lambda λ比較大時, λ I \lambda I λI佔據主要地位,這說明二次近似模型在該範圍內不夠好,列文伯格-馬夸爾特方法更接近於一階梯度下降法.
文章目錄
- ch02 初識SLAM
- ch03 三維空間剛體運動
- ch04 李群與李代數
- ch05 相機與影象
- ch06 非線性優化
- ch07 視覺里程計01
ch07 視覺里程計01
特徵點匹配
特徵點
根據特徵點匹配計算相機運動
根據特徵點匹配計算相機運動.根據相機的成像原理不同,分為以下3種情況：
- 當相機為單目時,我們只知道匹配點的畫素座標,是為2D-2D匹配,使用對極幾何求解.
- 當相機為雙目或RGB-D時,我們就知道匹配點的畫素座標和深度座標,是為3D-3D匹配,使用ICP求解.
- 如果有3D點及其在相機的投影位置,也能估計相機的運動,是為3D-2D匹配,使用PnP求解.
2D-2D匹配: 對極幾何
[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片儲存下來直接上傳(img-QVwt5blH-1587570602884)(1587436458419.png)]{:height=“50%” width=“50%”}
假設我們要求取兩幀影象 I 1 I_1 I1, I 2 I_2 I2之間的運動,設第一幀到第二幀的運動為 R R R, t t t,兩個相機中心分別為 O 1 O_1 O1, O 2 O_2 O2.考慮 I 1 I_1 I1中有一個特徵點 p 1 p_1 p1,它在 I 2 I_2 I2中對應著特徵點 p 2 p_2 p2.連線 O 1 p 1 → \overrightarrow{O_1 p_1} O1p1
和 O 2 p 2 → \overrightarrow{O_2 p_2} O2p2在三維空間中交於點 P P P,這時點 O 1 O_1 O1, O 2 O_2 O2, P P P三個點可以確定一個平面,稱為極平面. O 1 O 2 O_1O_2 O1O2 連線與像平面 I 1 I_1 I1, I 2 I_2 I2的交點分別為 e 1 e_1 e1, e 2 e_2 e2. e 1 e_1 e1, e 2 e_2 e2稱為極點, O 1 O 2 O_1O_2 O1O2稱為基線,極平面與兩個像平面 I 1 I_1 I1, I 2 I_2 I2之間的相交線 l 1 l_1 l1, l 2 l_2 l2稱為極線.
P P P在 I 1 I_1 I1下的相機座標為 P = [ X , Y , Z ] T P=[X,Y,Z]^T P=[X,Y,Z]T,兩個投影畫素點 p 1 p_1 p1, p 2 p_2 p2的畫素位置為 s 1 p 1 = K P s_1 p_1 = K P s1p1=KP, s 2 p 2 = K ( R P + t ) s_2 p_2 = K (RP + t) s2p2=K(RP+t).
取 p 1 p_1 p1, p 2 p_2 p2的歸一化座標 x 1 = K − 1 p 1 x_1 = K^{-1}p_1 x1=K−1p1, x 1 = K − 1 p 2 x_1 = K^{-1} p_2 x1=K−1p2,則可以推得 x 2 ≃ R x 1 + t x_2 \simeq R x_1+ t x2≃Rx1+t.上式中 ≃ \simeq ≃表示尺度意義上相等,即在齊次座標下是相等的,物理上表示對原點成投影關係.
經過推導,得到:
x 2 T t ∧ R x 1 = 0 (1) x_2^T t ^\wedge R x_1 = 0 \tag{1} x2Tt∧Rx1=0(1)
代入 p 1 p_1 p1, p 2 p_2 p2,得到:
p 2 T K − T t ∧ R K − 1 p 1 (2) p_2^T K^{-T} t ^\wedge R K^{-1} p_1 \tag{2} p2TK−Tt∧RK−1p1(2)
式 ( 1 ) (1) (1)和式 ( 2 ) (2) (2)都稱為對極約束,定義基礎矩陣 F F F和本質矩陣 E E E,可以進一步簡化對極約束:
E = t ∧ R F = K − T E K − 1 x 2 T E x 1 = p 2 T F p 1 = 0 (3) E = t ^\wedge R \qquad F = K^{-T}EK^{-1} \qquad x_2^TEx_1=p_2^TFp_1=0 \tag{3} E=t∧RF=K−TEK−1x2TEx1=p2TFp1=0(3)
由於 E E E與 F F F之間只差了相機內參,相機內參是已知的,因此實踐中往往使用形式更簡單的 E E E.考慮到 E E E的尺度等價性,可以用8對點來估計 E E E,是為八點法.
對於一對匹配點,其歸一化座標 x 1 = [ u 1 , v 1 , 1 ] T x_1=[u_1,v_1,1]^T x1=[u1,v1,1]T, x 2 = [ u 2 , v 2 , 1 ] T x_2=[u_2,v_2,1]^T x2=[u2,v2,1]T.根據對極約束,有
( u 1 , v 1 , 1 ) ( e 1 e 2 e 3 e 4 e 5 e 6 e 7 e 8 e 9 ) ( u 2 v 2 1 ) = 0 (u_1, v_1, 1) \left(e1e4e7e2e5e8e3e6e9
\right) \left(u2v21
\right) = 0 (u1,v1,1)⎝⎛e1e4e7e2e5e8e3e6e9⎠⎞⎝⎛u2v21⎠⎞=0
把矩陣 E E E展開為向量 e = [ e 1 , e 2 , e 3 , e 4 , e 5 , e 6 , e 7 , e 8 , e 9 ] T e=[e_1,e_2,e_3,e_4,e_5,e_6,e_7,e_8,e_9]^T e=[e1,e2,e3,e4,e5,e6,e7,e8,e9]T,對極約束可以寫成與 e e e有關的線性形式:
[ u 1 u 2 , u 1 v 2 , u 1 , v 1 u 2 , v 1 v 2 , v 2 , u 2 , v 2 , 1 ] ⋅ e = 0 [u_1u_2,u_1v_2,u_1, v_1u_2,v_1v_2,v_2, u_2,v_2,1] \cdot e = 0 [u1u2,u1v2,u1,v1u2,v1v2,v2,u2,v2,1]⋅e=0
把八對點對應的 x 1 x_1 x1, x 2 x_2 x2分別代入方程中,得到線性方程組:
( u 1 1 u 2 1 u 1 1 v 2 1 u 1 1 v 1 1 u 2 1 v 1 1 v 2 1 v 2 1 u 2 1 v 2 1 1 u 1 1 u 2 2 u 1 2 v 2 2 u 1 2 v 1 2 u 2 2 v 1 2 v 2 2 v 2 2 u 2 2 v 2 2 1 ⋮ ⋮ ⋮ ⋮ ⋮ ⋮ ⋮ ⋮ ⋮ u 1 1 u 2 8 u 1 8 v 2 8 u 1 8 v 1 8 u 2 8 v 1 8 v 2 8 v 2 8 u 2 8 v 2 8 1 ) ( e 1 e 2 e 3 e 4 e 5 e 6 e 7 e 8 e 9 ) = 0 \left(u11u12u11u22⋮u11u82u11v12u21v22⋮u81v82u11u21⋮u81v11u12v21u22⋮v81u82v11v12v21v22⋮v81v82v12v22⋮v82u12u22⋮u82v12v22⋮v8211⋮1
\right) \left(e1e2e3e4e5e6e7e8e9
\right) =0 ⎝⎜⎜⎜⎛u11u21u11u22⋮u11u28u11v21u12v22⋮u18v28u11u12⋮u18v11u21v12u22⋮v18u28v11v21v12v22⋮v18v28v21v22⋮v28u21u22⋮u28v21v22⋮v2811⋮1⎠⎟⎟⎟⎞⎝⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎛e1e2e3e4e5e6e7e8e9⎠⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎞=0
求得E後,對 E E E進行SVD分解以求取 R R R, t t t:設 E E E的SVD分解為 E = U Σ V T E = U \Sigma V^T E=UΣVT,則對應的 R R R, t t t分別為:t ∧ = U R Z ( π 2 ) Σ U T R = U R Z T ( π 2 ) Σ U T t^\wedge = U R_Z(\frac{\pi}{2}) \Sigma U^T \qquad R = U R_Z^T(\frac{\pi}{2}) \Sigma U^T t∧=URZ(2π)ΣUTR=URZT(2π)ΣUT
其中 R Z ( π 2 ) R_Z(\frac{\pi}{2}) RZ(2π)表示沿 Z Z Z軸旋轉90°得到的旋轉矩陣.-
尺度不確定性: 2D影象不具有深度資訊,這導致了單目視覺的尺度不確定性.
實踐中設 t t t為單位1,計算相機運動和和特徵點的3D位置,這被稱為單目SLAM的初始化.
-
初始化的純旋轉問題: 若相機發生純旋轉,導致 t t t為零,得到的 E E E也將為零,會導致我們無從求解R.因此單目初始化不能只有純旋轉,必須要有一定程度的平移.
-
多於8對點的情況:
對於八點法,有 A e = 0 Ae=0 Ae=0,其中 A A A為一個8×9的矩陣.
若匹配點的個數多於8個, A A A的尺寸變化,上述方程不成立.因此轉而求取最小化二次型
min e ∣ ∣ A e ∣ ∣ 2 2 = min e e T A T A e \min_e || Ae ||_2^2 = \min_e e^T A^T A e emin∣∣Ae∣∣22=emineTATAe是為最小二乘意義下的 E E E矩陣.
3D-2D匹配: PnP(Perspective-n-Point)
2D-2D的對極幾何方法需要8個或8個以上的點對（以八點法為例），且存在著初始化、純旋轉和尺度的問題。然而，如果兩張影象中其中一張特徵點的3D位置已知，那麼最少只需3個點對（需要至少一個額外點驗證結果）就可以估計相機運動。
在雙目或RGB-D的視覺里程計中，我們可以直接使用PnP估計相機運動。而在單目視覺里程計中，必須先進行初始化，然後才能使用PnP。
PnP問題有多種解決方法:
- 直接線性表變換(DLT): 先求解相機位姿,再求解空間點位置
- P3P: 先求解空間點位置,再求解相機位姿
- Bundle Adjustment: 最小化重投影誤差,同時求解空間點位置和相機位姿
考慮某個空間點 P P P的齊次世界座標為 P = ( X , Y , Z , 1 ) T P =(X,Y,Z, 1)^T P=(X,Y,Z,1)T .在影象 I 1 I_1 I1中投影到特徵點的歸一化畫素座標 x 1 = ( u 1 , v 1 , 1 ) T x_1 =(u_1, v_1, 1)^T x1=(u1,v1,1)T.此時相機的位姿 R R R, t t t是未知的,定義增廣矩陣 [ R ∣ t ] [R|t ] [R∣t](不同於變換矩陣 T T T)為一個3×4的矩陣,包含了旋轉與平移資訊,展開形式如下:
s ( u 1 v 1 1 ) = ( t 1 t 2 t 3 t 4 t 5 t 6 t 7 t 8 t 9 t 10 t 11 t 12 ) ( X Y Z 1 ) s \left(u1v11
\right) = \left(t1t5t9t2t6t10t3t7t11t4t8t12
\right) \left(XYZ1
\right) s⎝⎛u1v11⎠⎞=⎝⎛t1t5t9t2t6t10t3t7t11t4t8t12⎠⎞⎝⎜⎜⎛XYZ1⎠⎟⎟⎞
用最後一行把s消去,得到兩個約束:
{ t 1 T P − t 3 T P u 1 = 0 t 2 T P − t 3 T P v 1 = 0 \left\{tT1P−tT3Pu1=0tT2P−tT3Pv1=0
\right. {t1TP−t3TPu1=0t2TP−t3TPv1=0
其中 t 1 = ( t 1 , t 2 , t 3 , t 4 ) T \boldsymbol{t}_1 = (t_1, t_2, t_3, t_4)^T t1=(t1,t2,t3,t4)T, t 2 = ( t 5 , t 6 , t 7 , t 8 ) T \boldsymbol{t}_2 = (t_5, t_6, t_7, t_8)^T t2=(t5,t6,t7,t8)T, t 3 = ( t 9 , t 10 , t 11 , t 12 ) T \boldsymbol{t}_3 = (t_9, t_{10}, t_{11}, t_{12})^T t3=(t9,t10,t11,t12)T. t 1 \boldsymbol{t}_1 t1, t 2 \boldsymbol{t}_2 t2, t 3 \boldsymbol{t}_3 t3為待求量.
將 N N N對匹配的特徵點代入方程中,得到線性方程組:
( P 1 T 0 − u 1 P 1 T 0 P 1 T − v 1 P 1 T ⋮ ⋮ ⋮ P N T 0 − u N P N T 0 P N T − v N P N T ) ( t 1 t 2 t 3 ) = 0 \left(PT10⋮PTN00PT1⋮0PTN−u1PT1−v1PT1⋮−uNPTN−vNPTN
\right) \left(t1t2t3
\right) =0 ⎝⎜⎜⎜⎜⎜⎛P1T0⋮PNT00P1T⋮0PNT−u1P1T−v1P1T⋮−uNPNT−vNPNT⎠⎟⎟⎟⎟⎟⎞⎝⎛t1t2t3⎠⎞=0
只需6對匹配點即可求解增廣矩陣 [ R ∣ t ] [R|t ] [R∣t],若匹配點數多於6對時,可以求最小二乘解.對於求解出的旋轉矩陣 R R R,可以通過QR分解等手段將其投影到 S E ( 3 ) SE(3) SE(3)上.
[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片儲存下來直接上傳(img-9IuduXXH-1587570602886)(1587451327097.png)]
已知3對匹配點的世界座標 A A A, B B B, C C C和投影座標 a a a, b b b, c c c,根據三角形的餘弦定理,有
{ O A 2 + O B 2 − 2 O A ⋅ O B ⋅ cos 〈 a , b 〉 = A B 2 O B 2 + O C 2 − 2 O B ⋅ O C ⋅ cos 〈 b , c 〉 = B C 2 O A 2 + O C 2 − 2 O A ⋅ O C ⋅ cos 〈 a , c 〉 = A C 2 \left\{OA2+OB2−2OA⋅OB⋅cos〈a,b〉=AB2OB2+OC2−2OB⋅OC⋅cos〈b,c〉=BC2OA2+OC2−2OA⋅OC⋅cos〈a,c〉=AC2
\right. ⎩⎪⎨⎪⎧OA2+OB2−2OA⋅OB⋅cos〈a,b〉=AB2OB2+OC2−2OB⋅OC⋅cos〈b,c〉=BC2OA2+OC2−2OA⋅OC⋅cos〈a,c〉=AC2
記 x = O A / O C x=OA/OC x=OA/OC, y = O B / O C y=OB/OC y=OB/OC, u = B C 2 / A B 2 u=BC^2/AB^2 u=BC2/AB2, v = A C 2 / A B 2 v=AC^2/AB^2 v=AC2/AB2
{ ( 1 − u ) y 2 − u x 2 − cos 〈 b , c 〉 y + 2 u x y cos 〈 a , b 〉 + 1 = 0 ( 1 − w ) x 2 − w y 2 − cos 〈 a , c 〉 y + 2 w x y cos 〈 a , b 〉 + 1 = 0 \left\{(1−u)y2−ux2−cos〈b,c〉y+2uxycos〈a,b〉+1(1−w)x2−wy2−cos〈a,c〉y+2wxycos〈a,b〉+1=0=0
\right. {(1−u)y2−ux2−cos〈b,c〉y+2uxycos〈a,b〉+1(1−w)x2−wy2−cos〈a,c〉y+2wxycos〈a,b〉+1=0=0
上式中,三個餘弦角 cos 〈 a , b 〉 \cos \langle a,b \rangle cos〈a,b〉, cos 〈 b , c 〉 \cos \langle b,c \rangle cos〈b,c〉, cos 〈 a , c 〉 \cos \langle a,c \rangle cos〈a,c〉以及 u u u, v v v是已知的,可以求解出 x x x, y y y,進而求解出 A A A, B B B, C C C三點的相機座標.然後根據3D-3D的點對,計算相機的運動 R R R, t t t.
Bundle Adjustment: 最小化重投影誤差,同時求解空間點位置和相機位姿
設相機位姿變換矩陣 T T T,某空間點的世界座標 P i = [ X i , Y i , Z i ] T P_i =[X_i,Y_i,Z_i]^T Pi=[Xi,Yi,Zi]T,其投影的畫素座標為 u i = [ u i , v i ] T \boldsymbol{u}_i =[u_i ,v_i ]^T ui=[ui,vi]T,畫素位置與空間點位置的關係如下:
s i u i = K T P i s_i \boldsymbol{u}_i = K T P_i siui=KTPi由於相機位姿未知及觀測點的噪聲,上式存在一個誤差,稱為重投影誤差 e = u i − 1 s i K T P i e=u_i - \frac{1}{s_i} KTP_i e=ui−si1KTPi.因此我們對重投影誤差求和,尋找最好的相機位姿和特徵點的空間位置,最小化重投影誤差:
T ∗ = arg min T 1 2 ∑ i = 1 n ∣ ∣ u i − 1 s i K T P i ∣ ∣ 2 P i ∗ = arg min P i 1 2 ∑ i = 1 n ∣ ∣ u i − 1 s i K T P i ∣ ∣ 2 T^* = \arg \min_{T} \frac{1}{2} \sum_{i=1}^n ||u_i - \frac{1}{s_i} KTP_i||^2 \\ P_i^* = \arg \min_{P_i} \frac{1}{2} \sum_{i=1}^n ||u_i - \frac{1}{s_i} KTP_i||^2 T∗=argTmin21i=1∑n∣∣ui−si1KTPi∣∣2Pi∗=argPimin21i=1∑n∣∣ui−si1KTPi∣∣2使用最小二乘優化,要分別求 e e e對 T T T和 P P P的導數:
e ( x + Δ x ) ≈ e ( x ) + J Δ x e(x+\Delta x) \approx e(x) + J \Delta x e(x+Δx)≈e(x)+JΔx-
求 e e e對 T T T的導數:
當 e e e為畫素座標誤差(2維), x x x為相機位姿(6維)時, J J J將是一個2×6的矩陣.我們來推導 J J J的形式:
取中間變數 P ′ = ( T P ) 1 : 3 = [ X ′ , Y ′ , Z ′ ] T P' = (TP)_{1:3}=[X',Y',Z']^T P′=(TP)1:3=[X′,Y′,Z′]T
使用李代數求導的擾動模型,對 T T T左乘微小擾動 δ ξ \delta \xi δξ,求導得到:
∂ e ∂ δ ξ = lim δ ξ = 0 e ( δ ξ ⊕ ξ ) − e ( ξ ) δ ξ = ∂ e ∂ P ′ ∂ P ′ ∂ δ ξ \frac{\partial e}{\partial \delta \xi} = \lim_{\delta \xi =0} \frac{e(\delta \xi \oplus \xi) - e(\xi)}{\delta \xi} = \frac{\partial e}{\partial P'} \frac{\partial P'}{\partial \delta \xi} ∂δξ∂e=δξ=0limδξe(δξ⊕ξ)−e(ξ)=∂P′∂e∂δξ∂P′其中的 ⊕ \oplus ⊕表示李代數的左乘擾動
其中第一項 ∂ e ∂ P ′ \frac{\partial e}{\partial P'} ∂P′∂e:
∂ e ∂ P ′ = − [ ∂ u ∂ X ′ ∂ u ∂ Y ′ ∂ u ∂ Z ′ ∂ v ∂ X ′ ∂ v ∂ Y ′ ∂ v ∂ Z ′ ] = − [ f x Z ′ 0 − f x X ′ Z ′ 2 0 f y Z ′ − f y Y ′ Z ′ 2 ] \frac{\partial e}{\partial P'} = - \left[∂u∂X′∂v∂X′∂u∂Y′∂v∂Y′∂u∂Z′∂v∂Z′
fxZ′00fyZ′−fxX′Z′2−fyY′Z′2
\right] ∂P′∂e=−[∂X′∂u∂X′∂v∂Y′∂u∂Y′∂v∂Z′∂u∂Z′∂v]=−[Z′fx00Z′fy−Z′2fxX′−Z′2fyY′]
第二項 ∂ P ′ ∂ δ ξ \frac{\partial P'}{\partial \delta \xi} ∂δξ∂P′為變換後的點關於李代數的導數:
∂ P ′ ∂ δ ξ = ( T P ) ∂ δ ξ = ( T P ) ⊙ = [ I − P ′ ∧ 0 T 0 T ] \frac{\partial P'}{\partial \delta \xi} = \frac{(T P)}{\partial \delta \xi} = (TP) ^\odot = \left[I0T−P′∧0T
\right] ∂δξ∂P′=∂δξ(TP)=(TP)⊙=[I0T−P′∧0T]
在 P ′ P' P′定義中,取出前三維,得到
∂ P ′ ∂ δ ξ = [ I , − P ′ ∧ ] \frac{\partial P'}{\partial \delta \xi} = [ I , -P'^\wedge ] ∂δξ∂P′=[I,−P′∧]將兩項相乘,得到了2×6的雅可比矩陣 J T J^T JT
J T = ∂ e ∂ δ ξ = − [ f x Z ′ 0 − f x X ′ Z ′ 2 − f x X ′ Y ′ Z ′ 2 f x + f x X ′ 2 Z ′ 2 − f x Y ′ Z ′ 0 f y Z ′ − f y Y ′ Z ′ 2 − f y − f y Y ′ 2 Z ′ 2 f y X ′ Y ′ Z ′ 2 f y X ′ Z ′ ] J^T = \frac{\partial e}{\partial \delta \xi} = - \left[fxZ′00fyZ′−fxX′Z′2−fyY′Z′2−fxX′Y′Z′2−fy−fyY′2Z′2fx+fxX′2Z′2fyX′Y′Z′2−fxY′Z′fyX′Z′
-
\right] JT=∂δξ∂e=−[Z′fx00Z′fy−Z′2fxX′−Z′2fyY′−Z′2fxX′Y′−fy−Z′2fyY′2fx+Z′2fxX′2Z′2fyX′Y′−Z′fxY′Z′fyX′]
-
求 e e e對 P P P的導數
3D-3D匹配: ICP
對於一組已配對好的3D點:
P = { p 1 , ⋯ , p n } , P ′ = { p 1 ′ , ⋯ , p n ′ } P = \{p_1, \cdots ,p_n\}, \quad P' = \{p_1', \cdots, p_n'\} P={p1,⋯,pn},P′={p1′,⋯,pn′}現在,想要找一個歐氏變換 R R R, t t t,使得:
∀ i , p i = R p i ′ + t \forall i, \quad p_i = R p_i' + t ∀i,pi=Rpi′+tICP問題的求解包含兩種方式:
- 利用線性代數的求解(主要是SVD)
- 利用非線性優化方式的求解(類似於Bundle Adjustment)
定義第 i i i對點的誤差項為 e i = p i − ( R p i ′ + t ) e_i = p_i - (R p'_i + t) ei=pi−(Rpi′+t),定義兩組點的質心 p = 1 n ∑ i = 1 n ( p i ) p = \frac{1}{n} \sum_{i=1}^n (p_i) p=n1∑i=1n(pi), p ′ = 1 n ∑ i = 1 n ( p i ′ ) p' = \frac{1}{n} \sum_{i=1}^n (p_i') p′=n1∑i=1n(pi′)
構建最小二乘問題,求取最合適的 R R R, t t t.
min R , t J = 1 2 ∑ i = 1 n ∣ ∣ ( p i − ( R p i ′ + t ) ) ∣ ∣ 2 2 = 1 2 ∑ i = 1 n ∣ ∣ p i − p − R ( p i ′ − p ′ ) ∣ ∣ 2 + ∣ ∣ p − R p ′ − t ∣ ∣ 2minR,tJ=12∑i=1n||(pi−(Rp′i+t))||22=12∑i=1n||pi−p−R(p′i−p′)||2+||p−Rp′−t||2
R,tminJ=21i=1∑n∣∣(pi−(Rpi′+t))∣∣22=21i=1∑n∣∣pi−p−R(pi′−p′)∣∣2+∣∣p−Rp′−t∣∣2
左邊只和旋轉矩陣 R R R相關,而右邊既有 R R R也有 t t t,但只和質心相關.因此令左邊取最小值解出 R R R,代入到右邊令式子等於0求出 t t t.
定義去質心座標 q i = p i − p q_i=p_i-p qi=pi−p, q i ′ = p i ′ − p ′ q'_i=p'_i-p' qi′=pi′−p′,則優化目標可寫成:
R ∗ = min R ∑ i = 1 n ∣ ∣ p i − p − R ( p i ′ − p ′ ) ∣ ∣ 2 = min R ∑ i = 1 n − q i T R q i ′ = − t r ( R ∑ i = 1 n q i ′ q i T )R∗=minR∑i=1n||pi−p−R(p′i−p′)||2=minR∑i=1n−qTiRq′i=−tr(R∑i=1nq′iqTi)
R∗=Rmini=1∑n∣∣pi−p−R(pi′−p′)∣∣2=Rmini=1∑n−qiTRqi′=−tr(Ri=1∑nqi′qiT)
省略數學證明,定義矩陣:
W = ∑ i = 1 n q i q i ′ T W = \sum_{i=1}^n q_i q_i'^T W=i=1∑nqiqi′T
對矩陣 W W W進行SVD分解得到:
W = U Σ V T W = U \Sigma V^T W=UΣVT
可求解
R = U V T R = UV^T R=UVT使用李代數表達表達位姿,目標函式可以寫成
min ξ = 1 2 ∑ i = 1 n ∣ ∣ ( p i − exp ( ξ ∧ ) p i ′ ) ∣ ∣ 2 2 \min_{\xi} = \frac12 \sum_{i=1}^n ||(p_i - \exp(\xi^\wedge) p_i')||_2^2 ξmin=21i=1∑n∣∣(pi−exp(ξ∧)pi′)∣∣22
誤差項關於位姿的導數可以用李代數求導的擾動模型,計算導數得到:
∂ e ∂ δ ξ = − ( exp ( ξ ∧ ) p i ′ ) ⊙ \frac{\partial e}{\partial \delta \xi} = - (\exp (\xi^\wedge) p_i')^\odot ∂δξ∂e=−(exp(ξ∧)pi′)⊙
可以直接使用最小二乘優化方法求解位姿. -