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