一、前言
关于PNP问题就是指通过世界中的N个特征点与图像成像中的N个像点，计算出其投影关系，从而获得相机或物体位姿的问题。
opencv提供的solvepnp函数就是用来解决pnp问题。利用该函数可以实现测算相机/物体的空间姿态，也可以用来空间定位。
二、solvepnp函数解析
solvepnp函数原型
bool solvePnP(InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess=false, int flags=ITERATIVE )
objectPoints：特征点的世界坐标，坐标值需为float型，不能为double型，可以为mat类型，也可以直接输入vector<point3f>
imagePoints：特征点在图像中的像素坐标，可以输入mat类型，也可以直接输入vector<point3f>，注意输入点的顺序要与前面的特征点的世界坐标一一对应
cameraMatrix：相机内参矩阵
distCoeffs：相机的畸变参数【Mat_<double>(5, 1)】
rvec：输出的旋转向量
tvec：输出的平移矩阵
函数分析
solvepnp通过2D点和3D点求解相机的位姿(R,t)，在opencv3中常用的方法是epnp， DLS，迭代法。如果3D点分布在一个平面上，那么通过点对(3D点2D点)算出单应矩阵。然后进行SVD分解。如果平面，可直接使用SVD分解。在opencv2中，没有DLS方法。
三、旋转向量转换旋转矩阵
应用罗德里格斯变换
double rm[9];
cv::Mat rotM(3, 3, CV_64FC1, rm);
Rodrigues(rvec, rotM);
四、由旋转矩阵求得姿态角
代码：
float theta_z = atan2(rotMat[1][0], rotMat[0][0])*57.2958;
float theta_y = atan2(-rotMat[2][0], sqrt(rotMat[2][0] * rotMat[2][0] + rotMat[2][2] * rotMat[2][2]))*57.2958;
float theta_x = atan2(rotMat[2][1], rotMat[2][2])*57.2958;
理论：
以下图片内容转载自VShawn
最后得到的三个旋转角度都是相对于世界坐标系的旋转角度。
