EPnP

针对以上链接，提取个人可以看懂的部分

PnP问题的定义

Perspective-n-Point问题(PnP)的已知条件：

n个世界坐标系中的3D参考点(3D reference points)坐标;

与这n个3D点对应的、投影在图像上的2D参考点(2D reference points)坐标;

摄像头的内参K;
求解PnP问题可以得到摄像头的位姿。

大多数非迭代的PnP算法会首先求解特征点的深度，以获得特征点在相机坐标系中的3D坐标，而EPnP算法将世界坐标系中的3D坐标表示为一组虚拟的控制点的加权和。对于一般情形，EPnP算法要求控制点的数目为4，且这4个控制点不能共面。因为摄像头的外参未知，这四个控制点在摄像头参考坐标系下的坐标是未知的。而如果能求解出这四个控制点在摄像头参考坐标系下的坐标，我们就可以计算出摄像头的位姿。

Control Points & Barycentric Coordinates

控制点和重心坐标

4个控制点在世界坐标系中的坐标为 $c_j^w , j = 1 , … , 4$

在摄像头参考坐标系中的坐标为 $c_j^c , j = 1 ,…, 4 $

需要指出，在EPnP论文和本文中，$ p_i^w , c_j^w , p_i^c 和c_j^c $ 均非齐次坐标。

EPnP算法将参考点的坐标表示为控制点坐标的加权和：
$$
p_i^w= \sum_{j=1}^4 α_{ij}c_{jw},\qquad with \sum_{j=1}^4 α_{ij}=1
$$
其中 $α_i^j$ 是齐次barycentric坐标。

进一步，
$$
p_i^c = \sum_{j=1}^4 α_{ij}c_{jc}
$$
在上述推导过程中，用到了EPnP对权重 $α_i^j$ 的重要约束条件 $\sum_{j=1}^4 α_{ij}=1$ 。如果没有这个约束条件，上述推导将不成立。

本质上就是：3D参考点的齐次坐标是控制点齐次坐标的线性组合。

从上述分析过程中，我们也可以得到barycentric coodinates的计算方法：
$$
\begin{bmatrix}
α_{i1} \ α_{i2} \ α_{i3} \ α_{i4}
\end{bmatrix}
=C^{-1}
\begin{bmatrix}
P_i^w \ 1
\end{bmatrix}
$$

control points的选择

原则上，只要控制点满足 C 可逆就可以，但是论文中给出了一个具体的控制点确定方法。

3D参考点集为 $ {p_i^w , i = 1 , ⋯ ,n }$ , 选择3D参考点的重心为第一个控制点：
$$
c_1^w = \frac 1n \sum_{i=1} ^ n p_i^w
$$
进而得到矩阵：
$$
A =
\begin{bmatrix}
p_1^{w^T} - c_1 ^{w^T} \
…\
p_n^{w^T} - c_1 ^{w^T}
\end{bmatrix}
$$
记 $A^T A$ 的特征值为 $λ_{c,i} , i = 1 , 2 , 3$ , 对应的特征向量为 $v_{c , i} , i = 1 , 2 , 3 $ , 那么剩余的三个控制点可以按照下面的公式来确定：

$$
c_j^w = c_1^w + λ_{c,j-1}^{\frac12} v_{c,j-1} ,\qquad j=2,3,4
$$

求解控制点在摄像机坐标下的坐标

设K是摄像头的内参矩阵，可以通过标定获得。

$ { u_i }i=1,⋯,n 是参考点 {p_i}{i=1,2…,n}$ 是的2D投影，那么
$$
∀i, w_i
\begin{bmatrix}
u_i \ 1
\end{bmatrix}
=Kp i ^c =K \sum{j=1}^4 α _{ij} c _j^c
$$
把所有n个点串联起来，我们可以得到一个线性方程组：
$$
Mx=0
$$
其中 $x = [c_1^{c^T},c_2^{c^T}, c_3^{c^T},c_4^{c^T}]^T$ , x就是控制点在摄像头坐标系下的坐标，显然这是一个12 × 1 的向量，且 x 在M 的右零空间中。