P3P Kneip - A Novel Parametrization of the Perspective-Three-Point Problem for a Direct Computation of Absolute Camera Position and Orientation論文研讀

前言

Perspective-Three-Point (P3P)問題的目標是在已知 P 1 , P 2 , P 3 P_1,P_2,P_3 P1,P2,P3三個三維點和對應的二維點的情況下，求解相機的外參 R R R與 t t t。本篇論文提出了一種直接計算P3P問題closed-form solution的算法。

本文為Kneip論文裡第二章Theory的筆記，著重於補全論文裡省略的公式推導。

定義新相機座標系

原始相機座標系 ν ： ( C , ( 1 , 0 , 0 ) T , ( 0 , 1 , 0 ) T , ( 0 , 0 , 1 ) T ) \nu：(C,(1,0,0)^T,(0,1,0)^T,(0,0,1)^T) ν：(C,(1,0,0)T,(0,1,0)T,(0,0,1)T)。

相機座標系、相機中心及三點 P 1 , P 2 , P 3 P_1,P_2,P_3 P1,P2,P3的關係如圖所示：

由三點座標 P 1 , P 2 , P 3 P_1,P_2,P_3 P1,P2,P3做normalization得到三個單位向量 f 1 , f 2 , f 3 f_1,f_2,f_3 f1,f2,f3並定義新相機座標系 τ ： ( C , t x , t y , t z ) \tau：(C,t_x,t_y,t_z) τ：(C,tx,ty,tz)。

其中：
t x ⃗ = f 1 ⃗ t z ⃗ = f 1 ⃗ × f 2 ⃗ ∣ ∣ f 1 ⃗ × f 2 ⃗ ∣ ∣ t y ⃗ = t z ⃗ × t x ⃗ \begin{aligned} \vec{t_x} &= \vec{f_1} \\ \vec{t_z} &= \frac{\vec{f_1} \times \vec{f_2}}{||\vec{f_1} \times \vec{f_2}||} \\ \vec{t_y} &= \vec{t_z} \times \vec{t_x} \end{aligned} tx tz ty =f1 =∣∣f1 ×f2 ∣∣f1 ×f2 =tz ×tx
在計算 t y ⃗ \vec{t_y} ty 時，因為 t x ⃗ \vec{t_x} tx 與 t z ⃗ \vec{t_z} tz 正交，所以不需要做normalization。

t x , t y , t z t_x,t_y,t_z tx,ty,tz為 τ \tau τ的basis vector在 ν \nu ν裡的表達，它們組成的矩陣 T = [ t x , t y , t z ] T T = [t_x,t_y,t_z]^T T=[tx,ty,tz]T即由 ν \nu ν到 τ \tau τ的旋轉矩陣。

將點 f i f_i fi從 ν \nu ν轉到 τ \tau τ座標系：

f i τ = T ⋅ f i (1) f_i^\tau = T \cdot f_i \tag{1} fiτ=T⋅fi(1)

驗證： T ⋅ t x = [ t x , t y , t z ] T ⋅ t x = [ 1 , 0 , 0 ] T T \cdot t_x = [t_x,t_y,t_z]^T \cdot t_x = [1,0,0]^T T⋅tx=[tx,ty,tz]T⋅tx=[1,0,0]T，即 t x τ t_x^\tau txτ( t x t_x tx在 τ \tau τ座標系下的表達)。

定義新世界座標系

原始世界座標系： ( O , X , Y , Z ) (O,X,Y,Z) (O,X,Y,Z)

新世界座標系 η = ( P 1 , n x , n y , n z ) \eta = (P_1, n_x, n_y, n_z) η=(P1,nx,ny,nz)

其中：

n x ⃗ = P 1 P 2 ⃗ ∣ ∣ P 1 P 2 ⃗ ∣ ∣ n z ⃗ = n x ⃗ × P 1 P 3 ⃗ ∣ ∣ n x ⃗ × P 1 P 3 ⃗ ∣ ∣ n y ⃗ = n z ⃗ × n x ⃗ \begin{aligned} \vec{n_x} &= \frac{\vec{P_1P_2}}{||\vec{P_1P_2}||} \\ \vec{n_z} &= \frac{\vec{n_x} \times \vec{P_1P_3}}{||\vec{n_x} \times \vec{P_1P_3}||} \\ \vec{n_y} &= \vec{n_z} \times \vec{n_x} \end{aligned} nx nz ny =∣∣P1P2 ∣∣P1P2 =∣∣nx ×P1P3 ∣∣nx ×P1P3 =nz ×nx

由以上定義可以看出， P 1 , P 2 , P 3 P_1,P_2,P_3 P1,P2,P3不共線 η \eta η才可能存在。

新世界座標系及新相機座標系間的關係如圖所示：

由原始世界座標系轉到新世界座標系 η \eta η的旋轉矩陣： N = [ n x , n y , n z ] T N = [n_x,n_y,n_z]^T N=[nx,ny,nz]T。

將點 P i P_i Pi由原始世界座標系轉到新世界座標系 η \eta η：
P i η = N ⋅ ( P i − P 1 ) (2) P_i^\eta = N \cdot (P_i-P_1) \tag{2} Piη=N⋅(Pi−P1)(2)

定義平面 Π \Pi Π座標系

平面 Π \Pi Π座標系： ( P 1 , t x Π , t y Π , t z Π ) (P_1, t_x^\Pi, t_y^\Pi, t_z^\Pi) (P1,txΠ,tyΠ,tzΠ)。

與新相機座標系的關係

定義常數 d 12 = ∣ ∣ P 1 P 2 ∣ ∣ d_{12} = ||P_1P_2|| d12=∣∣P1P2∣∣。

從圖上可以得知：常數 cos ⁡ β = f 1 ⋅ f 2 \cos\beta = f_1 \cdot f_2 cosβ=f1⋅f2及關係式 ∣ ∣ C P 1 ∣ ∣ d 12 = s i n ( π − α − β ) s i n β \frac{||CP_1||}{d_{12}} = \frac{sin(\pi-\alpha-\beta)}{sin\beta} d12∣∣CP1∣∣=sinβsin(π−α−β)。

進一步定義常數 b b b：

b = cot ⁡ β = ± 1 1 − cos ⁡ 2 β − 1 = ± 1 1 − ( f 1 ⋅ f 2 ) 2 − 1 (3) b = \cot \beta = \pm\sqrt{\frac{1}{1-\cos^2\beta}-1} = \pm\sqrt{\frac{1}{1-(f_1 \cdot f_2)^2}-1} \tag{3} b=cotβ=±1−cos2β1−1 =±1−(f1⋅f2)21−1 (3)

相機中心在平面 Π \Pi Π的表達，為 α = ∠ P 2 P 1 C \alpha = \angle P_2P_1C α=∠P2P1C的函數：

C Π ( α ) = ( cos ⁡ α ∣ ∣ C P 1 ∣ ∣ sin ⁡ α ∣ ∣ C P 1 ∣ ∣ 0 ) = ( cos ⁡ α ⋅ d 12 sin ⁡ ( π − α − β ) sin ⁡ − 1 β sin ⁡ α ⋅ d 12 sin ⁡ ( π − α − β ) sin ⁡ − 1 β 0 ) = ( d 12 cos ⁡ α ( sin ⁡ α cot ⁡ β + cos ⁡ α ) d 12 sin ⁡ α ( sin ⁡ α cot ⁡ β + cos ⁡ α ) 0 ) = ( d 12 cos ⁡ α ( sin ⁡ α ⋅ b + cos ⁡ α ) d 12 sin ⁡ α ( sin ⁡ α ⋅ b + cos ⁡ α ) 0 ) (4) \begin{aligned} C^\Pi(\alpha) &= \begin{pmatrix} \cos\alpha ||CP_1||\\ \sin\alpha ||CP_1||\\ 0 \end{pmatrix} \\&= \begin{pmatrix} \cos\alpha \cdot d_{12}\sin(\pi-\alpha-\beta)\sin^{-1}\beta\\ \sin\alpha \cdot d_{12}\sin(\pi-\alpha-\beta)\sin^{-1}\beta\\ 0 \end{pmatrix} \\&= \begin{pmatrix} d_{12}\cos\alpha(\sin\alpha \cot\beta + \cos\alpha)\\ d_{12}\sin\alpha(\sin\alpha \cot\beta + \cos\alpha)\\ 0 \end{pmatrix} \\&= \begin{pmatrix} d_{12}\cos\alpha(\sin\alpha \cdot b + \cos\alpha)\\ d_{12}\sin\alpha(\sin\alpha \cdot b + \cos\alpha)\\ 0 \end{pmatrix} \end{aligned} \tag{4} CΠ(α)=⎝⎛cosα∣∣CP1∣∣sinα∣∣CP1∣∣0⎠⎞=⎝⎛cosα⋅d12sin(π−α−β)sin−1βsinα⋅d12sin(π−α−β)sin−1β0⎠⎞=⎝⎛d12cosα(sinαcotβ+cosα)d12sinα(sinαcotβ+cosα)0⎠⎞=⎝⎛d12cosα(sinα⋅b+cosα)d12sinα(sinα⋅b+cosα)0⎠⎞(4)

τ \tau τ的basis vector t x Π , t y Π , t z Π t_x^{\Pi}, t_y^{\Pi}, t_z^{\Pi} txΠ,tyΠ,tzΠ在 Π \Pi Π裡的表達：

t x Π = ( − cos ⁡ α , − sin ⁡ α , 0 ) T t y Π = ( sin ⁡ α , − cos ⁡ α , 0 ) T t z Π = ( 0 , 0 , 1 ) T \begin{aligned} t_x^\Pi &= (-\cos \alpha, -\sin \alpha, 0)^T \\ t_y^\Pi &= (\sin \alpha, -\cos \alpha, 0)^T \\ t_z^\Pi &= (0, 0, 1)^T \end{aligned} txΠtyΠtzΠ=(−cosα,−sinα,0)T=(sinα,−cosα,0)T=(0,0,1)T

從平面座標系 Π \Pi Π到新相機座標系 τ \tau τ的旋轉矩陣為 [ t x π , t y π , t z π ] T [t_x^\pi, t_y^\pi, t_z^\pi]^T [txπ,tyπ,tzπ]T。

從新相機座標系 τ \tau τ到平面 Π \Pi Π座標系的旋轉矩陣 [ t x π , t y π , t z π ] [t_x^\pi, t_y^\pi, t_z^\pi] [txπ,tyπ,tzπ]。

與新世界座標系的關係

Π \Pi Π與 η \eta η座標系的原點重合。

從平面 Π \Pi Π座標系到新世界座標系 η \eta η的旋轉矩陣為 R θ R_\theta Rθ，即沿 n x n_x nx軸旋轉 θ \theta θ：

R θ = ( 1 0 0 0 cos ⁡ θ − sin ⁡ θ 0 sin ⁡ θ cos ⁡ θ ) \begin{aligned} R_\theta = \begin{pmatrix} 1 & 0 & 0\\ 0 & \cos \theta & -\sin \theta \\ 0 & \sin \theta & \cos \theta \end{pmatrix} \end{aligned} Rθ=⎝⎛1000cosθsinθ0−sinθcosθ⎠⎞

新世界座標系到新相機座標系

相機中心在新世界座標系 η \eta η的表達，為 α \alpha α及 θ \theta θ的函數：

C η ( α , θ ) = R θ ⋅ C Π = ( 1 0 0 0 cos ⁡ θ − sin ⁡ θ 0 sin ⁡ θ cos ⁡ θ ) ⋅ ( d 12 cos ⁡ α ( sin ⁡ α ⋅ b + cos ⁡ α ) d 12 sin ⁡ α ( sin ⁡ α ⋅ b + cos ⁡ α ) 0 ) = ( d 12 cos ⁡ α ( sin ⁡ α ⋅ b + cos ⁡ α ) d 12 sin ⁡ α cos ⁡ θ ( sin ⁡ α ⋅ b + cos ⁡ α ) d 12 sin ⁡ α sin ⁡ θ ( sin ⁡ α ⋅ b + cos ⁡ α ) ) (5) \begin{aligned} C^\eta(\alpha, \theta) &= R_\theta \cdot C^\Pi \\&= \begin{pmatrix} 1 & 0 & 0\\ 0 & \cos \theta & -\sin \theta \\ 0 & \sin \theta & \cos \theta \end{pmatrix} \cdot \begin{pmatrix} d_{12}\cos\alpha(\sin\alpha \cdot b + \cos\alpha)\\ d_{12}\sin\alpha(\sin\alpha \cdot b + \cos\alpha)\\ 0 \end{pmatrix} \\&= \begin{pmatrix} d_{12}\cos\alpha(\sin\alpha \cdot b + \cos\alpha)\\ d_{12}\sin\alpha\cos\theta(\sin\alpha \cdot b + \cos\alpha)\\ d_{12}\sin\alpha\sin\theta(\sin\alpha \cdot b + \cos\alpha)\\ \end{pmatrix} \end{aligned} \tag{5} Cη(α,θ)=Rθ⋅CΠ=⎝⎛1000cosθsinθ0−sinθcosθ⎠⎞⋅⎝⎛d12cosα(sinα⋅b+cosα)d12sinα(sinα⋅b+cosα)0⎠⎞=⎝⎛d12cosα(sinα⋅b+cosα)d12sinαcosθ(sinα⋅b+cosα)d12sinαsinθ(sinα⋅b+cosα)⎠⎞(5)

從新世界座標系 η \eta η到新相機座標系 τ \tau τ的旋轉矩陣=(新相機到新世界)^T=(平面 Π \Pi Π到新世界 * 新相機到平面 Π \Pi Π)^T，寫成數學式如下：

Q ( α , θ ) = [ R θ ⋅ ( t x Π t y Π t z Π ) ] T = [ ( 1 0 0 0 cos ⁡ θ − sin ⁡ θ 0 sin ⁡ θ cos ⁡ θ ) ⋅ ( − cos ⁡ α sin ⁡ α 0 − sin ⁡ α − cos ⁡ α 0 0 0 1 ) ] T = ( − cos ⁡ α sin ⁡ α 0 − sin ⁡ α cos ⁡ θ − cos ⁡ α cos ⁡ θ − sin ⁡ θ − sin ⁡ α sin ⁡ θ − cos ⁡ α sin ⁡ θ cos ⁡ θ ) T = ( − cos ⁡ α − sin ⁡ α cos ⁡ θ − sin ⁡ α sin ⁡ θ sin ⁡ α − cos ⁡ α cos ⁡ θ − cos ⁡ α sin ⁡ θ 0 − sin ⁡ θ cos ⁡ θ ) (6) \begin{aligned} Q(\alpha, \theta) &= [R_\theta \cdot (t_x^\Pi t_y^\Pi t_z^\Pi)]^T \\&= [\begin{pmatrix} 1 & 0 & 0\\ 0 & \cos \theta & -\sin \theta \\ 0 & \sin \theta & \cos \theta \end{pmatrix} \cdot \begin{pmatrix} -\cos \alpha & \sin \alpha & 0\\ -\sin \alpha & -\cos \alpha & 0 \\ 0 & 0 & 1 \end{pmatrix}]^T \\&= \begin{pmatrix} -\cos \alpha & \sin \alpha & 0\\ -\sin \alpha \cos \theta & -\cos \alpha \cos \theta & - \sin\theta \\ -\sin \alpha \sin\theta & -\cos \alpha \sin \theta & \cos \theta \end{pmatrix}^T \\&= \begin{pmatrix} -\cos \alpha & -\sin \alpha \cos \theta & -\sin \alpha \sin\theta\\ \sin \alpha & -\cos \alpha \cos \theta & -\cos \alpha \sin \theta \\ 0 & - \sin\theta & \cos \theta \end{pmatrix} \end{aligned} \tag{6} Q(α,θ)=[Rθ⋅(txΠtyΠtzΠ)]T=[⎝⎛1000cosθsinθ0−sinθcosθ⎠⎞⋅⎝⎛−cosα−sinα0sinα−cosα0001⎠⎞]T=⎝⎛−cosα−sinαcosθ−sinαsinθsinα−cosαcosθ−cosαsinθ0−sinθcosθ⎠⎞T=⎝⎛−cosαsinα0−sinαcosθ−cosαcosθ−sinθ−sinαsinθ−cosαsinθcosθ⎠⎞(6)

將點 P i P_i Pi由新世界座標系 η \eta η轉到新相機座標系 τ \tau τ： P i η = Q ( α , θ ) ⋅ ( P i − C η ( α , θ ) ) P_i^\eta = Q(\alpha, \theta) \cdot (P_i-C^\eta(\alpha, \theta)) Piη=Q(α,θ)⋅(Pi−Cη(α,θ))。

求解 α \alpha α及 θ \theta θ

到目前為止未知的數有 α \alpha α及 θ \theta θ，這裡利用 P 3 τ P_3^\tau P3τ來列出關於 cos ⁡ θ \cos \theta cosθ的四次方程並求解。

因為 P 3 η P_3^\eta P3η在 η \eta η的xy平面上，所以可以將 P 3 η P_3^\eta P3η表示為 ( p 1 , p 2 , 0 ) (p_1,p_2,0) (p1,p2,0)。

欲求 P 3 τ P_3^\tau P3τ，套用上面由新世界座標系的 P 3 η P_3^\eta P3η到新相機座標系的公式：

P 3 τ = Q ( α , θ ) ⋅ ( P 3 η − C η ( α , θ ) ) = ( − cos ⁡ α − sin ⁡ α cos ⁡ θ − sin ⁡ α sin ⁡ θ sin ⁡ α − cos ⁡ α cos ⁡ θ − cos ⁡ α sin ⁡ θ 0 − sin ⁡ θ cos ⁡ θ ) ⋅ ( ( p 1 p 2 0 ) − ( d 12 cos ⁡ α ( sin ⁡ α ⋅ b + cos ⁡ α ) d 12 sin ⁡ α cos ⁡ θ ( sin ⁡ α ⋅ b + cos ⁡ α ) d 12 sin ⁡ α sin ⁡ θ ( sin ⁡ α ⋅ b + cos ⁡ α ) ) ) = ( − cos ⁡ α ⋅ p 1 − sin ⁡ α cos ⁡ θ ⋅ p 2 + d 12 ( sin ⁡ α ⋅ b + cos ⁡ α ) ( cos ⁡ 2 α + sin ⁡ 2 α cos ⁡ 2 θ + sin ⁡ 2 α sin ⁡ 2 θ ) sin ⁡ α ⋅ p 1 − cos ⁡ α cos ⁡ θ ⋅ p 2 + d 12 ( sin ⁡ α ⋅ b + cos ⁡ α ) ( − sin ⁡ α cos ⁡ α + sin ⁡ α cos ⁡ α cos ⁡ 2 θ + sin ⁡ α cos ⁡ α sin ⁡ 2 θ ) − sin ⁡ θ ⋅ p 2 + d 12 ( sin ⁡ α ⋅ b + cos ⁡ α ) ( sin ⁡ α sin ⁡ θ cos ⁡ θ − sin ⁡ α sin ⁡ θ cos ⁡ θ ) ) = ( − cos ⁡ α ⋅ p 1 − sin ⁡ α cos ⁡ θ ⋅ p 2 + d 12 ( sin ⁡ α ⋅ b + cos ⁡ α ) sin ⁡ α ⋅ p 1 − cos ⁡ α cos ⁡ θ ⋅ p 2 − sin ⁡ θ ⋅ p 2 ) (7) \small \begin{aligned} P_3^\tau &= Q(\alpha, \theta) \cdot (P_3^\eta-C^\eta(\alpha,\theta)) \\&= \begin{pmatrix} -\cos \alpha & -\sin \alpha \cos \theta & -\sin \alpha \sin\theta\\ \sin \alpha & -\cos \alpha \cos \theta & -\cos \alpha \sin \theta \\ 0 & - \sin\theta & \cos \theta \end{pmatrix} \cdot (\begin{pmatrix} p_1\\ p_2\\ 0\\ \end{pmatrix}-\begin{pmatrix} d_{12}\cos\alpha(\sin\alpha \cdot b + \cos\alpha)\\ d_{12}\sin\alpha\cos\theta(\sin\alpha \cdot b + \cos\alpha)\\ d_{12}\sin\alpha\sin\theta(\sin\alpha \cdot b + \cos\alpha)\\ \end{pmatrix}) \\&= \begin{pmatrix} -\cos \alpha \cdot p_1 -\sin \alpha \cos \theta \cdot p_2 + d_{12} (\sin\alpha \cdot b + \cos\alpha)(\cos^2\alpha + \sin^2\alpha \cos^2\theta + \sin^2\alpha \sin^2\theta) \\ \sin \alpha \cdot p_1 -\cos \alpha \cos \theta \cdot p_2 + d_{12} (\sin\alpha \cdot b + \cos\alpha)(-\sin\alpha \cos \alpha + \sin\alpha \cos \alpha \cos^2 \theta + \sin\alpha \cos \alpha \sin^2 \theta) \\ -\sin \theta \cdot p_2 + d_{12} (\sin\alpha \cdot b + \cos\alpha)(\sin\alpha \sin \theta \cos \theta - \sin\alpha \sin \theta \cos \theta) \end{pmatrix} \\&= \begin{pmatrix} -\cos \alpha \cdot p_1 -\sin \alpha \cos \theta \cdot p_2 + d_{12} (\sin\alpha \cdot b + \cos\alpha) \\ \sin \alpha \cdot p_1 -\cos \alpha \cos \theta \cdot p_2 \\ -\sin \theta \cdot p_2 \end{pmatrix} \end{aligned} \tag{7} P3τ=Q(α,θ)⋅(P3η−Cη(α,θ))=(−cosαsinα0−sinαcosθ−cosαcosθ−sinθ−sinαsinθ−cosαsinθcosθ)⋅((p1p20)−(d12cosα(sinα⋅b+cosα)d12sinαcosθ(sinα⋅b+cosα)d12sinαsinθ(sinα⋅b+cosα)))=(−cosα⋅p1−sinαcosθ⋅p2+d12(sinα⋅b+cosα)(cos2α+sin2αcos2θ+sin2αsin2θ)sinα⋅p1−cosαcosθ⋅p2+d12(sinα⋅b+cosα)(−sinαcosα+sinαcosαcos2θ+sinαcosαsin2θ)−sinθ⋅p2+d12(sinα⋅b+cosα)(sinαsinθcosθ−sinαsinθcosθ))=(−cosα⋅p1−sinαcosθ⋅p2+d12(sinα⋅b+cosα)sinα⋅p1−cosαcosθ⋅p2−sinθ⋅p2)(7)

相機座標系下 P 3 τ P_3^\tau P3τ經過縮放變成 f 3 τ f_3^\tau f3τ( P 3 τ P_3^\tau P3τ與 f 3 τ f_3^\tau f3τ的方向需相同)。

首先定義兩個常數：

ϕ 1 = f 3 , x τ f 3 , z τ and ϕ 2 = f 3 , y τ f 3 , z τ (8) \phi_1 = \frac{f^\tau_{3,x}}{f^\tau_{3,z}} \text{ and } \phi_2 = \frac{f^\tau_{3,y}}{f^\tau_{3,z}} \tag{8} ϕ1=f3,zτf3,xτ and ϕ2=f3,zτf3,yτ(8)

並有以下關係：

{ ϕ 1 = P 3 , x τ P 3 , z τ ϕ 2 = P 3 , y τ P 3 , z τ ⇔ { ϕ 1 = − cos ⁡ α ⋅ p 1 − sin ⁡ α cos ⁡ θ ⋅ p 2 + d 12 ( sin ⁡ α ⋅ b + cos ⁡ α ) − sin ⁡ θ ⋅ p 2 ϕ 2 = sin ⁡ α ⋅ p 1 − cos ⁡ α cos ⁡ θ ⋅ p 2 − sin ⁡ θ ⋅ p 2 ⇔ { sin ⁡ θ sin ⁡ α p 2 = − cot ⁡ α ⋅ p 1 − cos ⁡ θ ⋅ p 2 + d 12 ( b + cot ⁡ α ) − ϕ 1 sin ⁡ θ sin ⁡ α p 2 = p 1 − cot ⁡ α cos ⁡ θ ⋅ p 2 − ϕ 2 ⇒ { sin ⁡ θ sin ⁡ α p 2 = − cot ⁡ α ⋅ p 1 − cos ⁡ θ ⋅ p 2 + d 12 ( b + cot ⁡ α ) − ϕ 1 sin ⁡ θ sin ⁡ α p 2 = p 1 − cot ⁡ α cos ⁡ θ ⋅ p 2 − ϕ 2 ⇒ ϕ 2 ( cot ⁡ α + cos ⁡ θ ⋅ p 2 − d 12 b − d 12 cot ⁡ α ) = ϕ 1 ( − p 1 + cot ⁡ α cos ⁡ θ ⋅ p 2 ) ⇒ ( ϕ 2 ( p 1 − d 12 ) − cos ⁡ θ p 2 ϕ 1 ) cot ⁡ α = − p 1 ϕ 1 − ϕ 2 ( cos ⁡ θ ⋅ p 2 − d 12 b ) ) ⇒ cot ⁡ α = p 1 ϕ 1 + ϕ 2 ( cos ⁡ θ ⋅ p 2 − d 12 b ) ) cos ⁡ θ p 2 ϕ 1 − ( ϕ 2 ( p 1 − d 12 ) = ϕ 1 ϕ 2 p 1 + cos ⁡ θ ⋅ p 2 − d 12 ⋅ b ϕ 1 ϕ 2 cos ⁡ θ ⋅ p 2 − p 1 + d 12 (9) \begin{aligned} &\begin{cases} \phi_1 = \frac{P^\tau_{3,x}}{P^\tau_{3,z}} \\ \phi_2 = \frac{P^\tau_{3,y}}{P^\tau_{3,z}} \end{cases} \\ &\Leftrightarrow \begin{cases} \phi_1 = \frac{-\cos \alpha \cdot p_1 -\sin \alpha \cos \theta \cdot p_2 + d_{12} (\sin\alpha \cdot b + \cos\alpha)}{-\sin \theta \cdot p_2 } \\ \phi_2 = \frac{\sin \alpha \cdot p_1 -\cos \alpha \cos \theta \cdot p_2}{-\sin \theta \cdot p_2 } \end{cases} \\ &\Leftrightarrow \begin{cases} \frac{\sin \theta}{\sin \alpha}p_2 = \frac{-\cot \alpha \cdot p_1 - \cos \theta \cdot p_2 + d_{12} (b + \cot\alpha)}{-\phi_1} \\ \frac{\sin \theta}{\sin \alpha}p_2 = \frac{p_1 -\cot \alpha \cos \theta \cdot p_2}{-\phi_2} \end{cases} \\ &\Rightarrow \begin{cases} \frac{\sin \theta}{\sin \alpha}p_2 = \frac{-\cot \alpha \cdot p_1 - \cos \theta \cdot p_2 + d_{12} (b + \cot\alpha)}{-\phi_1} \\ \frac{\sin \theta}{\sin \alpha}p_2 = \frac{p_1 -\cot \alpha \cos \theta \cdot p_2}{-\phi_2} \end{cases} \\ &\Rightarrow \phi_2(\cot\alpha+\cos\theta \cdot p_2 -d_{12}b - d_{12}\cot \alpha) = \phi_1(-p_1 + \cot \alpha \cos \theta \cdot p_2) \\ &\Rightarrow (\phi_2(p_1-d_{12}) - \cos\theta p_2 \phi_1)\cot \alpha = -p_1\phi_1-\phi_2(\cos\theta \cdot p_2 - d_{12}b)) \\ &\Rightarrow \cot \alpha = \frac{p_1\phi_1+\phi_2(\cos\theta \cdot p_2 - d_{12}b))}{\cos\theta p_2 \phi_1 - (\phi_2(p_1-d_{12})} = \frac{\frac{\phi_1}{\phi_2}p_1 + \cos \theta \cdot p_2 - d_{12} \cdot b}{\frac{\phi_1}{\phi_2} \cos \theta \cdot p_2 - p_1 + d_{12}} \end{aligned} \tag{9} ⎩⎨⎧ϕ1=P3,zτP3,xτϕ2=P3,zτP3,yτ⇔{ϕ1=−sinθ⋅p2−cosα⋅p1−sinαcosθ⋅p2+d12(sinα⋅b+cosα)ϕ2=−sinθ⋅p2sinα⋅p1−cosαcosθ⋅p2⇔{sinαsinθp2=−ϕ1−cotα⋅p1−cosθ⋅p2+d12(b+cotα)sinαsinθp2=−ϕ2p1−cotαcosθ⋅p2⇒{sinαsinθp2=−ϕ1−cotα⋅p1−cosθ⋅p2+d12(b+cotα)sinαsinθp2=−ϕ2p1−cotαcosθ⋅p2⇒ϕ2(cotα+cosθ⋅p2−d12b−d12cotα)=ϕ1(−p1+cotαcosθ⋅p2)⇒(ϕ2(p1−d12)−cosθp2ϕ1)cotα=−p1ϕ1−ϕ2(cosθ⋅p2−d12b))⇒cotα=cosθp2ϕ1−(ϕ2(p1−d12)p1ϕ1+ϕ2(cosθ⋅p2−d12b))=ϕ2ϕ1cosθ⋅p2−p1+d12ϕ2ϕ1p1+cosθ⋅p2−d12⋅b(9)

到了這裡我們得到了以已知數 ϕ 1 \phi_1 ϕ1, ϕ 2 \phi_2 ϕ2及未知數 cos ⁡ θ \cos \theta cosθ表達的 cot ⁡ α \cot \alpha cotα。

ϕ 2 = P 3 , y τ P 3 , z τ ⇔ ϕ 2 = sin ⁡ α ⋅ p 1 − cos ⁡ α cos ⁡ θ ⋅ p 2 − sin ⁡ θ ⋅ p 2 ⇔ ( − sin ⁡ θ p 2 ϕ 2 ) 2 = ( sin ⁡ α ⋅ p 1 − cos ⁡ α cos ⁡ θ ⋅ p 2 ) 2 ⇔ sin ⁡ 2 θ p 2 2 ϕ 2 2 = sin ⁡ 2 α ( p 1 − cot ⁡ α cos ⁡ θ ⋅ p 2 ) 2 ⇔ sin ⁡ 2 θ sec ⁡ 2 α p 2 2 ϕ 2 2 = ( p 1 − cot ⁡ α cos ⁡ θ ⋅ p 2 ) 2 ⇔ ( 1 − cos ⁡ 2 θ ) ( 1 + cot ⁡ 2 α ) ϕ 2 2 p 2 2 = p 1 2 − 2 cot ⁡ α cos ⁡ θ ⋅ p 1 p 2 + cot ⁡ 2 α cos ⁡ 2 θ ⋅ p 2 2 ⇔ ( 1 − cos ⁡ 2 θ ) ϕ 2 2 p 2 2 cot ⁡ 2 α + ( 1 − cos ⁡ 2 θ ) ϕ 2 2 p 2 2 = p 1 2 − 2 cot ⁡ α cos ⁡ θ ⋅ p 1 p 2 + cot ⁡ 2 α cos ⁡ 2 θ ⋅ p 2 2 ⇔ ( ( 1 − cos ⁡ 2 θ ) ϕ 2 2 p 2 2 − cos ⁡ 2 θ ⋅ p 2 2 ) cot ⁡ 2 α + ( 1 − cos ⁡ 2 θ ) ϕ 2 2 p 2 2 = p 1 2 − 2 cot ⁡ α cos ⁡ θ ⋅ p 1 p 2 (10) \begin{aligned}\phi_2 = \frac{P^\tau_{3,y}}{P^\tau_{3,z}} &\Leftrightarrow \phi_2 = \frac{\sin \alpha \cdot p_1 -\cos \alpha \cos \theta \cdot p_2}{-\sin \theta \cdot p_2} \\ &\Leftrightarrow (-\sin \theta p_2 \phi_2)^2 = (\sin \alpha \cdot p_1 -\cos \alpha \cos \theta \cdot p_2)^2 \\ &\Leftrightarrow \sin^2 \theta p_2^2 \phi_2^2 = \sin^2 \alpha(p_1 -\cot \alpha \cos \theta \cdot p_2)^2 \\ &\Leftrightarrow \sin^2 \theta \sec^2 \alpha p_2^2 \phi_2^2 = (p_1 -\cot \alpha \cos \theta \cdot p_2)^2 \\ &\Leftrightarrow (1-\cos^2 \theta) (1+\cot^2 \alpha) \phi_2^2 p_2^2 = p_1^2 -2\cot \alpha \cos \theta \cdot p_1p_2 + \cot^2 \alpha \cos^2 \theta \cdot p_2^2 \\ &\Leftrightarrow (1-\cos^2 \theta)\phi_2^2 p_2^2\cot^2 \alpha + (1-\cos^2 \theta) \phi_2^2 p_2^2 = p_1^2 -2\cot \alpha \cos \theta \cdot p_1p_2 + \cot^2 \alpha \cos^2 \theta \cdot p_2^2 \\ &\Leftrightarrow ((1-\cos^2 \theta)\phi_2^2 p_2^2 - \cos^2 \theta \cdot p_2^2)\cot^2 \alpha + (1-\cos^2 \theta) \phi_2^2 p_2^2 = p_1^2 -2\cot \alpha \cos \theta \cdot p_1p_2\end{aligned} \tag{10} ϕ2=P3,zτP3,yτ⇔ϕ2=−sinθ⋅p2sinα⋅p1−cosαcosθ⋅p2⇔(−sinθp2ϕ2)2=(sinα⋅p1−cosαcosθ⋅p2)2⇔sin2θp22ϕ22=sin2α(p1−cotαcosθ⋅p2)2⇔sin2θsec2αp22ϕ22=(p1−cotαcosθ⋅p2)2⇔(1−cos2θ)(1+cot2α)ϕ22p22=p12−2cotαcosθ⋅p1p2+cot2αcos2θ⋅p22⇔(1−cos2θ)ϕ22p22cot2α+(1−cos2θ)ϕ22p22=p12−2cotαcosθ⋅p1p2+cot2αcos2θ⋅p22⇔((1−cos2θ)ϕ22p22−cos2θ⋅p22)cot2α+(1−cos2θ)ϕ22p22=p12−2cotαcosθ⋅p1p2(10)

得到 cos ⁡ θ \cos\theta cosθ和 cot ⁡ α \cot\alpha cotα的關係後，把公式(9)代入公式(10)，就可以列出 cos ⁡ θ \cos\theta cosθ的四次方程式：

a 4 ⋅ cos ⁡ 4 θ + a 3 ⋅ cos ⁡ 3 θ + a 2 ⋅ cos ⁡ 2 θ + a 1 ⋅ cos ⁡ θ + a 0 = 0 (11) a_4 \cdot \cos^4\theta + a_3 \cdot \cos^3\theta + a_2 \cdot \cos^2\theta + a_1 \cdot \cos\theta + a_0 = 0 \tag{11} a4⋅cos4θ+a3⋅cos3θ+a2⋅cos2θ+a1⋅cosθ+a0=0(11)

接著求出最多四個 cos ⁡ θ \cos\theta cosθ的解，每個 cos ⁡ θ \cos\theta cosθ對應一個 cot ⁡ α \cot\alpha cotα。

然後由 cos ⁡ θ \cos\theta cosθ及 cot ⁡ α \cot\alpha cotα計算出 θ \theta θ及 α \alpha α。

求解R和C

得到了 θ \theta θ及 α \alpha α後，依公式(5)及(6)計算出 C η ( α , θ ) C^\eta(\alpha,\theta) Cη(α,θ)及 Q ( α , θ ) Q(\alpha,\theta) Q(α,θ)。

相機中心在原始世界座標系裡的表達可由公式(2)的反運算得到：

C = P 1 + N T ⋅ C η (12) C = P_1 + N^T \cdot C^\eta \tag{12} C=P1+NT⋅Cη(12)

原始相機座標系 ν \nu ν到原始世界座標系的旋轉矩陣=新世界座標系 η \eta η到原世界座標系 * 新相機座標系 τ \tau τ到新世界座標系 η \eta η * 原始相機座標系 ν \nu ν到新相機座標系 τ \tau τ，寫成數學式如下：

R = N T ⋅ Q T ⋅ T (13) R = N^T \cdot Q^T \cdot T \tag{13} R=NT⋅QT⋅T(13)

將 C η C^\eta Cη代入公式(12)可得 C C C，將 Q Q Q代入公式(13)可得 R R R，至此我們已經求出了相機的四組外參。

通過對第四點重投影，我們可以選擇其中誤差最小的來當作最終的解。

P3P Kneip - A Novel Parametrization of the Perspective-Three-Point Problem for a Direct Computation相关推荐

基于图像的三维重建——P3P求解相机位姿(5)
文章目录 P3P简介直接线性变换法 Kneip算法算法原理及流程基于RANSAC的Kneip算法 P3P简介相机位姿求解是SFM中必要的一步,而P3P问题的目的就是通过三维点(3对)对应关系确 ...
MLpnp论文阅读及算法思想
MLpnp 整体上看,MLpnp是非常不错的位姿估计方法,速度快,准确率高. 参考博客:https://blog.csdn.net/qq_39266065/article/details/115614 ...
相机模型-鱼眼模型/鱼眼镜头标定基本原理及实现(2)
问题: 1 广角/超广角与鱼眼摄像机,角度界限 2 畸变模型中radtan畸变模型与鱼眼畸变模型在小于150范围是否都时能适用. (同数据,拟合模型不同,,参数结果不同,不欠拟合和过拟合就可) 3 ...
SLAM 中常用的相机模型畸变模型总结
文章目录 Overview Camera models Pinhole omnidirectional Distortion models Equidistant (EQUI) Radtan FOV ...
3月9日笔记：RANSAC随机样本一致性,灭点、对极几何计算、H矩阵、PNP估计相机位置，3D匹配、投影变换、N点定位求解姿态
[修订]: 2020-03-11:特征匹配分为:(1)2D-2D : (2)2D-3D : (3)3D-3D https://www.zhihu.com/question/51510464 2D-2 ...
OpenPR开源代码项目
欢迎大家访问OpenPR主页: http://www.openpr.org.cn , 并提出意见和建议!同时,OpenPR也期待您分享您的代码! OpenPR, stands for Open Pa ...
开源工具：OpenPR
欢迎大家访问OpenPR主页: http://www.openpr.org.cn, 并提出意见和建议!同时,OpenPR也期待您分享您的代码! OpenPR, stands for Open Patt ...
OpenMVG、OpenMVS配置及学习记录（Win10+VS2015）
更新于2018年7月24日经过了近一年的学习,总结各方面经验,还是极其强烈的建议初入三维重建的朋友几点: 1.不要在Windows系统下装openmvg和openmvs了,直接用上Linux系统吧, ...
vba交付图表设计_您是在为交付目的而“设计”吗？
vba交付图表设计重点 (Top highlight) It's a regular Monday morning. All the design team is organizing the ta ...

P3P Kneip - A Novel Parametrization of the Perspective-Three-Point Problem for a Direct Computation