EPnP论文笔记

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

1. EPnP简介

1.1 EPnP主要思想

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

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

1.2 EPnP算法流程

1.2.1 EPnP符号说明

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

1.2.2 算法流程

控制点的选择，Select four control points (PCA)
Compute the coefficient
Compute the in the camera coordinate
3.1
3.2
3.3
Recover 3d points in the camera coordinates
Compute the R and t

2. 关于控制点

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

2.1 Barycentric Coordinates重心坐标

Barycentric Coordinates

给定一个三角形，则在三角形所在的平面上的任意点，都表示为点的线性组合：

以下是重心坐标的推导过程，跳过不看：

2.2 求解权重因子

EPnP也是采用了这种类似的表示方法，用四个控制点的加权和(weighted sum)来表示参考点，在世界坐标系下，可得：

那么在相机坐标系下的参考点，可写为：
$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个控制点满足要求，那么

一共有3个未知数，4个方程式。这种方程式数量大于未知数个数的情况称之为超定方程。只存在最小二乘意义上的解。也就是，在一般情况下，不存在精确满足4个方程的解。
如何求解?

2.3 控制点的选择

理论上来说，控制点的选择没有太多讲究，只要保证可逆就行了。但是，作者在实验中发现其中一个参考点设置为质心后，该算法的稳定性会上升。这在某种程度上是说的通的，因为质心的使用，使得数据在坐标系上得到归一化。
先计算第一个控制点:

其他的三个点通过PCA计算得出，先计算协方差矩阵：

则，协方差矩阵为。
记的特征值为，对应的特征向量为，那么剩余的三个控制点可以按照下面的公式来确定：

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

3.1 获得线性方程组

设已知的相机内参矩阵为，是在图像坐标系上的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}$

由上可得，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.$
取个式子的系数在一起的得到的矩阵，就是控制点在相机坐标系下的坐标。
$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}$

整理后可得，。
显然落在的右零空间中，或者，可以表示为：

3.2 求解

3.2.1 求解

右奇异向量(right-singular vector)，可以对M进行SVD求解得到。但是SVD耗时很长，目前时间复杂度最低的SVD解法是。

不妨换个思路，可通过求解的0特征值对应的特征向量更快求解。计算矩阵是EPnP中最耗时的步骤，时间复杂度为。

在相机坐标系下，对于第i个控制点

3.2.2 求解

接下来，就是要求解。

根据的数量，对有不同的求解方式。是矩阵的零空间的维度。作者通过仿真实验，发现和相机焦距有关。

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

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

In theory, given perfect data from at least six points and a purely perspective camera model, the dimension of the null-space of should be exactly one because of the scale ambiguity.
In contrast, if one considers an affine camera model, the null-space of 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 is not clear beforehand, and it could be any value between 1 and 4.

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

EPnP作者建议只考虑的情况。确定后，我们可以通过下面这个约束去求解：

上面这个式子的意思是，摄像头的外参描述的只是坐标系的变换，并不会改变点之间的距离。用表示，我们可以得到：

对于4个控制点，我们一共可以得到个这样的方程。下面根据的不同取值，对进行求解。

一共有1个未知数，可以直接算出。

一共有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$
把上面的式子写成，因为未知数小于方程式的数量，用pseudo-inverse解出：

一共有6个未知数，有6个方程。求逆得

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

通过最初的约束方程得到

这里。例如，已知，可得。
如此，我们就可以用来表示其他的二次项，重新表示成4个未知数。
通过引入新的二次方程得到新的系数。
线性求解新方程组

如何选定的取值？

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

其中，是齐次点的2D距离。

4. 求解R,t

1. 计算控制点

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

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

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

5. 平面

6. 实验

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

Reference

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