PNP算法的目的是利用合作目标的尺寸信息，合作目标上特征点之间的位置关系以及提取到的特征点坐标和相机内参数来解算相机坐标系和目标坐标系之间的相对位置和姿态的关系。根据得到的坐标系转换关系，对捕捉到的每一幅图片都能解算出对应的目标坐标系下特征点的坐标。

位置姿态变换理论

坐标系之间的姿态变换是指从一个坐标系变换到另一个坐标系所需要经历的过程。

从坐标系变换到坐标系需要将坐标系分别绕、、轴旋转角度、、，这三个角度叫做从到的姿态，的原点在中的坐标值(、、)叫做坐标系到坐标系的位移。将点在下的坐标转换为在下的坐标就需要经过三个方向的旋转及平移变换。

坐标变换可以拆分成姿态变换和坐标平移两个过程。

定义点在坐标系下的坐标为(、、)，在中的坐标是(、、)。以逆时针旋转为正，则旋转矩阵分别为：

以上旋转矩阵是正交矩阵，正交矩阵的性质是各行各列的平方和等于 1，矩阵的逆等于矩阵的转置。

联立可得：

扫描二维码关注公众号，回复： 6126134 查看本文章

令

则点坐标从坐标系转换到坐标系中可表示为如下形式：

经典的P3P问题
           基于三个特征点的位置姿态测量方法是位姿测量算法中的经典方法，P3P测量模型中合作目标结构相对简单，在图像中检测到目标之后，特征点的匹配也相对容易。

P3P问题的基本约束方程为：

解该方程组可得到四组解，实际情况下相机和靶标的位置和姿态关系是唯一的，如何确定相机和靶标的位置关系是以上四种情况中的那一种情况，只有研究解和特征点排布与相机位置关系之间的联系。靶标是合作目标，可以利用靶标上特征点和相机的关系确定哪一组解是正确解。

参考沈阳自动化所的文章“关于 P3P 问题解的唯一性条件的几点讨论”，可知当合作靶标与摄像机处于某个特定的排布位置时，唯一解可以确定。此时能够得到、、三点到摄像机光心的距离。

利用这3个距离，再加上相机焦距，特征点的图像坐标值，图像中心点坐标值等信息，便能得到旋转矩阵以及平移矩阵。由此：

则旋转矩阵可表示为：

姿态角亦可求

由以上可知，PNP问题的目的是求取两个信息：①世界坐标系下的特征点到摄像机光心的距离；②合作目标相对于摄像机的姿态角，此姿态角可通过求旋转矩阵得到。