通过solvePnP求解相机位置 (代码可运行)

目录

一、Opencv函数使用

二、目标是求得相机在世界坐标系下的3D坐标。

1、法一：

2、法二：

3、法1附录源代码（可运行通过）

---

一、Opencv函数使用

solvePnP原型为：

bool cv::solvePnP	(	InputArray 	objectPoints,
InputArray 	imagePoints,
InputArray 	cameraMatrix,
InputArray 	distCoeffs,
OutputArray 	rvec,
OutputArray 	tvec,
bool 	useExtrinsicGuess = false,
int 	flags = SOLVEPNP_ITERATIVE 
)	

其中，rvec以及tvec为旋转向量与平移向量。

 在solvePnP调用结束后，需要使用罗德里格斯变化将旋转向量转换成旋转矩阵。

Mat r,t;
solvePnP(pts_3d,pts_2d,K,Mat(),r,t,false,cv::SOLVEPNP_EPNP);
Mat R;
Rodrigues(r, R);

---

二、目标是求得相机在世界坐标系下的3D坐标。

1、法一：

利用求得的旋转矩阵和平移矩阵：

Pcam代表物体在相机坐标系下的坐标，Pworld代表物体在世界坐标系下的坐标，R和T代表了将点的从世界坐标系下映射到相机坐标系下，可以知道solvePnP求出的刚好是这样的映射关系。
使Pcam = 0，则意味着物体移到了相机坐标系的原点，求出来的Pworld代表了相机在世界坐标系中的位置，P的z轴坐标就是深度信息。
0=RPworld+T
P = -inverse(R)*T

代码为：

	//Mat到Eigen格式转换
	Eigen::Matrix3f R_n;
	Eigen::Vector3f T_n;
	cv2eigen(rotMat, R_n);
	cv2eigen(tvec, T_n);
	Eigen::Vector3f P_oc;

	P_oc = -R_n.inverse()*T_n;
	cout << "世界坐标" << P_oc << std::endl;

其中cv2eigen需要头文件添加：（顺序不能错！！！先包含eigen相关库，再包含opencv库！）

#include

#include

---

2、法二：

1、根据旋转矩阵求出坐标旋转角。

	double r11 = rotM.ptr(0)[0];
	double r12 = rotM.ptr(0)[1];
	double r13 = rotM.ptr(0)[2];
	double r21 = rotM.ptr(1)[0];
	double r22 = rotM.ptr(1)[1];
	double r23 = rotM.ptr(1)[2];
	double r31 = rotM.ptr(2)[0];
	double r32 = rotM.ptr(2)[1];
	double r33 = rotM.ptr(2)[2];
/*************************************此处计算出相机的旋转角**********************************************/
	//计算出相机坐标系的三轴旋转欧拉角，旋转后可以转出世界坐标系。
	//旋转顺序为z、y、x
	//原理见帖子：
	double thetaz = atan2(r21, r11) / CV_PI * 180;
	double thetay = atan2(-1 * r31, sqrt(r32*r32 + r33*r33)) / CV_PI * 180;
	double thetax = atan2(r32, r33) / CV_PI * 180;

2、平移矩阵反向旋转得到相机在世界坐标系下坐标。

	/*************************************此处计算出相机坐标系原点Oc在世界坐标系中的位置**********************************************/
	/* 当原始坐标系经过旋转z、y、x三次旋转后，会与世界坐标系完全平行，而三次旋转中向量OcOw会跟着旋转 */
	/* 而我们想知道的是两个坐标系完全平行时，OcOw的值 */
	/* 因此，原始坐标系每次旋转完成后，对向量OcOw进行一次反相旋转，最终可以得到两个坐标系完全平行时的OcOw */
	/* 该向量乘以-1就是世界坐标系下相机的坐标 */
	/***********************************************************************************/

	//提出平移矩阵，表示从相机坐标系原点，跟着向量(x,y,z)走，就到了世界坐标系原点
	double tx = tvec.ptr(0)[0];
	double ty = tvec.ptr(0)[1];
	double tz = tvec.ptr(0)[2];

	//x y z 为唯一向量在相机原始坐标系下的向量值
	//也就是向量OcOw在相机坐标系下的值
	double x = tx, y = ty, z = tz;

	//进行三次反向旋转
	codeRotateByZ(x, y, -1 * thetaz, x, y);
	codeRotateByY(x, z, -1 * thetay, x, z);
	codeRotateByX(y, z, -1 * thetax, y, z);


	//获得相机在世界坐标系下的位置坐标
	//即向量OcOw在世界坐标系下的值
	double Cx = x*-1;
	double Cy = y*-1;
	double Cz = z*-1;

重点参考：

http://www.cnblogs.com/singlex/p/pose_estimation_1.html

---

---

3、法1附录源代码（可运行通过）

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include "opencv2/core/core.hpp"
#include "opencv2/highgui/highgui.hpp"
#include 
#include 
using namespace std;
using namespace cv;



int main()
{

	/****************a6000参数**********************/
	//初始化相机参数Opencv
	double camD[9] = {
		6800.7, 0, 3065.8,
		0, 6798.1, 1667.6,
		0, 0, 1 };
	cv::Mat camera_matrix = cv::Mat(3, 3, CV_64FC1, camD);

	//畸变参数
	double distCoeffD[5] = { -0.189314, 0.444657, -0.00116176, 0.00164877, -2.57547 };
	cv::Mat distortion_coefficients = cv::Mat(5, 1, CV_64FC1, distCoeffD);

	//P1-P4为XOY面上的共面点其Z坐标为0，P5的Z坐标不为0

	//测试用图1 DSC03321
	vector Points2D;
	Points2D.push_back(cv::Point2f(2985, 1688));	//P1
	Points2D.push_back(cv::Point2f(5081, 1690));	//P2
	Points2D.push_back(cv::Point2f(2997, 2797));	//P3
	//Points2D.push_back(cv::Point2f(5544, 2757));	//P4
	Points2D.push_back(cv::Point2f(4148, 673));	//P5

	//特征点世界坐标
	vector Points3D;
	Points3D.push_back(cv::Point3f(0, 0, 0));		//P1 三维坐标的单位是毫米
	Points3D.push_back(cv::Point3f(0, 200, 0));		//P2
	Points3D.push_back(cv::Point3f(150, 0, 0));		//P3
	//Points3D.push_back(cv::Point3f(150, 200, 0));	//P4
	Points3D.push_back(cv::Point3f(0, 100, 105));	//P5

	//初始化输出矩阵
	cv::Mat rvec;
	cv::Mat tvec;

	//三种方法求解
	//solvePnP(Points3D, Points2D, camera_matrix, distortion_coefficients, rvec, tvec, false, CV_ITERATIVE);	//实测迭代法似乎只能用4个共面特征点求解，5个点或非共面4点解不出正确的解
	solvePnP(Points3D, Points2D, camera_matrix, distortion_coefficients, rvec, tvec, false, CV_P3P);			//Gao的方法可以使用任意四个特征点，特征点数量不能少于4也不能多于4
	//solvePnP(Points3D, Points2D, camera_matrix, distortion_coefficients, rvec, tvec, false, CV_EPNP);			//该方法可以用于N点位姿估计

	cv::Mat rotMat;
	cv::Rodrigues(rvec, rotMat);  //由于solvePnP返回的是旋转向量，故用罗德里格斯变换变成旋转矩阵

	//Mat到Eigen格式转换
	Eigen::Matrix3f R_n;
	Eigen::Vector3f T_n;
	cv2eigen(rotMat, R_n);
	cv2eigen(tvec, T_n);
	Eigen::Vector3f P_oc;

	P_oc = -R_n.inverse()*T_n;
	cout << "世界坐标" << P_oc << std::endl;


	return 0;
}