（六）PNP

PNP算法

　　在视觉SLAM中，我们通常会有不同的观测数据。

　　比如单目相机中，我们获取的两帧图像通常只有匹配的像素坐标，这时我们就只能利用 对极几何来优化相对位姿，如果我们的特征点分布大体上接近一个平面，那么我们还可以估算两帧间的 单应矩阵；

　　如果我们有RGB-D相机，我们还可以获得匹配像素对应的深度信息，那么我们就得到了两组三维匹配点，这时就可以通过 ICP(迭代最近点)来进行运动估计，ICP又分利用SVD分解和Bundle Adjustment两种方式，但是本质上并无区别，毕竟SVD也是构建非线性最小二乘问题。不过，Bundle Adjustment问题可以同时优化点和位姿。

　　如果我们有一组三维点匹配二维点，这种类型的数据获取方式有多种。典型的是可以通过RBG-D来获取，前面我们提到RGB-D采集的数据可以用ICP进行优化位姿，实际上由于RGB-D相机本身的噪声，深度值估计是有误差的。因此在SLAM中通常会用PNP的方法来做估计，PNP也就是我们本讲要介绍的内容，利用一个带噪声的深度值进行位姿估计，总比用两个都带噪声的深度值来得合适些。另一种渠道是，单目三角化得到的地图点重投影到新帧中构建的PNP问题，原理一致。此外，还有双目图像。

　　由于历史上的大牛众多，提供了多种千奇百怪的PNP算法，因此篇幅会有点长。笔者这也算是呕心沥血之作了，先把几个方法写出来先，然后再进入主题吧：

　　1. 直接线性变换(DLT)；

　　2. P3P;

　　3. EPNP;

　　4. UPNP；

　　5. 光束平差法(Bundle Adjustment)。

已知条件

　　匹配的 $n$ 组三维点-二维点：

$$P^{r} = \{P_{1}^{r}, P_{2}^{r}, \dots, P_{n}^{r}\}, p^{c} = \{p_{1}^{c}, p_{2}^{c}, \dots, p_{n}^{c}\}$$

其中，$P^{r}$ 表示参考帧中的三维点，$p^{c}$ 表示当前帧匹配的二维点(像素坐标)。数据获取方式如前面介绍的。

问题

　　在已知条件下，求解参考帧到当前帧的相对位姿。(旋转矩阵$R$ 和位移向量$t$)

方法一：直接线性变换(DLT)

　　直接线性变换与我们前面介绍的对极几何，单应矩阵的计算是类似的，都是忽略相对位姿本身的性质，直接将其视为一个数值矩阵，优化完才利用某些约束条件恢复相对运动。

　　假设我们有一对匹配对：$P_{i}^{r} = [X, Y, Z, 1]^{T}$ 和 $p_{i}^{c} = [u, v, 1]^{T}$，两个坐标均为归一化坐标。假设相对位姿为：$T = [R | t]$，$p_{i}^{r}$ 的深度值为 $s$，则我们有：

$$ s \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \begin{bmatrix} t_{1} & t_{2} & t_{3} & t_{4} \\ t_{5} & t_{6} & t_{7} & t_{8} \\ t_{9} & t_{10} & t_{11} & t_{12} \end{bmatrix} \begin{bmatrix} X \\ Y \\ Z \\ 1 \end{bmatrix} $$

通过令 $T = \begin{bmatrix} t_{1}^{T} \\ t_{2}^{T} \\ t_{3}^{T} \end{bmatrix}$，则上式可以变成：

$$ s\begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \begin{bmatrix} t_{1}^{T} \\ t_{2}^{T} \\ t_{3}^{T} \end{bmatrix} \begin{bmatrix} X \\ Y \\ Z \\ 1 \end{bmatrix} = \begin{bmatrix} t_{1}^{T} \\ t_{2}^{T} \\ t_{3}^{T} \end{bmatrix} P_{i}^{c} $$

可以得到：

$$ \begin{aligned} u = \frac{t_{1}^{T}P_{i}^{c}}{t_{3}^{T}P_{i}^{c}} \\ v = \frac{t_{2}^{T}P_{i}^{c}}{t_{3}^{T}P_{i}^{c}} \end{aligned} $$

通过上述式子，我们去除了尺度因子的约束。进一步简化，我们得到两个约束条件：

$$ \begin{aligned} -t_{3}^{T} u P_{i}^{c} + t_{1}^{T}P_{i}^{c} = 0 \Rightarrow \begin{bmatrix}P_{i}^{c} & 0 & -uP_{i}^{c} \end{bmatrix} \begin{bmatrix} t_{1}^{T} \\ t_{2}^{T} \\ t_{3}^{T} \end{bmatrix} \\ -t_{3}^{T} v P_{i}^{c} + t_{2}^{T}P_{i}^{c} = 0 \Rightarrow \begin{bmatrix}0 & P_{i}^{c} & -vP_{i}^{c} \end{bmatrix} \begin{bmatrix} t_{1}^{T} \\ t_{2}^{T} \\ t_{3}^{T} \end{bmatrix} \\ \end{aligned} $$

由于相对位姿共有 $12$ 个未知数，因此至少需要六组匹配点，才能提供 $12$ 个约束条件用以解决这个问题。于是，我们构建了一个齐次方程：

$$AT = 0$$

矩阵 $A$ 是多组匹配点构成的约束条件，如果只有六组匹配点，则我们可以直接求得