EPnP算法

EPnP算法

相机坐标系用\(F^c\)，世界坐标系用\(F^w\)表示，任何一点可以用四个控制点\(p_i^w\)表示
\begin{equation}
p_i^w=\sum_{j=1}^4\alpha_{ij}c_j^w, \quad with \sum_{j=1}^4\alpha_{ij}=1
\end{equation}
对于相机坐标系同样成立
\begin{equation}
p_i^c=\sum_{j=1}^4\alpha_{ij}c_j^c, \quad with \sum_{j=1}^4\alpha_{ij}=1
\end{equation}
对于上面的公式，首先需要说明的是\(\alpha_{ij}\)确实存在。因为\(c_j^w\)或\(c_j^c\)构成的方程组是欠定的，所以一定存在解。
理论上来说，控制点可以随便选择，这里选择控制点为参考点的中心，其他的点在PCA得到的主轴上单位长度处，从而提高算法的稳定性。

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

根据投影方程得到世界坐标系中参考点坐标和相机坐标系中参考点的约束关系：
\begin{equation}
\forall i, \quad \omega_i
\left[
\begin{array}{c}
\mathbf{u_i} \
1
\end{array}
\right]
=Ap_i^c=A\sum_{j=1}^4\alpha_{ij}c_j^c
\end{equation}
写成矩阵的形式为：
\begin{equation}
\forall i,\quad \omega_i
\left[
\begin{array}{c}
u_i \
v_i \
1
\end{array}
\right]
=\left[
\begin{array}{ccc}
f_u & 0 & u_c\
0 & f_v & v_c\
0 & 0 & 1
\end{array}
\right]
\sum_{j=1}^4\alpha_{ij}
\left[
\begin{array}{c}
x_j^c\
y_j^c\
z_j^c
\end{array}
\right]
\end{equation}
将等式的第三列代入第一二列，得到
\begin{equation}
\sum_{j=1}^4\alpha_{ij}f_ux^c_j+\alpha_{ij}(u_c-u_i)z^c_j=0
\end{equation}
\begin{equation}
\sum_{j=1}^4\alpha_{ij}f_v y^c_j+\alpha_{ij}(v_c-v_i)z^c_j=0
\end{equation}
因此，可以得到下面的线性方程组：
\begin{equation}
\rm{Mx= 0}, \quad with\quad \rm{x=[c_1^{cT},c_2^{cT},c_3^{cT},c_4^{cT}]^T}
\end{equation}
上面的方程中，四个控制点总共12个未知变量，\(\rm{M}\)为\(2n\times 12\)的矩阵。因此，\(\rm{x}\)属于\(\rm{M}\)的右零空间，\(\rm{v_i}\)为矩阵\(\rm{M}\)的右奇异向量，可以通过求解\(\rm{M^TM}\)的零空间特征值得到。
\begin{equation}
\rm{x}=\sum _{i=1}^{N}\beta_i \rm{v}_i
\end{equation}
[说明]使用\(\rm{M^TM}\)比使用\(\rm{M}\)计算量更少，因为\(\rm{M^TM}\)是求解是常数复杂度，而\(\rm{M}\)是\(O(n^3)\)的复杂度，但是计算\(\rm{M^TM}\)的复杂度是\(O(n)\)的。

选择合适的线性组合

上面求解的\(\rm{x}\)中，需要确定\(\beta_i\)，也就是确定合适的线性组合。根据参考点的位置不同，矩阵\(\rm{M^TM}\)的零空间维数可能为\(N=1\rightarrow4\)维。求解\(\beta\)的策略是控制点在坐标系\(\mathcal{F}^w\)和\(\mathcal{F}^c\)中，两两之间的距离是相同，而\(\rm{x}\)的\(3k+1-3k\)分量表示分别表示不同的控制点在相机坐标系中的坐标，总共有\(C_4^2=6\)个约束。
如果\(N=1\)，则根据约束有
\begin{equation}
\Vert \beta \rm{v}^{[i]}-\beta \rm{v}^{[j]}\Vert^2=\Vert c_i^w - c_j^w\Vert^2
\end{equation}
所以
\[\beta=\frac{\sum_{[i,j]\in [1;4]}\Vert \rm{v}^{[i]} - \rm{v}^{[j]}\Vert\cdot \Vert \rm{c}^w_i - \rm{c}^w_j\Vert}{\sum_{[i,j]\in [1;4]}\Vert \rm{v}^{[i]} - \rm{v}^{[j]}\Vert^2}\]
如果\(N=2\)，
\begin{equation}
\Vert \beta_1 \rm{v}^{[i]}_1 + \beta_2 \rm{v}^{[i]}_2-(\beta_1 \rm{v}^{[j]}_1+\beta_2\rm{v}^{[j]}_2)\Vert^2=\Vert c_i^w - c_j^w\Vert^2
\end{equation}
由于\(\beta_1\)和\(\beta_2\)只以二次项出现在方程中，记\(\rm{\beta}=[\beta_1^2,\beta_1\beta_2, \beta_2^2]^T\)， \(\rho\)的每一项为\(\Vert c_i^w - c_j^w\Vert^2\)，得到相面的方程
\begin{equation}
L\beta = \rho
\end{equation}
其中\(L\)是由\(\rm{v}_1\)和\(\rm{v}_2\)构成的\(6\times 3\)的矩阵。
上面的方程可以通过\(\beta = (L^TL)^{-1}L^T\rho\)得到，然后通过选择合适的符号从\(\beta_1^2,\beta_1\beta_2,\beta_2^2\)使得所有的\(p_i^c\)有正的\(z\)坐标。

如果\(N=3\)则和\(N=2\)差不多，唯一的区别在于使用的是\(L\)的逆，而不是伪逆，此时的\(L\)为\(6\times 6\)的矩阵。

G-N优化

前面的步骤可以得到目标点在相机坐标系中的闭式解，作为G-N优化的初始值，优化的变量为\(\beta=[\beta_1, \cdots,\beta_N]^T\)，目标函数为
\begin{equation}
Error(\beta)=\sum_{(i,j) s.t. i<j}(\Vert c_i^c - c_j^c \Vert ^2-\Vert c_i^w - c_j^w\Vert ^2)
\end{equation}
该优化过程和参考点的数目无关，优化步骤和时间是常数。

计算R，t

前面的两步计算不同维数的零空间的误差，选择误差最小维数对应的\(\beta\)，从而得到\(\rm{x}\)，恢复出控制点在相机坐标系中的坐标并根据质心坐标系数得到参考点在相机坐标系的坐标。剩下的工作就是已知一组点云在两个坐标系中的坐标，求两个坐标系的位姿变换。
步骤如下：
（1）求中心点，\(p_c^c=\frac{\sum{p_c^i}}{N}\)，\(p_w^c=\frac{\sum{p_w^i}}{N}\)；
（2）去中心，\(q_c^i=p_c^i- p_c^c,q_w^i=p_w^i-p_w^c\)；
（3）计算\(H\)矩阵，\(H = \sum_{i=1}^{N}q_c^i q_w^{iT}\)
（4）对\(H\)进行SVD，\(H=U \Lambda V^T\)；
（5）计算\(X=VU^T\)，如果\(det(x)=1\)，则\(R=X\)，\(t=P_c^c- RP_w^c\)。否则\(R(2,\cdot)=-R(2,\cdot)\)