PNP即”Perspective-N-Points”，是求解 3D 到 2D 点对运动的方法。它描述了当我们知道n个3D空间点以及它们在图像上的位置时，如何估计相机所在的位姿。PnP 问题有很多种求解方法，例如用三对点估计位姿的 P3P（通常需要额外一个点进行验证结果），直接线性变换（DLT），EPnP（Efficient PnP，已知内参时用），UPnP（内参未知时用） 等等）。此外，还能用非线性优化的方式，构建最小二乘问题并迭代求解，也就是万金油式的 Bundle Adjustment。

P3P

已知：匹配点，点的世界坐标记为，图像上的2D点记为。

未知：相机系下3D点的坐标是未知的，即，一旦点在相机坐标系下的坐标能够算出，我们就得到了的对应点，把PnP问题转换为了ICP问题。

我们的目标就是通过纯几何的方法求出上述未知量，过程如下。

由于余弦定理可知：

对上面三式全体除以，记，得：

记，得：

将第一个式子中带入后面两个式子中，得：

上式中几个余弦角度：

是已知的，也是已知的，所以未知量仅有，解析地求解该方程组是一个复杂的过程，需要用**吴消元法**。这样就可以求得，然后带入求解，即可得到，进而得到。该方程最多可能得到四个解，但我们可以用第4个验证点来计算最可能的解，得到在相机坐标系下的坐标。然后，根据的点对，计算相机的运动，此处可参考文献Least-Squares Rigid Motion Using SVD

EPnP

问题描述

EPnP即Efficient PnP，参考文献 EPnP: An Accurate O(n) Solution to the PnP Problem。

问题描述如PnP，更加具体的，我们已知一组特征点，对于每个特征点，我们有如下信息：

• 特征点在世界坐标系的坐标：

• 特征点在成像平面上的坐标：

• 已知相机内参

求：世界坐标系到相机系的变换矩阵

算法假设

EPnP的思想是无论世界系还是相机系下的点都可以由4个控制点线性组合，记：

• 世界系下4个控制点表示为：

• 相机系下4个控制点表示为：

EPnP算法将参考点的坐标表示为控制点坐标的加权和：

其中是加权系数，一旦虚拟控制点确定后，且满足4个控制点不共面的前提，是唯一的。

控制点的存在性

现在讨论控制点的存在性，上式可以写成：

可见只要非奇异，就一定可以找到满足条件的，即：

接下来，我们讨论相机坐标系下，控制点和参考点之间的关系：

如何选择控制点

由于，因此，带入上式，得：

可见系数具有不变性，如果我们能够求出控制点在相机坐标系中的坐标，那么对于任意一个3D点k，我们可以求得其在相机系下的坐标：

这就变成了如P3P同样的问题了，即求解3D-3D位姿估计问题。

记世界系下所有3D点集为：

第一个控制点是所有3D点的重心：

对所有3D点去中心化，这些点罗列成矩阵形式：

对进行特征值分解（注意此时并非对A进行SVD分解，是为了减低时间复杂度，SVD分解的复杂度为），其特征值为，对应的特征向量为：

则剩余的3个控制点表示为如下公式：

求解控制点在相机系下的坐标

记为参考3D点的图像坐标，则：

其中是尺度因子，将控制点带入上式，得：

上式可以得到两个线性方程：

把这N个点的约束罗列在一起，我们就可以得到如下矩阵：

其中为12维向量：

维度，如下形式:

参考

• 深入EPnP算法
• 3d-2d位姿估计之EPnP算法