EPnP论文笔记

EPnP简介
关于控制点
计算相机坐标系下的控制点（GN优化没写）
计算R,t
平面（未完成）
实验（未完成）

1. EPnP简介

1.1 EPnP主要思想

先说一下问题定义，已知 $n$ 对世界坐标系中3D坐标和其对应的图像投影点的2D坐标的参考点对(Reference points)。

EPnP方案是将参考点的相机坐标表示为控制点(control point)的加权和，继而将问题转化成了对这四个控制点的相机坐标系的求解。对于非平面的情况来说，需要4个非共面的控制点，而对于平面来说只需要3个。

1.2 EPnP算法流程

1.2.1 EPnP符号说明

上标为 $w$ 的点是在世界坐标系下
上标为 $c$ 的点是在相机坐标系下
所有的 $p^w$ 、 $p^c$ 、 $c^w$ 、 $c^c$ 均非齐次坐标。

1.2.2 算法流程

控制点的选择，Select four control points $c^w_j$ (PCA)
Compute the coefficient $\alpha$
Compute the $c^c_j$ in the camera coordinate
3.1 $Mx = 0$
3.2 $L\beta = \rho$
3.3 $Gauss-Newton$
Recover 3d points in the camera coordinates
Compute the R and t

2. 关于控制点

关于如何选择控制点的问题讲放在稍后讲，先假设我们已有选好的四个控制点。

2.1 Barycentric Coordinates重心坐标

Barycentric Coordinates

给定一个三角形 $\triangle ABC$ ，则在三角形所在的平面上的任意点 $P$ ，都表示为点 $A、B、C$ 的线性组合：
$P = \begin{bmatrix} w_A,w_B,w_C \end{bmatrix} \begin{bmatrix} A\\ B \\ C \end{bmatrix}$
以下是重心坐标的推导过程，跳过不看：

2.2 求解权重因子 $\alpha$

EPnP也是采用了这种类似的表示方法，用四个控制点的加权和(weighted sum)来表示参考点，在世界坐标系下，可得：
$p^w_i = \sum_{j = 1}^{4}\alpha_{ij} c^w_j , with \sum^{4}_{j=1} \alpha_{ij} = 1$
那么在相机坐标系下的参考点 $P^c_i$ ，可写为：
$p^c_i = \begin{bmatrix} R,t \end{bmatrix} \begin{bmatrix} p_i^w \\1 \end{bmatrix} = \begin{bmatrix} R,t \end{bmatrix} \begin{bmatrix} \sum_{j = 1}^{4}\alpha_{ij} c^w_j \\1 \end{bmatrix} = \begin{bmatrix} R,t \end{bmatrix} \begin{bmatrix} \sum_{j = 1}^{4}\alpha_{ij} c^w_j \\ \sum^{4}_{j=1} \alpha_{ij} \end{bmatrix} = \sum^{4}_{j=1} \alpha_{ij} \begin{bmatrix} R,t \end{bmatrix} \begin{bmatrix} c_{j}^{w} \\1 \end{bmatrix} = \sum^{4}_{j=1} \alpha_{ij} c^{c}_{j}$

从上面的推导过程中，可以观察到无论是在世界坐标系和相机坐标系，他们在新的参考坐标下共享同一套权重。

为什么需要4个控制点？
假设3个控制点满足要求，那么
$p_i^w = \begin{bmatrix} \alpha_{i1},\alpha_{i2},\alpha_{i2}\end{bmatrix} \begin{bmatrix} c^w_1 \\ c^w_2 \\ c^w_3 \end{bmatrix}, \sum^{3}_{j=1} \alpha_{ij} c^{w}_{j} = 1$
一共有3个未知数，4个方程式。这种方程式数量大于未知数个数的情况称之为超定方程。只存在最小二乘意义上的解。也就是，在一般情况下，不存在精确满足4个方程的解。
如何求解 $\alpha$ ?
$\begin{bmatrix} \alpha_{i1} \\ \alpha_{i2} \\ \alpha_{i3}\\ \alpha_{i4} \end{bmatrix} = C^{-1} \begin{bmatrix} p^w_i \\ 1 \end{bmatrix}$
$C = \begin{bmatrix} c^w_1&c^w_2 &c^w_3 &c^w_4 \\ 1 &1 &1 &1\end{bmatrix}$

2.3 控制点的选择

理论上来说，控制点的选择没有太多讲究，只要保证 $C$ 可逆就行了。但是，作者在实验中发现其中一个参考点设置为质心后，该算法的稳定性会上升。这在某种程度上是说的通的，因为质心的使用，使得数据在坐标系上得到归一化。
先计算第一个控制点 $c^w_1$ :
$c^w_1 = \frac{1}{n} \sum^{n}_{i=1} p^w_i$
其他的三个点通过PCA计算得出，先计算协方差矩阵：
$A = \begin{bmatrix} {p^w_1}^T - {c^w_1}^T \\ \vdots \\ {p^{w}_{n}}^T - {c^w_n}^T \end{bmatrix}$
则，协方差矩阵为 $A^TA$ 。
记 $A^TA$ 的特征值为 $\lambda_{c,i}, i = 1,2,3$ ，对应的特征向量为 $v_{c,i}, i= 1,2,3$ ，那么剩余的三个控制点可以按照下面的公式来确定：
$c^w_j = c^w_1 + \lambda^{\frac{1}{2}}_{c,j-1} v_{c,j-1}, j = 2,3,4$

3. 计算相机坐标系下的控制点

3.1 获得线性方程组

设已知的相机内参矩阵为 $K$ ， ${\{u_i\} }_{i=1,2, ... , n}$ 是 ${\{p_i \} }_{i=1,2, ... , n}$ 在图像坐标系上的2D投影，那么
$w_i \begin{bmatrix} u_i\\ v_i\\ 1\end{bmatrix} = K p_i^{c} = K \sum_{j=1}^{4} \alpha_{ij} c_{j}^{c} = \begin{bmatrix} f_u &0 & u_c\\ 0 &f_v & v_c\\ 0 &0 &1\end{bmatrix} \sum^{4}_{j=1} \alpha_{ij} \begin{bmatrix} x^{c}_{j}\\ y^{c}_{j}\\ z^{c}_{j}\end{bmatrix}$
$w_i = \sum_{j=1}^{4} \alpha_{ij}z^{c}_{j}$
由上可得，2个线性方程组：
$\left\{ \begin{aligned} \sum^{4}_{j=1} \alpha_{ij} x^{c}_{j} f_u + \sum^{4}_{j=1} \alpha_{ij} z^{c}_{j}(u-u_i) = 0 \\ \sum^{4}_{j=1} \alpha_{ij} y^{c}_{j} f_v + \sum^{4}_{j=1} \alpha_{ij} z^{c}_{j}(v-v_i) = 0 \end{aligned} \right.$
取 $2n$ 个式子的系数在一起的得到 $2n*12$ 的矩阵 $M$ ， $x$ 就是控制点在相机坐标系下的坐标。
$M = \begin{bmatrix} \alpha_{11} f_{u} & 0& \alpha_{11} (u_c - u_1) & \cdots & \alpha_{14} f_{u} & 0 & \alpha_{14} (u_c - u_4) \\ 0 & \alpha_{11} f_v & \alpha_{11}(v_c - v_1) & \cdots & 0 & \alpha_{14} f_v & \alpha_{14}(v_c - v_4) \\ \vdots & \vdots & \vdots & \ddots & \vdots & \vdots & \vdots \\ \alpha_{n1} f_{u} & 0& \alpha_{n1} (u_c - u_1) & \cdots & \alpha_{n4} f_{u} & 0 & \alpha_{n4} (u_c - u_4) \\ 0 & \alpha_{n1} f_v & \alpha_{n1}(v_c - v_1) & \cdots & 0 & \alpha_{n4} f_v & \alpha_{n4}(v_c - v_4) \\ \end{bmatrix}$
$x = {\begin{bmatrix} {c^{c}_{1}}^T & {c^{c}_{2}}^T & {c^{c}_{3}}^T &{c^{c}_{4}}^T \end{bmatrix}}^T$
整理后可得， $Mx = 0$ 。
显然 $x$ 落在 $M$ 的右零空间中，或者 $x \in ker(M)$ ，可以表示为：
$x = \sum_{i=1}^{N} \beta_{i} v_{i}$

3.2 求解 $v_i和\beta_i$

3.2.1 求解 $v_i$

$v_i是矩阵M的N个0奇异值(null\ singular\ value)对应的N个$ 右奇异向量(right-singular vector)，可以对M进行SVD求解得到 $v_i$ 。但是SVD耗时很长，目前时间复杂度最低的SVD解法是 $O(n^2)$ 。

不妨换个思路，可通过求解 $M^T M$ 的0特征值对应的特征向量更快求解 $v_i$ 。计算矩阵 $M^T M$ 是EPnP中最耗时的步骤，时间复杂度为 $O(12*2n*12) = O(n)$ 。

在相机坐标系下，对于第i个控制点 $c_i^c$
$c^c_i = \sum^{N}_{k=1} \beta_k v^{[i]}_k$

3.2.2 求解 $\beta_i$

接下来，就是要求解 $\{ \beta_i\}_{i=1,2,..,N}$ 。

根据 $N$ 的数量，对 $\beta_i$ 有不同的求解方式。 $N$ 是矩阵 $M^T M$ 的零空间 $(null\ space)$ 的维度。作者通过仿真实验，发现 $N$ 和相机焦距 $f$ 有关。

填坑
可以观察到， $M^T M$ 的奇异值从第5个开始，都趋近为0。因为实验过程中，存在一些噪音，所以不是严格意义上为0。

两张图的纵轴都代表的是基于300组实验的 $N=1,2,3,4$ 占比情况。
左图：在同一张图片上加高斯噪声 $(\sigma = 10)$ ，横轴表示不同数量的点对
右图：在同一张图片上，固定6个匹配点对。横轴表示不同程度高斯噪音。
基本上， $N=1$ 出现的评率比较高。

In theory, given perfect data from at least six points and a purely perspective camera model, the dimension $N$ of the null-space of $M^T M$ should be exactly one because of the scale ambiguity.
In contrast, if one considers an affine camera model, the null-space of $M^T M$ would have dimensionality four, because of the depth uncertainty of the four control points.
Since a perspective camera with a large focal length may be approximated by an affine model, the value of $N$ is not clear beforehand, and it could be any value between 1 and 4.

原文，至今都没办法很好理解，以后回来填坑吧。

EPnP作者建议只考虑 $N=1,2,3,4$ 的情况。 $N$ 确定后，我们可以通过下面这个约束去求解 $\beta_i$ ：
$||c^{c}_i - c^{c}_j||^2 = ||c^w_i - c^w_j||^2$
上面这个式子的意思是，摄像头的外参描述的只是坐标系的变换，并不会改变点之间的距离。用 $\beta_i$ 表示 $c^c_i$ ，我们可以得到：
$||\sum^{N}_{k=1}\beta_{k}v^{[i]}_k - \sum^{N}_{k=1}\beta_{k}v^{[j]}_k||^2 = ||c^w_i - c^w_j||^2$
对于4个控制点，我们一共可以得到 $C^2_4 = 6$ 个这样的方程。下面根据 $N$ 的不同取值，对 $\beta$ 进行求解。

$N = 1$

一共有1个未知数，可以直接算出 $\beta$ 。
$\beta = \frac{\sum_{i,j \in [1,4]}||v^{[i]} - v^{[j]}|| · ||c^w_i - c^w_j||}{\sum_{i,j \in [1,4]}||v^{[i]} - v^{[j]}||^2}$

$N = 2$

一共有3个未知数
$\begin{bmatrix} (v^{[i]}_1 - v^{[j]}_1)^2 & (v^{[i]}_1 - v^{[j]}_1)(v^{[i]}_2 - v^{[j]}_2) & (v^{[i]}_2 - v^{[j]}_2)^2 \end{bmatrix} \begin{bmatrix} \beta_{11}\\ \beta_{12}\\ \beta_{22} \end{bmatrix} = || c^w_i - c^w_j||^2$
把上面的式子写成 $L\beta = \rho$ ，因为未知数小于方程式的数量，用pseudo-inverse解出：
$\beta = (L^T L)^{-1}L^T \rho$

$N = 3$

一共有6个未知数，有6个方程。求逆得 $\beta$
$\beta = L^{-1} \rho$

$N = 4$

一共有10个未知数，却只有6个方程。这里我们发现一个问题，本身我们只有 $\beta_1,\beta_2,\beta_3,\beta_4$ 这4个未知数，注意这里是一次项的。但是把式子展开后，在 $L$ 里的都是形如 $\beta_{12}$ 的二次项。
论文中使用relinearization，具体步骤如下：

通过最初的约束方程得到 $null\ space中的\beta_{ab}$
$\beta_{ab}\beta_{cd} = \beta_a \beta_b \beta_c \beta_d = \beta_{a'} \beta_{b'} \beta_{c'} \beta_{d'}$
这里 $\{a',b',c',d' \}是\{a,b,c,d\}排列的某种情况$ 。例如，已知 $\beta_{11},\beta_{12},\beta_{13}$ ，可得 $\beta_{23}= \frac{\beta_{12}\beta_{13}}{\beta_{11}}$ 。
如此，我们就可以用 $\beta_{11},\beta_{12},\beta_{13},\beta_{14}$ 来表示其他的二次项，重新表示成4个未知数。
通过引入新的二次方程得到新的系数。
线性求解新方程组

如何选定 $N$ 的取值？

作者不建议直接选定 $N$ 的值，而是把4个 $N$ 值都算出来，选择重投影误差(reprojection error)最小的那个 $N$ 值。

$res = \sum_{i} dist^2(A[R|t]\begin{bmatrix} p^{w}_{i} \\ 1 \end{bmatrix}, u_i)$

其中， $dist(m,n)$ 是齐次点 $m和n$ 的2D距离。

4. 求解R,t

1. 计算控制点 $c^c_i$

$c^c_i = \sum^{N}_{k=1}$

2. 计算参考点的相机坐标

$p^c_i = \sum^{4}_{j=1} \alpha_{ij} c^c_j$

3. 运用ICP算法就可以求解出R和t了！！！

具体流程可以看我另一篇文章：ICP算法

5. 平面

6. 实验

在实际中要使用EPnP，匹配点的个数 $n$ 应大于15。

Reference

深入EPnP算法
EPnP算法
ORBSLAM2中的EPnP算法
【泡泡机器人公开课】第三十九课：PnP 算法简介&代码解析-柴政
- 视频
- PPT
重心坐标

EPnP论文笔记