EPnP笔记

问题描述

已知n个空间点在世界坐标系下的坐标 $P_i^w$ 及其图像坐标系下的坐标 $u_i$ ，所要求解的问题是如何根据空间点和图像点之间的对应关系求解旋转矩阵R和平移向量t。

EPnP基本思路

-EPnP算法将世界坐标系中的3D坐标表示为一组虚拟的控制点的加权和。对于一般情形，EPnP算法要求控制点的数目为4，且这4个控制点不能共面。通过求解4个控制点在相机坐标系下的坐标，就可以进一步求解出相机位姿

求解虚拟控制点图像坐标

在世界坐标系中设定4个非共面虚拟控制点，齐次坐标记为 $\{C_i^w|i=1,2,3,4\}$

注意这里是在世界坐标系中设定控制点

四个点通常选为：
$C_1^w=[0,0,0,1]^T$$ $$C_2^w=[0,0,0,1]^T$$ $$C_3^w=[0,0,0,1]^T$$ $$C_4^w=[0,0,0,1]^T$

image
空间点的世界坐标和控制点有如下关系
$p_i^w=\sum^4_{j=1}\alpha_{ij}C_j^w,with\sum^4_{j=1}=\alpha_{ij}=1$
向量 $[\alpha_{i1},\alpha_{i2},\alpha_{i3},\alpha_{i4}]^T$ 可以看作空间点 $P_i^W$ 在以控制点 $\{C_i^w|i=1,2,3,4\}$ 为基的欧氏空间的坐标。
同样的由于线性关系在欧式变换下的不变形有：
$p_i^c=\sum^4_{j=1}\alpha_{ij}C_j^c$
空间点 $P_i$ 和图像点 $u_i$ 的投影成像关系为：
$\lambda_iu_i=K[R,t]P_i^w=KP^c_i=K\sum_{j=1}^4\alpha C_j^c$
设控制点相机坐标系下 $C_j^c=[x_j^c,y_j^c,z_j^c]^T$ ，则
$\lambda_i=\sum_{j=1}^4\alpha_{ij}z_j^c$
把上面两个式子结合可得：
$\sum_{j=1}^4\alpha_{ij}fx_j^c-\alpha_{ij}u_iz_j^c=0$
$\sum_{j=1}^4\alpha_{ij}fy_j^c-\alpha_{ij}v_iz_j^c=0$

令 $Z=[Z_1^{cT},Z_2^{cT},Z_4^{cT},Z_4^{cT}]^T$ , $Z^c_j=[fx_j^c,fy^c_j,z_j^c]^T,j=1,2,3,4$ ，所以Z是1*12的向量

n个空间点可以得到线性方程组:
$MX=0$
其中 $X=[c_1^{cT},C_2^{cT},C_3^{cT},C_4^{cT}]^T$ ,每个元素是控制点在摄像头坐标系下的坐标。
$X=\sum^N_{i=1}\beta_iv_i$
可以计算 $M^TM$ 的特征向量得到 $v_i$ ，但还需要求出 $\beta_i$
对于第i个控制点：
$c_i^c=\sum^N_{k=1}\beta_kv_k^{[i]}$
$v_k^{[i]}$ 是特征向量 $v_k$ 的第i个3×1 sub-vector

高斯牛顿优化：
摄像头的外参描述的只是坐标变换，不会改变控制点之间的距离
$||\sum^N_{k=1}\beta_kv_k^{[i]}-\sum^N_{k=1}\beta_kv_k^{[j]}||^2=||c_i^w-c^w_j||^2$
优化目标函数：
$Error(\beta)=\sum_{(i,j)s.t.i<j}(||\sum^N_{k=1}\beta_kv_k^{[i]}-\sum^N_{k=1}\beta_kv_k^{[j]}||^2-||c_i^w-c^w_j||^2)^2$

计算相机位姿

控制点在相机坐标系下的坐标
$c_i^c=\sum^N_{k=1}\beta_kv_k^{[i]},i=1,...,4$
计算3D参考点在摄像头参考坐标系下的坐标：
$p_i^c=\sum^4_{j=1}\alpha_{ij}C_j^c,i=1,...,n$
计算 $\{P_i^w\}_{i=1,...,n}$ 的重心 $P^w_0$ 和矩阵A:
$A= \left[ \begin{matrix} p_1^{w^T}-p_0^{w^T}\\ ....\\ p_n^{w^T}-p_0^{w^T} \end{matrix} \right]$
计算 $\{P_i^c\}_{i=1,...,n}$ 的重心 $P^c_0$ 和矩阵B:
$B= \left[ \begin{matrix} p_1^{c^T}-p_0^{c^T}\\ ....\\ p_n^{c^T}-p_0^{c^T} \end{matrix} \right]$
计算H：
$H=B^TA$
计算H的SVD分解：
$H=U\Sigma V^T$
计算旋转R
$R=UV^T$
如果 $|R|<0$ ,那么 $R(2,:)=-R(2,:)$
计算平移t：
$t=p^c_0-Rp^w_0$