opencv中solvePnPRansac函数求解相机位姿

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

PnP（Perspective n Points）就是你有n个点的3D位置和它们的投影，然后要算相机的位姿。这个倒是SLAM里最常见的情况，因为你会有一堆的地图点和像素点等着你算。PnP的做法有好多种：直接线性变换，P3P，EPnP，UPnP等等，基本你去找OpenCV的SolvePnP中的参数即可，好用的都在那里。除此之外，通常认为线性方程解PnP，在有噪声的情况下表现不佳，所以一般以EPnP之类的解为初始值，构建一个Bundle Adjustment（BA）去做优化。

一、函数介绍

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

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

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

cout<<"pnp OK="<最后我们从旋转向量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(0,0);
 T(1,3) = tvec.at(1,0);
 T(2,3) = tvec.at(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="<

备注：

//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等求位姿方法)