相机姿态估计（三）--P3P

P3P

论文：Complete Solution Classification for the Perspective-Three-Point Problem

  我们首先需要知道的是P3P并不是直接根据3D-2D点求出相机位姿矩阵，而是先求出对应的2D点在当前相机坐标系下的3D坐标，然后根据世界坐标系下的3D坐标和当前相机坐标系下的3D坐标求解相机位姿的。P3P的求解是从余弦定理开始的，设相机坐标中心为点P，A、B、C为不共线的三个3D点，D为验证3D点，根据余弦定理有如下公式：

接下来其实是对上述3个式子消元化简的过程，同时除以

，

并且使得

，

则可得：

然后再次进行替换，另：

，

可得：

将第一个式子代入第2，3式，可以化简得到：

 

接下来的过程就是如何通过上述两个式子求解A，B，C在当前相机坐标系下的坐标。首先需要明确的是哪些量是已知量，输入的是3D-2D的坐标，也即

 

都是已知的。因为首先AB，BC，AC的距离都是可以根据输入的3D点求得，而输入的2D点可以求解三个余弦值（如何求解，像素坐标根据相机内参矩阵和畸变参数可以求得在归一化图像平面上的3D坐标，此时 z=1，故余弦值可求）。此时未知数仅x，y两个，所以理论上两个未知数两个方程，是可求的。（从x，y求PA，PB，PC也可求）

　　3、具体的求解过程：

　　3.1、首先是根据2D坐标求解余弦值得过程，首先是由像素坐标到归一化图像坐标的转变，根据就是相机模型

然后是L2归一化的过程，我们知道求解角度的时候用的是归一化坐标（此归一化非彼归一化，上面是归一化到z值等于1的平面上，这里讲的是数学上的归一化）

有了上述值就可以求解余弦值了

同理可求。

　　3.2、根据3D坐标求解AB，AC，BC的值，以AB为例

AC，BC同理可求，所以v，w也可以求解。

 　　3.3、接下来就是一个二元二次方程的求解，比较难求，但是这在数学上是可以求解的，需要用到Wu Ritt的零点分解方法，它可以将原方程等效成一组特征列（Characteristic Serial, CS），凡是原方程组的解都会是CS的解，但是CS的解不一定是原方程的解，所以需要验证，这里的等效方程为：

其中的未知数a1~a4都是已知的，因为原方程的系数是已知的，后文有系数附录，因此我们可以求得x，y的值，4次方程组理论上有4组解，但其实只有一组是合适的。

　　3.4、求得了x，y的值，就可以求取PA，PB，PC的值，根据下面的公式，AB已知，可以先求PC，然后分别求解PB，PA：

 但是我们需要的是A，B，C在相机坐标系下的坐标，而不是PA，PB，PC的长度，所以还需根据长度求取点的坐标，求解方法是用向量公式：

其中a是单位向量，||PA||是模值，所得即A在相机坐标系下的坐标。

　　最后求得了A，B，C的坐标就可以通过世界坐标系到当前相机坐标的变换求解相机位姿，注意上面求得了4组解，这里需要使用D点确认哪组解是最合适的。