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 个点可以求解。
