solvepnp三维位姿估算

一、前言

关于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

最后得到的三个旋转角度都是相对于世界坐标系的旋转角度。