相机位姿估计1：根据四个特征点估计相机姿态 随文Demo

相机位姿估计1：根据四个特征点估计相机姿态 随文Demo 使用opencv基于特征点估计位姿

/*
例程基于VS2013开发，使用的是OpenCV2.4.X，大家运行前需要将opencv的路径重新配置成自己电脑上的，不懂的话参考我的博客《OpenCV2+入门系列（一）：OpenCV2.4.9的安装与测试》。例程中提供两张照片，其中DSC03323就是“实验”中所用图片，例程在计算完成后，会在D盘根目录下生成两个txt，分别存储：相机在世界坐标系的坐标、相机的三个旋转角。
@2016-11-18
@Shawan
@http://www.cnblogs.com/singlex/
*/


#include “stdafx.h“
#include 
#include 
#include 
#include 
using namespace std;

//将空间点绕Z轴旋转
//输入参数 x y为空间点原始x y坐标
//thetaz为空间点绕Z轴旋转多少度，角度制范围在-180到180
//outx outy为旋转后的结果坐标
void codeRotateByZ（double x double y double thetaz double& outx double& outy）
{
	double x1 = x;//将变量拷贝一次，保证&x == &outx这种情况下也能计算正确
	double y1 = y;
	double rz = thetaz * CV_PI / 180;
	outx = cos（rz） * x1 - sin（rz） * y1;
	outy = sin（rz） * x1 + cos（rz） * y1;
}

//将空间点绕Y轴旋转
//输入参数 x z为空间点原始x z坐标
//thetay为空间点绕Y轴旋转多少度，角度制范围在-180到180
//outx outz为旋转后的结果坐标
void codeRotateByY（double x double z double thetay double& outx double& outz）
{
	double x1 = x;
	double z1 = z;
	double ry = thetay * CV_PI / 180;
	outx = cos（ry） * x1 + sin（ry） * z1;
	outz = cos（ry） * z1 - sin（ry） * x1;
}

//将空间点绕X轴旋转
//输入参数 y z为空间点原始y z坐标
//thetax为空间点绕X轴旋转多少度，角度制，范围在-180到180
//outy outz为旋转后的结果坐标
void codeRotateByX（double y double z double thetax double& outy double& outz）
{
	double y1 = y;//将变量拷贝一次，保证&y == &y这种情况下也能计算正确
	double z1 = z;
	double rx = thetax * CV_PI / 180;
	outy = cos（rx） * y1 - sin（rx） * z1;
	outz = cos（rx） * z1 + sin（rx） * y1;
}


//点绕任意向量旋转，右手系
//输入参数old_x，old_y，old_z为旋转前空间点的坐标
//vx，vy，vz为旋转轴向量
//theta为旋转角度角度制，范围在-180到180
//返回值为旋转后坐标点
cv::Point3f RotateByVector（double old_x double old_y double old_z double vx double vy double vz double theta）
{
	double r = theta * CV_PI / 180;
	double c = cos（r）;
	double s = sin（r）;
	double new_x = （vx*vx*（1 - c） + c） * old_x + （vx*vy*（1 - c） - vz*s） * old_y + （vx*vz*（1 - c） + vy*s） * old_z;
	double new_y = （vy*vx*（1 - c） + vz*s） * old_x + （vy*vy*（1 - c） + c） * old_y + （vy*vz*（1 - c） - vx*s） * old_z;
	double new_z = （vx*vz*（1 - c） - vy*s） * old_x + （vy*vz*（1 - c） + vx*s） * old_y + （vz*vz*（1 - c） + c） * old_z;
	return cv::Point3f（new_x new_y new_z）;
}
void test（）;

int main（int argc _TCHAR* argv[]）
{
	vector Points2D;
	/****************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）;


	//DSC03321
	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

	////DSC03323
	//Points2D.push_back（cv::Point2f（3062 3073））;		//P1 单位是像素
	//Points2D.push_back（cv::Point2f（38

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2016-11-19 14:46  相机位姿估计1：根据四个特征点估计相机姿态 随文Demo\
     文件     1064603  2016-11-19 14:46  相机位姿估计1：根据四个特征点估计相机姿态 随文Demo\DSC03321.jpg
     文件     1564775  2016-11-19 14:46  相机位姿估计1：根据四个特征点估计相机姿态 随文Demo\DSC03323.jpg
     目录           0  2016-11-19 14:44  相机位姿估计1：根据四个特征点估计相机姿态 随文Demo\SlovePNPByOpenCV\
     文件         994  2016-10-31 15:36  相机位姿估计1：根据四个特征点估计相机姿态 随文Demo\SlovePNPByOpenCV.sln
     文件        5062  2016-11-05 21:55  相机位姿估计1：根据四个特征点估计相机姿态 随文Demo\SlovePNPByOpenCV\SlovePNPByOpenCV.vcxproj
     文件        1323  2016-11-03 16:06  相机位姿估计1：根据四个特征点估计相机姿态 随文Demo\SlovePNPByOpenCV\SlovePNPByOpenCV.vcxproj.filters
     文件         412  2016-11-05 21:55  相机位姿估计1：根据四个特征点估计相机姿态 随文Demo\SlovePNPByOpenCV\SlovePNPByOpenCV.vcxproj.user
     文件        9111  2016-11-20 20:59  相机位姿估计1：根据四个特征点估计相机姿态 随文Demo\SlovePNPByOpenCV\SolvePNPByOpenCV.cpp
     文件         222  2016-10-31 15:36  相机位姿估计1：根据四个特征点估计相机姿态 随文Demo\SlovePNPByOpenCV\stdafx.cpp
     文件         234  2016-10-31 15:36  相机位姿估计1：根据四个特征点估计相机姿态 随文Demo\SlovePNPByOpenCV\stdafx.h
     文件         236  2016-10-31 15:36  相机位姿估计1：根据四个特征点估计相机姿态 随文Demo\SlovePNPByOpenCV\targetver.h
     文件         378  2016-11-19 14:39  相机位姿估计1：根据四个特征点估计相机姿态 随文Demo\说明.txt