PnP 問題即 Perspective-n-Point 。
有 P3P 方法，使用三個點對就能求解。但是先按照熟悉的方法，寫一寫。最后寫 P3P 方法，P3P 方法還是比較晦澀的，不是無腦方法。
1. 八點法（使用正交補推導出線性系統）
我希望把 SLAM 問題中基礎理論都歸納到統一的框架下，但我總是糾結 PnP 問題中三維點坐標是齊次的還是非齊次的，這樣搞得很頭疼。
在我的框架下已經有 Homography 矩陣和 Fundamental 矩陣，我是按照 MVG 這書的 DLT 方法計算 Homography 矩陣的，而 Fundamental 矩陣在 MVG 書上的推導比較晦澀，難懂。我是按照多年前看 Daniel Cremers 教授課程的方法推導的，具體的方法寫在了以前的博客 【多視圖幾何】TUM 課程 第5章 雙視圖重建：線性方法 中。
現在我想把 PnP 問題歸類到這兩類方法中。
PnP 問題是求解相機 \(\mathbf{R}\) 和 \(\mathbf{t}\) 的過程。它的輸入是世界坐標系下的 3D 點 \(\mathbf{X}\)（非齊次坐標 3 x 1） 與影像上的像素坐標 2D 點 \(\mathbf{x}\) （齊次坐標 3 x 1）。
按照剛體運動方程與投影方程，我們可以列出：
\[\begin{align} \mathbf{x} = \mathbf{K}(\mathbf{R}\mathbf{X} + \mathbf{t}) \end{align} \]
現在想辦法列出 \(\mathbf{X}\) 與 \(\mathbf{x}\) 之間的線性方程：
\[\begin{align} \lambda \mathbf{K}^{-1}\mathbf{x} = \mathbf{R}\mathbf{X} + \mathbf{t} \label{eq:basic_trans} \end{align} \]
\(\lambda\) 是空間點在相機空間直角坐標系下的深度。
（接下來的步驟與 Fundamental Matrix 的推導過程類似。）
消除 \(\mathbf{t}\)，只留下兩項：
\[\begin{align} \lambda\hat{\mathbf{t}} \mathbf{K}^{-1} \mathbf{x} = \hat{\mathbf{t}} \mathbf{R} \mathbf{X} \end{align} \]
等式兩側同時左乘以 \({(\mathbf{K}^{-1} \mathbf{x})}^T\)：
\[\begin{align} \lambda{(\mathbf{K}^{-1} \mathbf{x})}^T \hat{\mathbf{t}} \mathbf{K}^{-1} \mathbf{x} = {(\mathbf{K}^{-1} \mathbf{x})}^T \hat{\mathbf{t}} \mathbf{R} \mathbf{X} \end{align} \]
令 \(\mathbf{x}^{\prime} = \mathbf{K}^{-1} \mathbf{x}\) （即歸一化相機坐標系坐標）：
\[\begin{align} \lambda{\mathbf{x}^{\prime}}^T \hat{\mathbf{t}} \mathbf{x}^{\prime} = {\mathbf{x}^{\prime}}^T \hat{\mathbf{t}} \mathbf{R} \mathbf{X} \end{align} \]
等式左側等於 0，因為 \(\hat{\mathbf{t}} \mathbf{x}^{\prime}=\mathbf{t} \times \mathbf{x}^{\prime}\) 與 \(\mathbf{x}^{\prime}\) 垂直，兩者之間的內積為 0：
\[\begin{align} 0 = {\mathbf{x}^{\prime}}^T \hat{\mathbf{t}} \mathbf{R} \mathbf{X} \end{align} \]
接下來的求解步驟，可以參考 【多視圖幾何】TUM 課程 第5章 雙視圖重建：線性方法 。
\(\hat{\mathbf{t}} \mathbf{R}\) 是 3 x 3 的矩陣。這種方法需要 8 個點獲得 8 個方程進行求解，\(\hat{\mathbf{t}} \mathbf{R}\) 方程是認為除了尺度減少了 1 個自由度，有 8 個自由度。
2. 六點法（DLT 方法）
從公式 (\ref{eq:basic_trans}) 出發。
先將 \(\mathbf{X}\) 非齊次坐標轉換成齊次坐標，此時有
\[\begin{align} \lambda \mathbf{x}^{\prime} = \begin{bmatrix} \mathbf{R} & \mathbf{t} \end{bmatrix} \mathbf{X} \label{eq:dlt_start} \end{align} \]
其中 \(\mathbf{x}^{\prime} = \begin{bmatrix} u^{\prime} \\ v^{\prime} \\ 1 \end{bmatrix} = \mathbf{K}^{-1}\mathbf{x}\)。
令 \(\mathbf{P} = \begin{bmatrix} \mathbf{R} & \mathbf{t} \end{bmatrix}\)。\(\mathbf{P}\) 可以寫作 \(\begin{bmatrix} {\mathbf{p}^1}^T \\ {\mathbf{p}^2}^T \\ {\mathbf{p}^3}^T \end{bmatrix}\)，是一個 3 x 4 的矩陣。
(\ref{eq:dlt_start}) 兩側同時左乘以 \(\hat{\mathbf{x}^{\prime}}\) 於是有：
\[\begin{align} \mathbf{0} = \begin{bmatrix} 0 & -1 & v^{\prime} \\ 1 & 0 & -u^{\prime} \\ -v^{\prime} & u^{\prime} & 0 \end{bmatrix} \begin{bmatrix} {\mathbf{p}^1}^T\mathbf{X} \\ {\mathbf{p}^2}^T\mathbf{X} \\ {\mathbf{p}^3}^T\mathbf{X} \end{bmatrix} \end{align} \]
\[\begin{align} \mathbf{0} = \begin{bmatrix} v^{\prime}{\mathbf{p}^3}^T\mathbf{X} - {\mathbf{p}^2}^T\mathbf{X} \\ {\mathbf{p}^1}^T\mathbf{X} - u^{\prime}{\mathbf{p}^3}^T\mathbf{X} \\ u^{\prime}{\mathbf{p}^2}^T\mathbf{X} - v^{\prime}{\mathbf{p}^1}^T\mathbf{X} \end{bmatrix} \end{align} \]
\[\begin{align} \mathbf{0} = \begin{bmatrix} v^{\prime}\mathbf{X}^T{\mathbf{p}^3} - \mathbf{X}^T{\mathbf{p}^2} \\ \mathbf{X}^T{\mathbf{p}^1} - u^{\prime}\mathbf{X}^T{\mathbf{p}^3} \\ u^{\prime}\mathbf{X}^T{\mathbf{p}^2} - v^{\prime}\mathbf{X}^T{\mathbf{p}^1} \end{bmatrix} \end{align} \]
\[\begin{align} \mathbf{0} = \begin{bmatrix} \mathbf{0}^T & -\mathbf{X}^T & v^{\prime}\mathbf{X}^T \\ \mathbf{X}^T & \mathbf{0}^T & - u^{\prime}\mathbf{X}^T \\ - v^{\prime}\mathbf{X}^T & u^{\prime}\mathbf{X}^T & \mathbf{0}^T \end{bmatrix} \begin{bmatrix} \mathbf{p}^1 \\ \mathbf{p}^2 \\ \mathbf{p}^3 \end{bmatrix}\end{align} \]
每一個匹配點對可以列出 2 個方程，一共需要列出 3*4 = 12 個方程，6 個點可以求解。
