一文读懂PnP问题及opencv solvePnP、solvePnPRansac函数

solvePnP——Perspective-n-Point

• 参考资料：一文了解PnP算法 PnP问题

一、位姿求解方法

1. 对极约束：2D-2D，通过二维图像点的对应关系，恢复两帧之间相机的运动。
2. PnP：3D-2D，求解3D到2D点对运动的方法。已知3D空间点及其在相机投影位置时，求解相机运动。
3. ICP：3D-3D，配对好的3D点，已知世界坐标系下的3D点和相机坐标系下的3D点。
   

二、PnP概念

如果场景的三维结构已知，利用多个控制点在三维场景中的坐标及其在图像中的透视投影坐标即可求解出相机坐标系与世界坐标系之间的绝对位姿关系，包括绝对平移向量t以及旋转矩阵R，该类求解方法统称为N点透视位姿求解 （Perspective-n-Point，PnP问题）。这里的控制点是指准确知道三维空间坐标位置，同时也知道对应图像平面坐标的点。

已知条件

• n个世界坐标系3D参考点(3D reference points)坐标；
• 与3D点对应的、投影在图像上相机图像坐标系的2D参考点(2D referece points)坐标；并不是相机坐标系或相机像素坐标系，注意区分、
• 相机的内参K；

求解：

• 世界坐标系与摄像机坐标系之间的位姿变换 ：{ R ∣ t }

三、PnP的使用场景

1. 求解相机的位姿，一般应用于AR，人脸跟踪等；通常输入的是物体在世界坐标系下的3D点以及这些3D点在图像上投影的2D点，因此求得的是相机（相机坐标系）相对于真实物体（世界坐标系）的位姿。
2. 求取前一帧到当前帧的相机位姿变换，一般用于slam中；通常输入的是上一帧中的3D点（在上一帧的相机坐标系下表示的点）和这些3D点在当前帧中投影得到的2D点，所以它求得得是当前帧相对于上一帧得位姿变换
   

四、OpenCV函数solvePnP、solvePnPRansac

官网链接：OpenCV: Camera Calibration and 3D Reconstruction

• objectPoints：特征点的世界坐标，坐标值需为float型，不能为double型，可以为mat类型，也可以直接输入vector
• imagePoints：特征点在图像中的像素坐标，可以输入mat类型，也可以直接输入vector，注意输入点的顺序要与前面的特征点的世界坐标一一对应
• cameraMatrix：相机内参矩阵
• distCoeffs：相机的畸变参数 Mat_(5, 1)
• rvec：输出的旋转向量：
• tvec：输出的平移向量
• useExtrinsicGuess： 用于SOLVEPNP迭代的参数。如果为true（1），函数使用提供的rvec和tvec值分别作为旋转和平移向量的初始近似，并进一步优化它们。默认值为False。
• flags：PnP的计算方法包括：DLT，P3P，EPnE，UPnP，每种求解方式存在其限制。

enum { SOLVEPNP_ITERATIVE = 0,
       SOLVEPNP_EPNP      = 1, //!< EPnP: Efficient Perspective-n-Point Camera Pose Estimation @cite lepetit2009epnp
       SOLVEPNP_P3P       = 2, //!< Complete Solution Classification for the Perspective-Three-Point Problem 
       SOLVEPNP_DLS       = 3, //!< A Direct Least-Squares (DLS) Method for PnP  @cite hesch2011direct
       SOLVEPNP_UPNP      = 4, //!< Exhaustive Linearization for Robust Camera Pose and Focal Length Estimation 
       SOLVEPNP_AP3P      = 5, //!< An Efficient Algebraic Solution to the Perspective-Three-Point Problem 
       SOLVEPNP_MAX_COUNT      //!< Used for count
};//flag参数PnP计算方法

solvePnP的一个缺点是对异常值不够鲁棒，当我们用相机定位真实世界的点，可能存在错匹配，对误匹配进行RANSAC过滤，RANSAC为（Random Sample Consensus, 随机抽样一致性）。

五. PnP问题求解原理

目前解法主要有：直接线性变换（DLT），P3P，EPnE，UPnP，非线性优化方法BA。

• 直接线性变换法DLT：PnP问题之DLT解法 – 知乎
• P3P：P3P推导 利用余弦定理
• EPnP:深入EPnP算法
• 非线性优化方法：Bundle Adjustment最小化重投影误差求解PnP–BA
  前面三种方法均可直接调用OpenCV函数接口，而非线性优化的方法实现形式多样，可以利用Ceres、G2o等开源优化库或者手写高斯牛顿法实现。