PnP算法

PnP(Perspective-n-Point)是求解3D到2D点的对应方法。它描述了当知道n个3D空间点及其位置，如何估计相机的位姿。如果两张图像中的一张特征点3D位置已知，那么至少需要3个点对(以及至少一个额外验证点验证结果)就可以计算相机的运动。

PnP的应用范围很广比如两阶段法的6D姿态估计以及视觉SLAM等等。

特征点的3D位置可以由三角化或者RGB-D相机的深度图确定，当然还有其他方法。

PnP的数学模型

PnP问题的几何结构如下图所示，给定3D点的坐标以及对应2D点的坐标以及内参矩阵，求解相机的姿态。

OpenCV 中的 PNP 求解函数

void solvePnP(InputArray objectPoints,

              InputArray imagePoints,

              InputArray cameraMatrix,

              InputArray distCoeffs,

              OutputArray rvec,

              OutputArray tvec,

              bool useExtrinsicGuess=false,

              int flags = CV_ITERATIVE);

参数：

objectPoints - 世界坐标系下的控制点的坐标，vector<Point3f> 的数据类型在这里可以使用

imagePoints - 在图像坐标系下对应的控制点的坐标。vector<Point2f> 在这里可以使用

cameraMatrix - 相机的内参矩阵，通过相机标定可以得到

distCoeffs - 相机的畸变系数，通过相机标定可以得到

rvec - 输出的旋转向量。使坐标点从世界坐标系旋转到相机坐标系

tvec - 输出的平移向量。使坐标点从世界坐标系平移到相机坐标系

flags - 默认使用CV_ITERATIV迭代法

solvePnP 里有三种解法：P3P， EPnP，迭代法（默认）。