1.问题背景—— 什么是PnP问题？

PnP(Perspective-n-Point)是求解 3D 到 2D 点对运动的方法。它描述了当我们知道n 个 3D 空间点以及它们的投影位置时,如何估计相机所在的位姿。——《视觉SLAM十四讲》

通俗的讲，PnP问题就是在已知世界坐标系下N个空间点的真实坐标以及这些空间点在图像上的投影，如何计算相机所在的位姿。罗嗦一句：已知量是空间点的真实坐标和图像坐标，未知量（求解量）是相机的位姿。

PnP 问题有很多种求解方法，例如用三对点估计位姿的 P3P 、直接线性变换(DLT)、EPnP。此外,还能用非线性优化的方式,构建最小二乘问题并迭代求解,也就是万金油式的 Bundle Adjustment。下面介绍逐一介绍。

2.PnP问题的求解方法

由于历史上的大牛众多，提供了多种千奇百怪的PNP算法，因此篇幅会有点长。下面是几种方案

直接线性变换（DLT）
P3P
EPNP
UPNP
光束平差法（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^r_i $ 的深度值为 $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$ 是多组匹配点构成的约束条件，如果只有六组匹配点，则我们可以直接求得满足要求的解。如果不止六组点，那么我们就需要构建一个非线性方程来求解。这与我们先前介绍的对极几何，单应矩阵等利用SVD求解的方法一致。通过SVD分解，我们得到了 12维的 $T$ 矩阵。

利用直接线性变换（DLT）估计的是一个射影变换，因为对于我们上述构建的问题，仅仅需要其满足一定的空间变换关系，而对于矩阵 $T$ 并没有更多的约束。实际上，由于 $T=[R|t]$，旋转矩阵需要满足一定的约束性质，因此需要进一步对解进行分析，比如QR分解,SVD分解，将结果从矩阵空间重新投影到 $SE(3)$ 流形上，转换成旋转和平移两部分。

通过SVD分解，我们得到了 $T$ 的数值解，这个解是不带尺度的。“旋转矩阵”为：

\[ \hat{R} = \begin{bmatrix} t_{1} & t_{2} & t_{3} \\ t_{5} & t_{6} & t_{7} \\ t_{9} & t_{10} & t_{11} \end{bmatrix} \]

同样，通过SVD分解：

\[ \begin{bmatrix} U & \Sigma & V \end{bmatrix} = SVD(\hat{R}) \]

于是我们可以得到旋转矩阵：

\[ \begin{aligned} R_{1} = UV^{T} \\ R_{2} = -UV^{T} \end{aligned} \]

理论上，$Σ$ 的对角线应该非常接近，取均值，则尺度因子 $β$ 可以由下式得到：

\[ \beta = \pm 1 / (tr(\Sigma) / 3) \]

于是，我们可以计算平移向量：

\[ \begin{aligned} t_{1} = \beta_{1} \begin{bmatrix} t_{4} & t_{8} & t_{12} \end{bmatrix} \\ t_{2} = \beta_{2} \begin{bmatrix} t_{4} & t_{8} & t_{12} \end{bmatrix} \end{aligned} \]

通过上面的计算，我们得到了四组解，一说到有四组解，我们是不是想起了对极几何的操作？没错，我们还是一样利用点必须位于相机前方，即深度值必须为正这么一个约束对四组解进行验证，找到正确的解。

方法二：P3P

接下来，要介绍另一种PNP方法：P3P，只用三组3D-2D匹配点加一组用于验证，对数据要求较少。

P3P的思路比较朴素，就是利用点的空间分布关系在不同相机坐标系下保持不变，以及相似三角形原理计算出参考帧三维点 $P^c_i$ 对应的参考帧三维点 $P^r_i$（实际上，参考帧只有二维点 $p^r_i$，因此才需要算三维点）。有了参考帧的三维空间点以及匹配的当前帧的三维空间点，我们可以构建一个类似于对极几何一般的约束问题，比如 $AE=b$，再分解出四组解，利用验证组选择正确的解。

首先引入一个图：

上述描绘的是空间点$A_w,B_w,C_w$在当前相机坐标系下的对应点，对应像素坐标分别为$u,v,w$。设当前相机光心为 $O$，则可以得到三组相似三角形：

\[ \Delta Ouv \sim \Delta OAB, \Delta Ouw \sim \Delta OAC, \Delta Ovw \sim \Delta OBC \]

利用余弦定理可以得到：

\[ \begin{aligned} OA^{2} + OB^{2} - 2OA\cdot OB \cdot cos<ou, ov> = AB^{2} \\ OA^{2} + OC^{2} - 2OA\cdot OC \cdot cos<ou, ow> = AC^{2} \\ OB^{2} + OC^{2} - 2OB\cdot OC \cdot cos<ov, ow> = BC^{2} \end{aligned} \]

对上述所有式子除以 $OC^2$并令$x=\frac{OA}{OC},y=\frac{OB}{OC}$，可以得到：

\[ \begin{aligned} x^{2} + y^{2} - 2x\cdot y \cdot cos<ou, ov> = \frac{AB^{2}}{OC^{2}} \\ x^{2} + 1 - 2x\cdot cos<ou, ow> = \frac{AC^{2}}{OC^{2}} \\ y^{2} + 1 - 2y\cdot cos<ov, ow> = \frac{BC^{2}}{OC^{2}} \end{aligned} \]

上述式子中，由于像素坐标已知，显然余弦值可求，而右边的式子中，$A,B,C$ 的空间关系也是已知的，但是我们不知道 $OA,OB,OC$ 是未知的，前两项是待求，最后一项我们需要将他消除，则我们通过令：$a = \frac{AB^{2}}{OC^{2}}, ab = \frac{AC^{2}}{OC^{2}}, ac = \frac{BC^{2}}{OC^{2}}$，可以得到 $b = \frac{AC^{2}}{AB^{2}}, c = \frac{BC^{2}}{AB^{2}}$，我们可以将第一个式子带入其余两个式子中，以此消去 $OA$，即：

\[ \begin{aligned} (1 - b)x^{2} -by^{2} + 1 - 2x\cdot cos<ou, ow> + 2b\cdot x\cdot y\cdot cos<ou, ov> = 0 \\ -cx^{2} + (1 - c)\cdot y^{2} + 1 - 2y\cdot cos<ov, ow> + 2b\cdot x\cdot y\cdot cos<ou, ov> = 0 \end{aligned} \]

最后我们得到了两个二元二次方程，该方程比较复杂，需要使用一些特殊的求解方法，视觉SLAM十四讲中推荐的是使用吴消元法。具体的操作，笔者搜索到一篇博文[相机位姿求解——P3P问题]提供了求解方法。得到 $x,y$ 后，我们将其代回原式子中，可以计算得到 $OA,OB,OC$。但我们需要求的是一个坐标点，而不单单是长度，因此还需要乘上一个单位方向向量。（笔者认为这个单位向量可以通过归一化平面的坐标与像素点坐标来计算）。

OK，通过上述方法，我们得到了三组点，于是我们利用类对极几何的方法计算出四组解，一组点可以提供三个约束条件，三组点刚好满足约束要求。于是我们利用第四组点进行验证，选择正确的解。至此，P3P的方法我们就介绍完毕了。

有一点值得注意的是，P3P算法只要求3组点用于计算，1组点用于验证，再多的数据也用不了。很显然，如果我们选择的数据对存在噪声太大甚至是错误的，都会直接影响到最终结果。因此，实际上P3P也不是很好嘛，计算还那么难。也提供一篇大家都在推荐的文章：Complete solution classification for the perspective-three-point problem。

方法三：EPNP

PnP是利用已知匹配点对以及相机内参来求解相机位姿的算法，而EPnP则是针对$n≥3$情况下相机位姿求解的$O(n)$时间的算法。它描述了当我们知道 n 个 3D 空间点以及它们的投影位置时，如何估计相机所在的位姿。

基本原理

上图表示场景中的平面π在两相机的成像，设平面π在第一个相机坐标系下的单位法向量为$N$，其到第一个相机中心（坐标原点）的距离为d，则平面π可以表示为：

其中，世界坐标系中的点pwi��可以表示为：

pwi=4∑j=1αijcwj, with4∑j=1αij=1

��=∑�=14��, ��ℎ∑�=14��=1

对于相机坐标系中的点pci��,有：

pci=4∑j=1αijccj, with4∑j=1αij=1

��=∑�=14��, ��ℎ∑�=14��=1

对于上面的公式来说，首先需要说明的是αij��确实存在，因为cwj��构成的方程组是非正定的，所以一定存在解。理论上来说，控制点可以随便选择，这里选择控制点为参考点的中心，其他的点在主方向的单位长度处，从而提高算法的稳定性。

控制点在相机坐标系下的坐标

首先需要求解4个控制点在世界坐标系下的坐标，按照上述说法，就是找到点云的重心和点云的三个主方向，可以参考主成分分析PCA。

根据投影方程得到世界坐标系中参考点坐标和相机坐标系中参考点的约束关系：

∀i,wi⎡⎢⎣uivi1⎤⎥⎦=Apci=A4∑j=1αijccj

∀�,��[��1]=��=�∑�=14��

写成矩阵的形式为：

∀i,wi⎡⎢⎣uivi1⎤⎥⎦=⎡⎢⎣fu0uc0fvvc001⎤⎥⎦4∑j=1αij⎡⎢
⎢⎣xcjycjzcj⎤⎥
⎥⎦

∀�,��[��1]=[��0��0��001]∑�=14��[��]

将等式拆解，从第三行得到：

wi=4∑j=1αijzcj

��=∑�=14��

将wi��代入一二行，可以得到如下等式：

∑4j=1αijfuxcj+αij(uc−ui)zcj=0∑4j=1αijfvycj+αij(vc−vi)zcj=0

∑�=14��+��(��−��)��=0∑�=14��+��(��−��)��=0

因此，可以得到如下线性方程组：

MX=0

��=0

上面的方程中，四个控制点总共12个未知变量，M�为2n×122�×12的矩阵。因此，x为矩阵M的右奇异向量，可以通过SVD得到。

x=N∑i=1βivi

�=∑�=1��

β�是分解得到的奇异值，个数为1-4个。

说明：使用MTM��比使用M计算量更小，因为MTM��的求解是常数复杂度，而M�求逆是O(n3)�(�3)的复杂度，但是计算MTM��的复杂度是O(n)�(�)的。

计算R和t

通过前面的就算可以求出控制点在相机坐标系下的位置，下面就需要恢复出参考点在相机坐标系中的坐标。剩下的工作就是已知一组点云在两个坐标系中的坐标，求两个坐标系的位姿变换。

首先计算质心坐标：

pcc=∑picN pcw=∑piwN

��=∑�� =∑��

然后计算去质心坐标：

qic=pic−pccqiw=piw−pcw

1.问题背景—— 什么是PnP问题 ？