EPnP算法概述-Charlie小站

论文原文：https://infoscience.epfl.ch/record/160138/files/top.pdf。

1. 参数化

已知空间中$n$个3D参考点$\{\mathbf{p}_i\}_{i \in [1,...,n]}$及其对应的图像点$\{\mathbf{m}_i\}_{i \in [1,...,n]}$。假设空间中有4个控制点$\{\mathbf{c}_i\}_{i \in [1,2,3,4]}$。于是每一个3D点都可以表示为控制点的凸组合，即
\begin{equation}
\mathbf{p}_i = \sum_{j=1}\alpha_{ij}, \text{with} \sum_{j=1}^4 = \alpha_{i,j}=1
\end{equation}
（$\alpha_{i,j}$称为质心坐标，其数值与参考系的平移和旋转无关）

为了区分，用上角标$w$和$c$代表世界坐标系和相机坐标系，如$\mathbf{p}_i^c$，$\mathbf{p}_i^w$。

控制点的选取：理论上控制点可以任意4个不共面的点。实践操作表明，其中一个点选为3D参考点的质心，另外三个点沿着点集的主方向选择，即要求这3个点与质心的连线分别与3个主方向平行。

2. 解空间

投影方程
\begin{equation}
\label{eq:projection}
w_i
\begin{bmatrix}
u_i \\ v_i \\ 1
\end{bmatrix}
=
\begin{bmatrix}
f_u &0 & c_u\\ 0 & f_v & c_v \\ 0&0&1
\end{bmatrix}
\sum_{j=1}^4\alpha_{ij}
\begin{bmatrix}
x_j^c \\ y_j^c \\ z_j^c
\end{bmatrix}
\end{equation}

方程$\eqref{eq:projection}$的最后一行说明$w_i = \sum_{j=1}^4\alpha_{ij}z_j^c$，将其代入前两行，可得到线性方程组
\begin{align}
\label{eq:linear-sys-a}
\sum_{j=1}^4\alpha_{ij}f_u x_j^c+\alpha_{ij}(u_c-u_i)z_j^c &=0 \\
\label{eq:linear-sys-b}
\sum_{j=1}^4\alpha_{ij}f_v y_j^c+\alpha_{ij}(v_c-v_i)z_j^c &=0
\end{align}
注意到$\eqref{eq:linear-sys-a}$和$\eqref{eq:linear-sys-b}$中消去了$w_i$项。

将$n$个点的方程组合在一起，可以得到一个具有如下形式的线性方程
\begin{equation}
\label{eq:linear}\mathbf{M}\mathbf{x} = 0
\end{equation}
其中$\mathbf{x} = [\mathbf{c}_1^{c\top},\mathbf{c}_2^{c\top},\mathbf{c}_3^{c\top},\mathbf{c}_4^{c\top}]$，是一个12维的未知变量，由4个控制点的坐标组成；$\mathbf{M}$是1个$2n\times 12$维的矩阵。显然，方程\eqref{eq:linear}的解空间对应$\mathbf{M}^{\top}\mathbf{M}$的零空间。

3. 定解

设$\{\mathbf{v}_i\}$为$\mathbf{M}$的右奇异向量，那么$\mathbf{x} = \sum_{i=1}^N\beta_i\mathbf{v}_i$。在理想透视投影模型下，$\mathbf{M}^{\top}\mathbf{M}$的零空间的维数严格等于1。若考虑仿射投影模型，维数等于4。由于大焦距的透视相机可以被一个仿射模型近似，因此$N$的取值具有不确定性，可能为1或者为4。而且由于实际中存在噪声，通常找不到严格为0的奇异值，但是可能会非常小。

基于上述原因，对$N\in\{1,2,3,4\}$的所有情况求解，保留误差者作为最终解。误差定义如下
\begin{equation}
e = \sum_i\mathrm{dist}\left(\mathbf{K}[\mathbf{R | \mathbf{t}}]\begin{bmatrix}\mathbf{P}_i^w\\1\end{bmatrix},\mathbf{u}_i\right)
\end{equation}
其中$\mathrm{dist}(\tilde{\mathbf{m}},\mathbf{n})$表示由齐次向量表示的点$\mathbf{m}$和点$\mathbf{n}$之间的距离。

以下分别对$N=1,2,3,4$几种情况讨论

$N=1$：此时$\mathbf{x} = \beta \mathbf{v}$，利用保距离约束计算$\beta$。令$\mathbf{v}^{[i]}$为$\mathbf{c}_i^c$在$\mathbf{v}$中对应的子向量。例如，$\mathbf{v}^{[1]}$表示$\mathbf{v}$中的前3个元素。保距离约束为
\begin{equation}
\|\beta \mathbf{v}^{[i]} - \beta \mathbf{v}^{[j]}\| = \|\mathbf{c}_i^w - \mathbf{c}_j^w\|
\end{equation}
其封闭解为
\begin{equation}
\beta = \frac{\sum_{\{i,j\}\in[1;4]}\|\mathbf{v}^{[i]} - \mathbf{v}^{[j]}\|\cdot \|\mathbf{c}_i^w - \mathbf{c}_j^w\|}{\sum_{\{i,j\}\in[1;4]}\|\mathbf{v}^{[i]} - \mathbf{v}^{[j]}\|^2}
\end{equation}
$N=2$：此时$\mathbf{x} = \beta_1 \mathbf{v}_1 + \beta_2 \mathbf{v}_2$。距离约束为
\begin{equation}
\label{eq:case2}
\|(\beta_1 \mathbf{v}_1^{[i]}+\beta_2 \mathbf{v}_2^{[i]}) - (\beta_1 \mathbf{v}_1^{[j]}+\beta_2 \mathbf{v}_2^{[j]})\| = \|\mathbf{c}_i^w - \mathbf{c}_j^w\|
\end{equation}
等式$\eqref{eq:case2}$可以整理成$A\beta_1^2+B\beta_1\beta_2+C\beta_2^2 = D$的形式。利用这一形式，令$\mathbf{b} = [\beta_1^2,\beta_1\beta_2,\beta_2^2]^\top$，可将方程$\eqref{eq:case2}$转化为线性形式
\begin{equation}
\mathbf{L}\mathbf{b} = \mathbf{q}
\end{equation}
求解得$\mathbf{b}$之后，可进一步求解$\beta_1$和$\beta_2$。这一方法称之为为线性化方法。需要注意的是，解出的$\beta_1$和$\beta_2$需要令所有的$\mathbf{p}_i^c$的$z$分量为正数。
$N=3$：按照$N=2$的方式如法炮制。区别在于，维数从4维上升到6维。
$N=4$：有4个未知数需要求解。理论上，由6个距离约束已经足够解出4个未知数。但是如果继续使用线性化方法，会将未知数个数提升到10个。这种情况下，先解出根据齐次线性方程，即
\begin{equation}
\mathbf{L}\tilde{\mathbf{b}} = 0
\end{equation}
解出$\mathbf{b}$所在的空间。注意到如下事实
\begin{equation}
\label{eq:permutation}
\beta_{ab}\beta_{cd} = \beta_a\beta_b\beta_c\beta_d = \beta_{a'}\beta_{b'}\beta_{c'}\beta_{d'}
\end{equation}
其中$\{a',b',c',d'\}$为$\{a,b,c,d\}$的任意置换。利用式\eqref{eq:permutation}的约束并再次使用线性化方法可解出$\mathbf{b}$。