通过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 <<