opencv EpnP在输入4对完全对应的点对，计算出来的结果不稳定，分析原型，给出公式推导

EpnP（Efficient Perspective-n-Point）是一种相机姿态估计的算法，它可以通过已知的三维空间中的特征点和对应的图像中的二维特征点的对应关系，来计算相机的位姿（旋转矩阵和平移向量）。Opencv的 EpnP 算法基于Li的论文《A Two-Stage Algorithm for the Non-Coplanar Pose Estimation Problem》实现，它的稳定性相对较好，但也不是完全稳定的，可能会受到一些因素的影响，如噪声、点对的数量和质量等。

EpnP算法的公式推导比较复杂，这里简单介绍一下它的基本原理和一些关键的公式。假设我们已知三维空间中的 $n$ 个点 $P_i$ 和对应的图像中的 $n$ 个点 $p_i$，其中 $i=1,2,...,n$。我们假设相机的内参矩阵已知，为 $K$，相机的旋转矩阵为 $R$，平移向量为 $t$。我们的目标是求解 $R$ 和 $t$。

EpnP算法的核心思想是，将图像点的坐标系和相机坐标系都变换到三维空间中，然后通过最小化重投影误差来求解相机的位姿。具体来说，我们可以将图像点 $p_i$ 和相机坐标系的原点 $O_c$ 的连线，看作是一条与相机坐标系的 $Z$ 轴平行的射线，然后将这条射线与三维空间中对应的特征点 $P_i$ 相交，得到一个三维点 $X_i$。然后，我们可以将这个三维点 $X_i$ 投影到图像平面上，得到一个新的点 $p'_i$。这个点 $p'_i$ 应该与原始点 $p_i$ 非常接近，如果它们之间的距离很小，就说明我们的估计比较准确。

通过上述过程，我们可以得到 $n$ 个点对 $(P_i, p'_i)$，其中 $p'_i$ 是通过将 $P_i$ 投影到图像平面上得到的。我们可以将 $p'_i$ 和 $p_i$ 的坐标差看作是重投影误差，然后通过最小化所有点对的重投影误差来求解相机的位姿。具体来说，我们可以定义重投影误差为：

$$
e_i = \frac{\|p_i - p'_i\|^2}{\sigma_i^2},
$$

其中 $\sigma_i$ 是第 $i$ 个点的误差权重。通常情况下，我们可以将 $\sigma_i$ 设置为所有点对的重投影误差的平均值。然后，我们可以通过最小化所有点对的重投影误差的和来求解相机的位姿：

$$
\min_{R,t} \sum_{i=1}^n e_i.
$$

这个问题可以通过线性代数的方法来求解。具体来说，我们可以将相机坐标系的原点 $O_c$ 和三维点 $P_i$ 都表示为相机坐标系下的坐标向量，然后将射线的方向向量表示为 $u_i$。这样，我们就可以得到一个线性方程组：

$$
\begin{bmatrix}
u_1^T \\
u_2^T \\
\vdots \\
u_n^T
\end{bmatrix}
\begin{bmatrix}
r_{11} & r_{12} & r_{13} & t_1 \\
r_{21} & r_{22} & r_{23} & t_2 \\
r_{31} & r_{32} & r_{33} & t_3
\end{bmatrix}
=
\begin{bmatrix}
X_1^T \\
X_2^T \\
\vdots \\
X_n^T
\end{bmatrix}
,
$$

其中 $r_{ij}$ 是旋转矩阵 $R$ 的元素，$t_i$ 是平移向量 $t$ 的第 $i$ 个分量，$u_i$ 是射线的方向向量，$X_i$ 是三维点 $P_i$ 在相机坐标系下的坐标向量。

我们可以将上述线性方程组中的旋转矩阵和平移向量组成一个 $3\times 4$ 的矩阵 $M$。然后，我们可以使用 SVD（奇异值分解）方法来求解这个线性方程组，具体来说，我们可以将 $M$ 分解为 $M=U\Sigma V^T$ 的形式，然后求解 $R$ 和 $t$：

$$
R = UV^T, \quad t = \bar{t} - R\bar{P},
$$

其中 $\bar{t}$ 和 $\bar{P}$ 分别是所有三维点的重心和所有图像点的重心。这个求解过程相对比较简单，而且可以通过 SVD 方法来求解，不需要进行迭代，因此比较高效。

总的来说，EpnP算法是一种比较高效且准确的相机位姿估计算法，它可以通过已知的三维空间中的特征点和对应的图像中的二维特征点的对应关系，来计算相机的位姿（旋转矩阵和平移向量）。