相机姿态估计（四）--AP3P

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中实现了。