【三维重建】ePnP

PnP问题应用与一下场景：
已知三维点和对应二维点以及相机相机内参数，可以获取相机外参。请添加图片描述
我们介绍其中的一种算法：ePnP

算法流程

1、ePnP算法首先在世界坐标系内寻找4个控制点，记作 $C_1^w,C_2^w,C_3^w,C_4^w$ ，使得：
对于世界坐标系内任意一点 $P_1^w,$ 存在对应的 $\alpha_i=[\alpha_{i1},\alpha_{i2},\alpha_{i3},\alpha_{i4}]^T$ ，满足：
$P_i^w = \sum_{j=1}^4\alpha_{ij}C_j^w, {\kern 20pt} with\sum_{j=1}^4\alpha_{ij}=1$
世界坐标系上的 $P^w$ 经过 $R, t$ 变换可以得到相机坐标系下的点 $P^c$ :
$P_i^c = RP_i^w+t=R\left(\sum_{j=1}^4\alpha_{ij}C_j^w\right) + t$
由于 $\sum_{j=1}^4\alpha_{ij}=1$ ,因此 $t=\sum_{j=1}^4\alpha_{ij}t$ 带入上式得 $C_j^w是世界坐标系下的控制点，C_j^c是世界坐标系下的控制点)$ :
$P_i^c = \sum_{j-1}^4\alpha_{ij}(RC_j^w + t)= \sum_{j-1}^4\alpha_{ij}C_j^c,{\color{red} \left(C_j^c = RC_j^w+t \right)}$

通过内参矩阵，建立相机坐标系下的点 $P^c$ 到像素坐标系下的点 $p$ 的映射：
$p_i = s_i\begin{bmatrix}u_i \\ v_i \\ 1 \end{bmatrix} = KP^c_i=\begin{bmatrix}f_u &0 & u_c \\0& f_v & v_c \\ 0&0&1 \end{bmatrix}\sum_{j=1}^4\alpha_{ij}\begin{bmatrix}C_{xj}^c \\C_{yj}^c \\ C_{zj}^c \end{bmatrix}$
将公式展开，可得：
$\sum_{j=1}^4\alpha_{ij}f_uC^c_{xj} + \alpha_{ij}(u_c-u_i)C_{zj}^c = 0 \\ \sum_{j=1}^4\alpha_{ij}f_vC^c_{yj} + \alpha_{ij}(v_c-v_i)C_{zj}^c = 0$
对于上述公式只有相机坐标系下的四个控制点 $C_j^c$ 未知，每个控制点有三个参数，因此一共有12个未知数。每对点能建立两个方程组，所以，至少要6对点才能进行求解。
$f_u \begin{bmatrix} \alpha_{i1} & \alpha_{i2} & \alpha_{i3} & \alpha_{i4} \end{bmatrix}\begin{bmatrix} C_{x1}^c \\ C_{x2}^c \\ C_{x3}^c \\ C_{x4}^c\end{bmatrix} + (u_c-u_i) \begin{bmatrix} \alpha_{i1} & \alpha_{i2} & \alpha_{i3} & \alpha_{i4} \end{bmatrix} \begin{bmatrix} C_{z1}^c \\ C_{z2}^c \\ C_{z3}^c \\ C_{z4}^c\end{bmatrix} =0 \\ f_v \begin{bmatrix} \alpha_{i1} & \alpha_{i2} & \alpha_{i3} & \alpha_{i4} \end{bmatrix}\begin{bmatrix} C_{y1}^c \\ C_{y2}^c \\ C_{y3}^c \\ C_{y4}^c\end{bmatrix} + (v_c-v_i) \begin{bmatrix} \alpha_{i1} & \alpha_{i2} & \alpha_{i3} & \alpha_{i4} \end{bmatrix} \begin{bmatrix} C_{z1}^c \\ C_{z2}^c \\ C_{z3}^c \\ C_{z4}^c\end{bmatrix} =0 \\\\\\ \begin{bmatrix} f_u\alpha_{i1} & f_u\alpha_{i2} & f_u\alpha_{i3} & f_u\alpha_{i4} &0&0&0&0&(u_c-u_i) \alpha_{i1} & (u_c-u_i) \alpha_{i2} & (u_c-u_i) \alpha_{i3} & (u_c-u_i) \alpha_{i4} \\0&0&0&0&f_v\alpha_{i1} & f_v\alpha_{i2} & f_v\alpha_{i3} & f_v\alpha_{i4}& (v_c-v_i) \alpha_{i1} & (v_c-v_i) \alpha_{i2} & (v_c-v_i) \alpha_{i3} & (v_c-v_i) \alpha_{i4} \end{bmatrix}\begin{bmatrix}C_{x1}^c \\ C_{x2}^c \\ C_{x3}^c \\ C_{x4}^c\\C_{y1}^c \\ C_{y2}^c \\ C_{y3}^c \\ C_{y4}^c \\ C_{z1}^c \\ C_{z2}^c \\ C_{z3}^c \\ C_{z4}^c\end{bmatrix} =0$
找到相机坐标系下的控制点后，就能够求得相机外参:
$\begin{aligned} &C_j^c = RC_j^w+t\\\\ &\begin{bmatrix}C_{xj}^c \\ C_{yj}^c \\ C_{zj}^c \end{bmatrix} = \begin{bmatrix} r_1 & r_2 & r_3 \\ r_4 & r_5 & r_6 \\r_7 & r_8 & r_9 \end{bmatrix}\begin{bmatrix}C_{xj}^w \\ C_{yj}^w \\ C_{zj}^w \end{bmatrix} + \begin{bmatrix} t_1 \\ t_2 \\ t_3\end{bmatrix} \\\\ &C_{xj}^c = r_1C_{xj}^w + r_2C_{yj}^w + r_3C_{zj}^w + t_1\\ &C_{yj}^c = r_4C_{xj}^w + r_5C_{yj}^w + r_6C_{zj}^w + t_2\\ &C_{zj}^c = r_7C_{xj}^w + r_8C_{yj}^w + r_9C_{zj}^w + t_3\\ \\\\ &\begin{bmatrix}C_{x1}^c \\ C_{y1}^c \\C_{z1}^c \\C_{x2}^c \\ C_{y2}^c \\C_{z2}^c \\C_{x3}^c \\ C_{y3}^c \\C_{z3}^c \\C_{x4}^c \\ C_{y4}^c \\C_{z4}^c\end{bmatrix} = \begin{bmatrix} C_{x1}^w & C_{y1}^w & C_{z1}^w&0&0&0&0&0&0&1&0&0 \\ 0&0&0&C_{x1}^w & C_{y1}^w & C_{z1}^w &0&0&0&0&1&0\\ 0&0&0&0&0&0& C_{x1}^w & C_{y1}^w & C_{z1}^w &0&0&1\\ C_{x2}^w & C_{y2}^w & C_{z2}^w&0&0&0&0&0&0&1&0&0 \\ 0&0&0&C_{x2}^w & C_{y2}^w & C_{z2}^w &0&0&0&0&1&0\\ 0&0&0&0&0&0& C_{x2}^w & C_{y2}^w & C_{z2}^w &0&0&1 \\ C_{x3}^w & C_{y3}^w & C_{z3}^w&0&0&0&0&0&0&1&0&0 \\ 0&0&0&C_{x3}^w & C_{y3}^w & C_{z3}^w &0&0&0&0&1&0\\ 0&0&0&0&0&0& C_{x3}^w & C_{y3}^w & C_{z3}^w &0&0&1\\ C_{x4}^w & C_{y4}^w & C_{z4}^w&0&0&0&0&0&0&1&0&0 \\ 0&0&0&C_{x4}^w & C_{y4}^w & C_{z4}^w &0&0&0&0&1&0\\ 0&0&0&0&0&0& C_{x4}^w & C_{y4}^w & C_{z4}^w &0&0&1 \end{bmatrix} \begin{bmatrix}r_1 \\ r_2 \\ r_3 \\ r_4 \\ r_5 \\ r_6 \\ r_7 \\ r_8 \\ r_9 \\t_1\\t_2\\t_3 \end{bmatrix} \end{aligned}$

控制点选取

原则上，就是只要选择3个线性无关的点，就可以表示任意一个三维点，但由于方程组是4，如何是3个控制点只能求得最小二乘解。论文中给出了具体的选择方法。 3D参考点集为 $\left\{P^w_i,i=1,\cdots,n \right \}$ , 选择3D点的中心为第一个控制点:
$C^w_1=\frac{1}{n}\sum_{i=1}^nP^w_i$
进而得到矩阵:
$A=\begin{bmatrix}{(P_{1}^{w})}^{T} - {(C_{1}^{w})}^{T} \\ \cdots \\ {(P_{n}^{w})}^{T} - {(C_{n}^{w})}^{T} \end{bmatrix}$
记 $A^TA$ 的特征值为 $\lambda_i$ , 特征向量为 $V_i$ ,那么剩下的三个点为：
$C^w = C_1^w +\lambda^{\frac{1}{2}}_iV_i,\ i = 1,2,3$

求解 $\alpha$

联立方程组:
$\begin{aligned} &P_i^w = \alpha_{i1}C_1^w+\alpha_{i2}C_2^w+\alpha_{i3}C_3^w+\alpha_{i4}C_4^w \\\\ &\begin{bmatrix} x_i \\y_i\\z_i\end{bmatrix} = \alpha_{i1}\begin{bmatrix} C_{x1}^w \\C_{y1}^w\\C_{z1}^w\end{bmatrix}+\alpha_{i2}\begin{bmatrix} C_{x2}^w \\C_{y2}^w\\C_{z2}^w\end{bmatrix}+\alpha_{i3}\begin{bmatrix} C_{x3}^w \\C_{y3}^w\\C_{z3}^w\end{bmatrix}+\alpha_{i4}\begin{bmatrix} C_{x4}^w \\C_{y4}^w\\C_{z4}^w\end{bmatrix} \end{aligned}$
每个点可以得到4个方程组:
$x_i=\alpha_{i1}C_{x1}^w+\alpha_{i2}C_{x2}^w+\alpha_{i3}C_{x3}^w+\alpha_{i4}C_{x4}^w\\ y_i=\alpha_{i1}C_{y1}^w+\alpha_{i2}C_{y2}^w+\alpha_{i3}C_{y3}^w+\alpha_{i4}C_{y4}^w\\ z_i=\alpha_{i1}C_{z1}^w+\alpha_{i2}C_{z2}^w+\alpha_{i3}C_{z3}^w+\alpha_{i4}C_{z4}^w\\ 1= \alpha_{i1}+\alpha_{i2}+\alpha_{i3}+\alpha_{i4}$
用矩阵的方式可表示为：
$\begin{bmatrix} x_i \\ y_i \\ z_i \\ 1 \end{bmatrix} = \begin{bmatrix} C_{x1}^w &C_{x2}^w &C_{x3}^w &C_{x4}^w \\ C_{y1}^w &C_{y2}^w &C_{y3}^w &C_{y4}^w \\ C_{z1}^w &C_{z2}^w &C_{z3}^w &C_{z4}^w \\ 1&1&1&1 \end{bmatrix} \begin{bmatrix} \alpha_{i1} \\ \alpha_{i2} \\ \alpha_{i3} \\ \alpha_{i4}\end{bmatrix} \longrightarrow \begin{bmatrix} C_{x1}^w &C_{x2}^w &C_{x3}^w &C_{x4}^w \\ C_{y1}^w &C_{y2}^w &C_{y3}^w &C_{y4}^w \\ C_{z1}^w &C_{z2}^w &C_{z3}^w &C_{z4}^w \\ 1&1&1&1 \end{bmatrix} ^{-1}\begin{bmatrix} x_i \\ y_i \\ z_i \\ 1 \end{bmatrix} = \begin{bmatrix} \alpha_{i1} \\ \alpha_{i2} \\ \alpha_{i3} \\ \alpha_{i4}\end{bmatrix}$