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,迭代法(默认)。
两种常见的求解方法
迭代法
P3P三对点估计位姿
迭代法cv::SOLVEPNP_ITERATIVE
SOLVEPNP_ITERATIVE的迭代方法是基于Levenberg-Marquardt优化。 在这种情况下,函数会找到一个使重新投影误差最小的位姿(pose),该位姿是观察到的投影imagePoints与使用projectPoints将objectPoints投影的点之间的平方距离的总和(参考)。
P3P三对点估计位姿cv::SOLVEPNP_AP3P
P3P只需要使用3对匹配点。
ABC为三个世界坐标中的点,abc为ABC投影在图像上的点。已知ABC的世界坐标系中的坐标,abc的相机坐标系坐标。O为相机中心,三角形Oac和OAC,Oab和OAB,Obc和OBC两两相似。根据余弦定理有
可以看出式子中有3个未知数,所以可以解除对应的3个点的解,然后就变成3D-3D的问题了,使用ICP就可以获得最后的相机的姿态了。