AP3P
论文：An Efficient Algebraic Solution to the Perspective-Three-Point Problem
从数学的角度，提出了经典P3P算法的更快，更鲁棒，更准确的求解算法。根据已知3对点，可以计算相机位姿和参数。
问题定义：
给定特征fi i = 1, 2, 3在参考坐标系中的位置,以及特征点在相机坐标系中的方向测量向量
，目标是估计相机的旋转矩阵和位置
。其中C表示相机坐标系，G表示 参考坐标系。
根据几何关系，我们得出：
其中di表示参考坐标系中相机到特征点的欧式距离，两两相减，并投影到对应面的法向量：
为了计算旋转矩阵中的角度，我们定义如下因子分解：
其中：
将式子5代替到公式2中，得到标量方程：（未知）
然后利用罗德里格斯变换:
为了求其他两个角度，定义如下因子分解：
利用旋转矩阵特性：
化简式10：
式13表述的几何关系下图所示：
接着，再次采用罗德里格斯变换得到：
展开并重排上式：
其中：
出现3次，由此，我们重写式12：
为了化简等价于16式的18式，我们寻找一个，使得下式成立：
推出：
然后，根据19、8式得到：
接着，利用14式，展开18式，得到一个16式的等价结果：
在24式中，代替17式为0，并重排结果：
带入具体的i = 1，2到式25，得到：
因为：
继续化简式26，同时 引入新参数：
然后用代替
：
根据28式，得到：
计算两边的归一化结果：
得到一个关于的4阶多项式.更加简洁的表达如下：
剩下的就是如何计算的问题了，作者采用，逼近的方式找到式38的近似解，一旦找到式38的4个解，便采用牛顿方法进行优化，以最小代价提高精度。
另外，要注意的是：
对于每个cos，可能会得到2个sin值，如下：
这会导致计算出2个旋转矩阵， 因此，借用di>0,可以排除一个无效的。下一步，根据每对（cos ，sin
），计算（cos
，sin
），利用式37：
最后，根据19、27，5，我们找到相机位置，也可以利用5，12，18式，更快速得到：
我们定义如下旋转矩阵：
因此：
将式54带入53，得：
接着，计算相机位置：
这里，为了速度，只计算了一个点i=3,如果更在意精度，可以用最小二乘拟合i=1,2,3点的结果。
最后，AP3P已经在OpenCV3中实现了。