P3P位姿估计推导

之前博客已经讲过P3P算法了《学习笔记之——P3P与ICP位姿估计算法及实验》本博文是具体推导的整理

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

 

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

　　具体的求解过程：

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

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

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

同理可求。

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

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

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

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

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

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

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

最后求得了A，B，C的坐标就可以通过世界坐标系到当前相机坐标的变换求解相机位姿

​

 

其中下标c表示Camera，w表示World。 

​

​

如上图所示，当合作标志特征点个数为3时，且3点不共线时，记

​

​参考资料

https://www.cnblogs.com/mafuqiang/p/8302663.html