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

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$三個三維點和對應的二維點的情況下，求解相機的外參$R$與$t$。本篇論文提出了一種直接計算P3P問題closed-form solution的算法。

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

定義新相機座標系

原始相機座標系$\nu ：(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$做normalization得到三個單位向量$f_1,f_2,f_3$並定義新相機座標系$\tau ：(C,t_x,t_y,t_z)$。

其中：
$$\begin{array}{rl}\overrightarrow{t_x}& =\overrightarrow{f_1}\\ \overrightarrow{t_z}& =\frac{\overrightarrow{f_1}\times \overrightarrow{f_2}}{||\overrightarrow{f_1}\times \overrightarrow{f_2}||}\\ \overrightarrow{t_y}& =\overrightarrow{t_z}\times \overrightarrow{t_x}\end{array}$$
在計算$\overrightarrow{t_y}$時，因為$\overrightarrow{t_x}$與$\overrightarrow{t_z}$正交，所以不需要做normalization。

$t_x,t_y,t_z$為$\tau$的basis vector在$\nu$裡的表達，它們組成的矩陣$T=[t_x,t_y,t_z{]}^T$即由$\nu$到$\tau$的旋轉矩陣。

將點$f_i$從$\nu$轉到$\tau$座標系：

$$f_i^{\tau }=T\cdot f_i\text{\hspace{1em}(1)}$$

驗證：$T\cdot t_x=[t_x,t_y,t_z{]}^T\cdot t_x=[1,0,0{]}^T$，即$t_x^{\tau }$($t_x$在$\tau$座標系下的表達)。

定義新世界座標系

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

新世界座標系$\eta =(P_1,n_x,n_y,n_z)$

其中：

$$\begin{array}{rl}\overrightarrow{n_x}& =\frac{\overrightarrow{P_1{P}_2}}{||\overrightarrow{P_1{P}_2}||}\\ \overrightarrow{n_z}& =\frac{\overrightarrow{n_x}\times \overrightarrow{P_1{P}_3}}{||\overrightarrow{n_x}\times \overrightarrow{P_1{P}_3}||}\\ \overrightarrow{n_y}& =\overrightarrow{n_z}\times \overrightarrow{n_x}\end{array}$$

由以上定義可以看出，$P_1,P_2,P_3$不共線$\eta$才可能存在。

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

由原始世界座標系轉到新世界座標系$\eta$的旋轉矩陣：$N=[n_x,n_y,n_z{]}^T$。

將點$P_i$由原始世界座標系轉到新世界座標系$\eta$：
$$P_i^{\eta }=N\cdot (P_i-P_1)\text{\hspace{1em}(2)}$$

定義平面$\Pi$座標系

平面$\Pi$座標系：$(P_1,t_x^{\Pi },t_y^{\Pi },t_z^{\Pi })$。

與新相機座標系的關係

定義常數$d_{12}=||P_1{P}_2||$。

從圖上可以得知：常數$\cos \beta =f_1\cdot f_2$及關係式$\frac{||C{P}_1||}{d_{12}}=\frac{sin(\pi -\alpha -\beta )}{sin\beta }$。

進一步定義常數$b$：

$$b=\cot \beta =\pm \sqrt{\frac{1}{1-{\cos }^2\beta }-1}=\pm \sqrt{\frac{1}{1-(f_1\cdot f_2{)}^2}-1}\text{\hspace{1em}(3)}$$

相機中心在平面$\Pi$的表達，為$\alpha =\angle P_2{P}_{1}C$的函數：

$$\begin{array}{rl}{C}^{\Pi }(\alpha )& =\left(\begin{array}{c}\cos \alpha ||C{P}_1||\\ \sin \alpha ||C{P}_1||\\ 0\end{array}\right)\\ & =\left(\begin{array}{c}\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{array}\right)\\ & =\left(\begin{array}{c}{d}_{12}\cos \alpha (\sin \alpha \cot \beta +\cos \alpha )\\ d_{12}\sin \alpha (\sin \alpha \cot \beta +\cos \alpha )\\ 0\end{array}\right)\\ & =\left(\begin{array}{c}{d}_{12}\cos \alpha (\sin \alpha \cdot b+\cos \alpha )\\ d_{12}\sin \alpha (\sin \alpha \cdot b+\cos \alpha )\\ 0\end{array}\right)\end{array}\text{\hspace{1em}(4)}$$

$\tau$的basis vector$t_x^{\Pi },t_y^{\Pi },t_z^{\Pi }$在$\Pi$裡的表達：

$$\begin{array}{rl}{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{array}$$

從平面座標系$\Pi$到新相機座標系$\tau$的旋轉矩陣為$[t_x^{\pi },t_y^{\pi },t_z^{\pi }{]}^T$。

從新相機座標系$\tau$到平面$\Pi$座標系的旋轉矩陣$[t_x^{\pi },t_y^{\pi },t_z^{\pi }]$。

與新世界座標系的關係

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

從平面$\Pi$座標系到新世界座標系$\eta$的旋轉矩陣為$R_{\theta }$，即沿$n_x$軸旋轉$\theta$：

$$\begin{array}{r}{R}_{\theta }=\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos \theta & -\sin \theta \\ 0& \sin \theta & \cos \theta \end{array}\right)\end{array}$$

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

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

$$\begin{array}{rl}{C}^{\eta }(\alpha ,\theta )& =R_{\theta }\cdot C^{\Pi }\\ & =\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos \theta & -\sin \theta \\ 0& \sin \theta & \cos \theta \end{array}\right)\cdot \left(\begin{array}{c}{d}_{12}\cos \alpha (\sin \alpha \cdot b+\cos \alpha )\\ d_{12}\sin \alpha (\sin \alpha \cdot b+\cos \alpha )\\ 0\end{array}\right)\\ & =\left(\begin{array}{c}{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{array}\right)\end{array}\text{\hspace{1em}(5)}$$

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

$$\begin{array}{rl}Q(\alpha ,\theta )& =[R_{\theta }\cdot (t_x^{\Pi }{t}_y^{\Pi }{t}_z^{\Pi }){]}^T\\ & =[\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos \theta & -\sin \theta \\ 0& \sin \theta & \cos \theta \end{array}\right)\cdot \left(\begin{array}{ccc}-\cos \alpha & \sin \alpha & 0\\ -\sin \alpha & -\cos \alpha & 0\\ 0& 0& 1\end{array}\right){]}^T\\ & ={\left(\begin{array}{ccc}-\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{array}\right)}^T\\ & =\left(\begin{array}{ccc}-\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{array}\right)\end{array}\text{\hspace{1em}(6)}$$

將點$P_i$由新世界座標系$\eta$轉到新相機座標系$\tau$：$P_i^{\eta }=Q(\alpha ,\theta )\cdot (P_i-C^{\eta }(\alpha ,\theta ))$。

求解$\alpha$及$\theta$

到目前為止未知的數有$\alpha$及$\theta$，這裡利用$P_3^{\tau }$來列出關於$\cos \theta$的四次方程並求解。

因為$P_3^{\eta }$在$\eta$的xy平面上，所以可以將$P_3^{\eta }$表示為$(p_1,p_2,0)$。

欲求$P_3^{\tau }$，套用上面由新世界座標系的$P_3^{\eta }$到新相機座標系的公式：

$$\begin{array}{rl}{P}_3^{\tau }& =Q(\alpha ,\theta )\cdot (P_3^{\eta }-C^{\eta }(\alpha ,\theta ))\\ & =\left(\begin{array}{ccc}-\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{array}\right)\cdot (\left(\begin{array}{c}{p}_1\\ p_2\\ 0\end{array}\right)-\left(\begin{array}{c}{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{array}\right))\\ & =\left(\begin{array}{c}-\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{array}\right)\\ & =\left(\begin{array}{c}-\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{array}\right)\end{array}\text{\hspace{1em}(7)}$$

相機座標系下$P_3^{\tau }$經過縮放變成$f_3^{\tau }$($P_3^{\tau }$與$f_3^{\tau }$的方向需相同)。

首先定義兩個常數：

$${\varphi }_1=\frac{f_{3,x}^{\tau }}{f_{3,z}^{\tau }}\text{ and }{\varphi }_2=\frac{f_{3,y}^{\tau }}{f_{3,z}^{\tau }}\text{\hspace{1em}(8)}$$

並有以下關係：

$$\begin{array}{rl}& \{\begin{array}{l}{\varphi }_1=\frac{P_{3,x}^{\tau }}{P_{3,z}^{\tau }}\\ {\varphi }_2=\frac{P_{3,y}^{\tau }}{P_{3,z}^{\tau }}\end{array}\\ & \iff \{\begin{array}{l}{\varphi }_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}\\ {\varphi }_2=\frac{\sin \alpha \cdot p_1-\cos \alpha \cos \theta \cdot p_2}{-\sin \theta \cdot p_2}\end{array}\\ & \iff \{\begin{array}{l}\frac{\sin \theta }{\sin \alpha }{p}_2=\frac{-\cot \alpha \cdot p_1-\cos \theta \cdot p_2+d_{12}(b+\cot \alpha )}{-{\varphi }_1}\\ \frac{\sin \theta }{\sin \alpha }{p}_2=\frac{p_1-\cot \alpha \cos \theta \cdot p_2}{-{\varphi }_2}\end{array}\\ & \Rightarrow \{\begin{array}{l}\frac{\sin \theta }{\sin \alpha }{p}_2=\frac{-\cot \alpha \cdot p_1-\cos \theta \cdot p_2+d_{12}(b+\cot \alpha )}{-{\varphi }_1}\\ \frac{\sin \theta }{\sin \alpha }{p}_2=\frac{p_1-\cot \alpha \cos \theta \cdot p_2}{-{\varphi }_2}\end{array}\\ & \Rightarrow {\varphi }_2(\cot \alpha +\cos \theta \cdot p_2-d_{12}b-d_{12}\cot \alpha )={\varphi }_1(-p_1+\cot \alpha \cos \theta \cdot p_2)\\ & \Rightarrow ({\varphi }_2(p_1-d_{12})-\cos \theta p_2{\varphi }_1)\cot \alpha =-p_1{\varphi }_1-{\varphi }_2(\cos \theta \cdot p_2-d_{12}b))\\ & \Rightarrow \cot \alpha =\frac{p_1{\varphi }_1+{\varphi }_2(\cos \theta \cdot p_2-d_{12}b))}{\cos \theta p_2{\varphi }_1-({\varphi }_2(p_1-d_{12})}=\frac{\frac{{\varphi }_1}{{\varphi }_2}{p}_1+\cos \theta \cdot p_2-d_{12}\cdot b}{\frac{{\varphi }_1}{{\varphi }_2}\cos \theta \cdot p_2-p_1+d_{12}}\end{array}\text{\hspace{1em}(9)}$$

到了這裡我們得到了以已知數${\varphi }_1$,${\varphi }_2$及未知數$\cos \theta$表達的$\cot \alpha$。

$$\begin{array}{rl}{\varphi }_2=\frac{P_{3,y}^{\tau }}{P_{3,z}^{\tau }}& \iff {\varphi }_2=\frac{\sin \alpha \cdot p_1-\cos \alpha \cos \theta \cdot p_2}{-\sin \theta \cdot p_2}\\ & \iff (-\sin \theta p_2{\varphi }_2{)}^2=(\sin \alpha \cdot p_1-\cos \alpha \cos \theta \cdot p_2{)}^2\\ & \iff {\sin }^2\theta p_2^2{\varphi }_2^2={\sin }^2\alpha (p_1-\cot \alpha \cos \theta \cdot p_2{)}^2\\ & \iff {\sin }^2\theta {\sec }^2\alpha p_2^2{\varphi }_2^2=(p_1-\cot \alpha \cos \theta \cdot p_2{)}^2\\ & \iff (1-{\cos }^2\theta )(1+{\cot }^2\alpha ){\varphi }_2^2{p}_2^2=p_1^2-2\cot \alpha \cos \theta \cdot p_1{p}_2+{\cot }^2\alpha {\cos }^2\theta \cdot p_2^2\\ & \iff (1-{\cos }^2\theta ){\varphi }_2^2{p}_2^2{\cot }^2\alpha +(1-{\cos }^2\theta ){\varphi }_2^2{p}_2^2=p_1^2-2\cot \alpha \cos \theta \cdot p_1{p}_2+{\cot }^2\alpha {\cos }^2\theta \cdot p_2^2\\ & \iff ((1-{\cos }^2\theta ){\varphi }_2^2{p}_2^2-{\cos }^2\theta \cdot p_2^2){\cot }^2\alpha +(1-{\cos }^2\theta ){\varphi }_2^2{p}_2^2=p_1^2-2\cot \alpha \cos \theta \cdot p_1{p}_2\end{array}\text{\hspace{1em}(10)}$$

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

$$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\text{\hspace{1em}(11)}$$

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

然後由$\cos \theta$及$\cot \alpha$計算出$\theta$及$\alpha$。

求解R和C

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

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

$$C=P_1+N^T\cdot C^{\eta }\text{\hspace{1em}(12)}$$

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

$$R=N^T\cdot Q^T\cdot T\text{\hspace{1em}(13)}$$

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

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