3D-2D：PnP算法原理

根据高博士的《视觉SLAM十四讲》内容和相关博客的内容，对PnP问题有了一点点新的理解。再次记录一下。
参考博客：
http://www.cnblogs.com/singlex/p/pose_estimation_0.html
http://www.cnblogs.com/singlex/p/pose_estimation_1.html
http://www.cnblogs.com/singlex/p/pose_estimation_3.html

1.问题背景—— 什么是PnP问题 ？

PnP(Perspective-n-Point)是求解 3D 到 2D 点对运动的方法。它描述了当我们知道n 个 3D 空间点以及它们的投影位置时,如何估计相机所在的位姿。——《视觉SLAM十四讲》

通俗的讲，PnP问题就是在已知世界坐标系下N个空间点的真实坐标以及这些空间点在图像上的投影，如何计算相机所在的位姿。罗嗦一句：已知量是空间点的真实坐标和图像坐标，未知量（求解量）是相机的位姿。

PnP 问题有很多种求解方法，例如用三对点估计位姿的 P3P 、直接线性变换(DLT)、EPnP。此外,还能用非线性优化的方式,构建最小二乘问题并迭代求解,也就是万金油式的 Bundle Adjustment。下面介绍逐一介绍。

2.PnP问题的求解方法

2.1 P3P

2.1.1 算法的实际理解

PnP问题是在已知n 个 3D 空间点以及它们的投影位置时估计相机所在的位姿。那么 n 最小为多少时我们才能进行估算呢（最少需要几个3D-2D点对）？

我们可以设想以下场景，设相机位于点Oc，P1、P2、P3……为特征点。

场景1：N = 1时
当只有一个特征点P1，我们假设它就在图像的正中央，那么显然向量OcP1就是相机坐标系中的Z轴，此时相机永远是面对P1，于是相机可能的位置就是在以P1为球心的球面上，此外球的半径也无法确定，于是有无数个解。

场景2：N = 2时
现在多了一个约束条件，显然OcP1P2形成一个三角形，由于P1、P2两点位置确定，三角形的边P1P2确定。再加上向量OcP1和OcP2，从Oc点射向特征点的方向角也能确定。于是能够计算出OcP1的长度=r1，OcP2的长度=r2。于是这种情况下得到两个球：以P1为球心，半径为r1的球A；以P2为球心，半径为r2的球B。显然，相机位于球A，球B的相交处，依旧是无数个解。

场景3：N = 3时
这次又多了一个以P3为球心的球C，相机这次位于ABC三个球面的相交处，终于不再是无数个解了，这次应该会有4个解，其中一个就是我们需要的真解——即相机真实的位姿。

场景4：N > 3时
N=3时求出4组解，好像再加一个点就能解决这个问题了，事实上也几乎如此。说几乎是因为还有其他一些特殊情况，这些特殊情况就不再讨论了。N>3后，能够求出正解了，但为了一个正解就又要多加一个球D显然不够"环保"，为了更快更节省计算机资源地解决问题，先用3个点计算出4组解获得四个旋转矩阵、平移矩阵。根据公式：

将第四个点的世界坐标代入公式，获得其在图像中的四个投影（一个解对应一个投影），取出其中投影误差最小的那个解，就是我们所需要的正解。

2.1.2 算法的数学推导

P3P 需要利用给定的三个点的几何关系。它的输入数据为三对 3D-2D 匹配点。记 3D点为 A, B, C,2D 点为 a, b, c,其中小写字母代表的点为大写字母在相机成像平面上的投影，如上图所示。此外，P3P 还需要使用一对验证点,以从可能的解出选出正确的那一个(类似于对极几何情形)。记验证点对为 D − d,相机光心为 O。
请注意,我们知道的是A, B, C 在世界坐标系中的坐标,而不是在相机坐标系中的坐标。

首先,显然,三角形之间存在对应关系:

对于三角形Oab 和 OAB，利用余弦定理可得：

对于另外两组三角形，也有同样的结论：