目录
一、Opencv函数使用
二、目标是求得相机在世界坐标系下的3D坐标。
1、法一：
2、法二：
3、法1附录源代码（可运行通过）
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);利用求得的旋转矩阵和平移矩阵：
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 <Eigen/Core>
#include <opencv2/core/eigen.hpp>
1、根据旋转矩阵求出坐标旋转角。
double r11 = rotM.ptr<double>(0)[0];
double r12 = rotM.ptr<double>(0)[1];
double r13 = rotM.ptr<double>(0)[2];
double r21 = rotM.ptr<double>(1)[0];
double r22 = rotM.ptr<double>(1)[1];
double r23 = rotM.ptr<double>(1)[2];
double r31 = rotM.ptr<double>(2)[0];
double r32 = rotM.ptr<double>(2)[1];
double r33 = rotM.ptr<double>(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<double>(0)[0];
double ty = tvec.ptr<double>(0)[1];
double tz = tvec.ptr<double>(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
#include <stdio.h>
#include <tchar.h>
#include <opencv2\opencv.hpp>
#include <math.h>
#include <iostream>
#include <fstream>
#include <Eigen/Core>
#include <Eigen/LU>
#include "opencv2/core/core.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <opencv2/core/eigen.hpp>
#include <Eigen/Dense>
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<cv::Point2f> 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<cv::Point3f> 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;
}