/*
例程基于VS2013開發(fā)，使用的是OpenCV2.4.X，大家運行前需要將opencv的路徑重新配置成自己電腦上的，不懂的話參考我的博客《OpenCV2+入門系列（一）：OpenCV2.4.9的安裝與測試》。例程中提供兩張照片，其中DSC03323就是“實驗”中所用圖片，例程在計算完成后，會在D盤根目錄下生成兩個txt，分別存儲：相機在世界坐標系的坐標、相機的三個旋轉(zhuǎn)角。
@2016-11-18
@Shawan
@http://www.cnblogs.com/singlex/
*/


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

//將空間點繞Z軸旋轉(zhuǎn)
//輸入?yún)?shù)?x?y為空間點原始x?y坐標
//thetaz為空間點繞Z軸旋轉(zhuǎn)多少度，角度制范圍在-180到180
//outx?outy為旋轉(zhuǎn)后的結(jié)果坐標
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軸旋轉(zhuǎn)
//輸入?yún)?shù)?x?z為空間點原始x?z坐標
//thetay為空間點繞Y軸旋轉(zhuǎn)多少度，角度制范圍在-180到180
//outx?outz為旋轉(zhuǎn)后的結(jié)果坐標
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軸旋轉(zhuǎn)
//輸入?yún)?shù)?y?z為空間點原始y?z坐標
//thetax為空間點繞X軸旋轉(zhuǎn)多少度，角度制，范圍在-180到180
//outy?outz為旋轉(zhuǎn)后的結(jié)果坐標
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;
}


//點繞任意向量旋轉(zhuǎn)，右手系
//輸入?yún)?shù)old_x，old_y，old_z為旋轉(zhuǎn)前空間點的坐標
//vx，vy，vz為旋轉(zhuǎn)軸向量
//theta為旋轉(zhuǎn)角度角度制，范圍在-180到180
//返回值為旋轉(zhuǎn)后坐標點
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參數(shù)**********************/
	//初始化相機參數(shù)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）;

	//畸變參數(shù)
	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：根據(jù)四個特征點估計相機姿態(tài)?隨文Demo\
?????文件?????1064603??2016-11-19?14:46??相機位姿估計1：根據(jù)四個特征點估計相機姿態(tài)?隨文Demo\DSC03321.jpg
?????文件?????1564775??2016-11-19?14:46??相機位姿估計1：根據(jù)四個特征點估計相機姿態(tài)?隨文Demo\DSC03323.jpg
?????目錄???????????0??2016-11-19?14:44??相機位姿估計1：根據(jù)四個特征點估計相機姿態(tài)?隨文Demo\SlovePNPByOpenCV\
?????文件?????????994??2016-10-31?15:36??相機位姿估計1：根據(jù)四個特征點估計相機姿態(tài)?隨文Demo\SlovePNPByOpenCV.sln
?????文件????????5062??2016-11-05?21:55??相機位姿估計1：根據(jù)四個特征點估計相機姿態(tài)?隨文Demo\SlovePNPByOpenCV\SlovePNPByOpenCV.vcxproj
?????文件????????1323??2016-11-03?16:06??相機位姿估計1：根據(jù)四個特征點估計相機姿態(tài)?隨文Demo\SlovePNPByOpenCV\SlovePNPByOpenCV.vcxproj.filters
?????文件?????????412??2016-11-05?21:55??相機位姿估計1：根據(jù)四個特征點估計相機姿態(tài)?隨文Demo\SlovePNPByOpenCV\SlovePNPByOpenCV.vcxproj.user
?????文件????????9111??2016-11-20?20:59??相機位姿估計1：根據(jù)四個特征點估計相機姿態(tài)?隨文Demo\SlovePNPByOpenCV\SolvePNPByOpenCV.cpp
?????文件?????????222??2016-10-31?15:36??相機位姿估計1：根據(jù)四個特征點估計相機姿態(tài)?隨文Demo\SlovePNPByOpenCV\stdafx.cpp
?????文件?????????234??2016-10-31?15:36??相機位姿估計1：根據(jù)四個特征點估計相機姿態(tài)?隨文Demo\SlovePNPByOpenCV\stdafx.h
?????文件?????????236??2016-10-31?15:36??相機位姿估計1：根據(jù)四個特征點估計相機姿態(tài)?隨文Demo\SlovePNPByOpenCV\targetver.h
?????文件?????????378??2016-11-19?14:39??相機位姿估計1：根據(jù)四個特征點估計相機姿態(tài)?隨文Demo\說明.txt