opencv中solvePnPRansac函数求解相机位姿

PnP（Perspective n Points）：2D—3D，求解相机位姿

一、函数介绍

源码位置：An example of how to use solvePNPRansac for object detection can be found at opencv_source_code/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/

二、例子

 cv::Mat rvec = cv::Mat::zeros(3, 1, CV_32FC1);          // output rotation vector
 cv::Mat tvec = cv::Mat::zeros(3, 1, CV_32FC1); 

// RANSAC parameters
int iterationsCount = 300;      // number of Ransac iterations.
float reprojectionError = 5.991;  // maximum allowed distance to consider it an inlier.
double confidence = 0.95;        // ransac successful confidence.

cv::Mat inliers;

//vector<Point3f> vPt3D；// 世界坐标：x=(u-u0)*depth/fx; y = (v-v0)*depth/fy; z=depth; （去除深度值不存在的点对，顺序必须一致）

//vector<Point2f> vPt2D；//像素坐标，不需要归一化，会受噪声影响

bool b = solvePnPRansac(vPt3D, vPt2D, K, DistCoef, rvec, tvec, false, iterationsCount, reprojectionError, confidence, inliers, SOLVEPNP_ITERATIVE);   //点对个数必须大于4

cout<<"pnp OK="<<b<<", inliers point num="<<inliers.rows<<endl;

最后我们从旋转向量rvec和平移向量tvec得到变换矩阵，该代码的运行需要有opencv、Eigen库的支持。（不然会出现计算的R、t数值很大，Rcw；相机坐标到世界坐标的转换）

//1.首先通过罗德里格斯变换将旋转向量rvec转换成3*3的旋转矩阵r
cv::Mat R;
cv::Rodrigues(rvec, R);
Eigen::Matrix3d r;
cv::cv2eigen(R, r);

//2.然后将平移向量tvec和旋转矩阵r转换成变换矩阵T（M2 = T*M1）
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
Eigen::AngleAxisd angle(r);
Eigen::Translation<double, 3> trans(tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2));


  


  
T = angle;  //3*4

 T(0,3) = tvec.at<double>(0,0); 

 T(1,3) = tvec.at<double>(1,0); 

 T(2,3) = tvec.at<double>(2,0);

 Eigen::Quaterniond q(r);//四元数   


cout << "q:" << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl; 


double w = q.x();   


double x = q.y();   


double y = q.z();   


double z = q.w();   


double a1 = atan2(2*(w*x+y*z),1-2*(x*x+y*y))*180/CV_PI;//旋转角度   ,绕x轴旋转


double a2 = asin(2*(w*y-z*x))*180/CV_PI;//绕y轴旋转
double a3 = atan2(2*(w*z+x*y), 1-2*(y*y+z*z))*180/CV_PI;  


cout<<"a1="<<a1<<", a2="<<a2<<",a3="<<a3<<endl; 



备注：

//3.求解变换矩阵T的逆矩阵temp
T = angle.inverse();
Eigen::Matrix<double,3,1> t;
cv::cv2eigen(tvec, t);
t = -1 * angle.inverse().matrix() *t;
T(0, 3) = t(0);
T(1, 3) = t(1);
T(2, 3) = t(2);
Eigen::Matrix<double, 4, 4> temp = Eigen::Matrix<double, 4, 4>::Zero();
for (int i = 0; i < 3; ++i){
    for (int j = 0; j < 4; ++j){
        temp(i, j) = T(i, j);
    }
}
temp(3, 0) = 0;
temp(3, 1) = 0;
temp(3, 2) = 0;
temp(3, 3) = 1;


补充知识：
摄像机坐标系中，摄像机在原点，x轴向右，z轴向前（朝向屏幕内或摄像机方向），y轴向上（不是世界的上方而是摄像机本身的上方）。



 cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t();

   cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3);



参考文献：
http://blog.csdn.net/gxsheen/article/details/52636852 （R、t向量转换成变换矩阵）
http://www.cnblogs.com/gaoxiang12/p/4669490.html （例子）


http://blog.csdn.net/cp32212116/article/details/38705297 （四元数、旋转角度）
http://docs.opencv.org/3.1.0/d9/d0c/group__calib3d.html#ga50620f0e26e02caa2e9adc07b5fbf24e （函数说明)
https://www.zhihu.com/question/51510464/answer/132196916 (2D-3D 2D-2D等求位姿方法)