一文详解PnP算法原理

PnP(Perspective-n-Point)问题的几何结构如图1所示，给定3D点的坐标、对应2D点坐标以及内参矩阵，求解相机的位姿。

数学语言描述如下：

图1.PnP几何结构

1.直接线性变换法(Direct Linear Transform，DLT)

假设：摄像机已经校准过了。

已知：

求解相机的外参：R、t

透视投影模型为：

每组3D-2D匹配点对应两个方程，一共有12个未知数，至少需要6组匹配点。

设有N组匹配点，则：

上式写成矩阵形式：

AF=0

当N=6时，可以直接求解线性方程组。

因此, 旋转矩阵, 平移矩阵求得:

2.P3P

P3P问题是已知三个3D目标点与其2D投影之间的对应关系，来确定标定相机的位姿问题。