计算 solvepnp 和 solvepnpRansac 求解空间某一三维坐标系到摄像机三维坐标系的三维旋转R 和三维平移 T

参考：

pnp问题与 solvepnp函数：https://www.jianshu.com/p/b97406d8833c

对图片进行二维仿射变换cv2.warpAffine() or 对图片进行二维射影变换cv2.warpPerspective ：https://www.jianshu.com/p/1c6512d475cc

关键：今天裁图过程中发现裁出来的一些图较正常图发生了奇怪的仿射变换，最后发现是solvepnp求解出的头部坐标系到摄像机坐标系的RT有错误，改用solvepnpRansac之后得到解决。

//cv::solvePnP(p_3d17, p_2d17, camInstris, camDistort, cam_r, cam_t);
cv::solvePnPRansac(p_3d17, p_2d17, camInstris, camDistort, cam_r, cam_t, true, 1000, 1, 0.99);

solvepnpRansac比solvepnp多出了三个参数，1000为算法迭代次数。

pnp问题介绍

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

image.png

经典的P3P问题可以转化为一个四面体形状的确定问题，如图所示。即已知条件为知道控制点 A,B,C的位置以及在摄像机中的投影坐标求棱长边a,b,c的问题。通过余弦定理，再利用点云配准方法可以得到摄像机坐标系相对于世界坐标系的平移以及旋转。图中的P点相当于相机的光心，A,B,C相当于世界坐标系下已知相对位置关系的三个控制点，A‘,B‘,C‘为图像坐标系中对应的三个点。PNP解决的是纯数学问题，数学证明在此处省略。

Opencv中PNP的求解函数

solvepnp

void solvePnP(InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess=false, int flags = CV_ITERATIVE)

Parameters:

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

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

cameraMatrix - 相机的内参矩阵

distCoeffs - 相机的畸变系数

以上两个参数通过相机标定可以得到。相机的内参数的标定参见：http://www.cnblogs.com/star91/p/6012425.html

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

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

flags - 默认使用CV_ITERATIV迭代法

solvepnpRansac

应用了pnp算法，提高了精度。