#include?“opencv2/opencv.hpp“
#include?“iostream“

using?namespace?cv;
using?namespace?std;


int?h_min?=?75;
int?s_min?=?75;
int?v_min?=?75;
int?h_max?=?177;
int?s_max?=?195;
int?v_max?=?177;

Scalar?hsv_min（h_min?s_min?v_min）;
Scalar?hsv_max（h_max?s_max?v_max）;


void?getTarget2dPoinstion（const?cv::RotatedRect?&?rect?vector?&?target2d?const?cv::Point2f?&?offset）?{
	Point2f?vertices[4];
	rect.points（vertices）;//把矩形的四個點復制給四維點向量
	Point2f?lu?ld?ru?rd;
	sort（vertices?vertices?+?4?[]（const?Point2f?&?p1?const?Point2f?&?p2）?{?return?p1.x?	if?（vertices[0].y?		lu?=?vertices[0];
		ld?=?vertices[1];
	}
	else?{
		lu?=?vertices[1];
		ld?=?vertices[0];
	}
	if?（vertices[2].y?		ru?=?vertices[2];
		rd?=?vertices[3];
	}
	else?{
		ru?=?vertices[3];
		rd?=?vertices[2];
	}

	target2d.clear（）;
	target2d.push_back（lu?+?offset）;
	target2d.push_back（ru?+?offset）;
	target2d.push_back（rd?+?offset）;
	target2d.push_back（ld?+?offset）;
}
vector?Generate3DPoints（）
{
	std::vector?points;
	float?x?y?z;
	x?=?0;?y?=?0;?z?=?0;
	points.push_back（cv::Point3f（x?y?z））;

	x?=?12;?y?=?0;?z?=?0;
	points.push_back（cv::Point3f（x?y?z））;

	x?=?12;	y?=?3;?z?=?0;
	points.push_back（cv::Point3f（x?y?z））;

	x?=?0;?y?=?3;?z?=?0;
	points.push_back（cv::Point3f（x?y?z））;

	//for?（unsigned?int?i?=?0;?i?	//{
	//	cout?<	//}

	return?points;
}
//將空間點繞Z軸旋轉
//輸入參數?x?y為空間點原始x?y坐標
//thetaz為空間點繞Z軸旋轉多少度，角度制范圍在-180到180
//outx?outy為旋轉后的結果坐標
static?void?CodeRotateByZ（double?x?double?y?double?thetaz?double&?outx?double&?outy）
{
	double?x1?=?x;//將變量拷貝一次，保證&x?==?&outx這種情況下也能計算正確
	double?y1?=?y;
	double?rz?=?thetaz?*?3.14?/?180;
	outx?=?cos（rz）?*?x1?-?sin（rz）?*?y1;
	outy?=?sin（rz）?*?x1?+?cos（rz）?*?y1;
}
static?void?CodeRotateByY（double?x?double?z?double?thetay?double&?outx?double&?outz）
{
	double?x1?=?x;
	double?z1?=?z;
	double?ry?=?thetay?*?3.14?/?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為旋轉后的結果坐標
static?void?CodeRotateByX（double?y?double?z?double?thetax?double&?outy?double&?outz）
{
	double?y1?=?y;//將變量拷貝一次，保證&y?==?&y這種情況下也能計算正確
	double?z1?=?z;
	double?rx?=?thetax?*?3.14?/?180;
	outy?=?cos（rx）?*?y1?-?sin（rx）?*?z1;
	outz?=?cos（rx）?*?z1?+?sin（rx）?*?y1;
}
void?main（）
{
	VideoCapture?cap（0）;
	Mat?objPM;//三維點矩陣
	vector?objectPoints?=?Generate3DPoints（）;//三維坐標點
	vector?projectedPoints;//三維點投影到二維點的向量用于重畫
	Mat（objectPoints）.convertTo（objPM?CV_32F）;//把三維點向量變成三維點矩陣
	double?cameraD[3][3]?=?{?{?412.92673??0?318.52471?}
	{?0?412.27311???258.65883?}
	{?0?0??1.0000?}?};//通過matlab標定的單相機內參
	double?distC[5]?=?{?0.24775?-?0.50118?-?0.00513???0.00706??0.00000?};//通過matlab獲取的相機畸變參數
	Mat?cameraMatrix（3?3?cv::Da

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----

????..A..H.?????27648??2018-03-15?16:06??姿態解算8--成功測距\.vs\姿態解算8\v14\.suo

?????文件?????236032??2018-03-15?12:22??姿態解算8--成功測距\x64\Debug\姿態解算8.exe

?????文件????2720560??2018-03-15?12:22??姿態解算8--成功測距\x64\Debug\姿態解算8.ilk

?????文件????4091904??2018-03-15?12:22??姿態解算8--成功測距\x64\Debug\姿態解算8.pdb

?????文件?????733583??2018-03-15?09:51??姿態解算8--成功測距\姿態解算8\Debug\main.obj

?????文件????1100800??2018-03-15?09:51??姿態解算8--成功測距\姿態解算8\Debug\vc140.idb

?????文件????2183168??2018-03-15?09:51??姿態解算8--成功測距\姿態解算8\Debug\vc140.pdb

?????文件???????1935??2018-03-15?09:51??姿態解算8--成功測距\姿態解算8\Debug\姿態解算8.log

?????文件????????682??2018-03-15?09:51??姿態解算8--成功測距\姿態解算8\Debug\姿態解算8.tlog\CL.command.1.tlog

?????文件??????33122??2018-03-15?09:51??姿態解算8--成功測距\姿態解算8\Debug\姿態解算8.tlog\CL.read.1.tlog

?????文件????????532??2018-03-15?09:51??姿態解算8--成功測距\姿態解算8\Debug\姿態解算8.tlog\CL.write.1.tlog

?????文件??????????2??2018-03-15?09:51??姿態解算8--成功測距\姿態解算8\Debug\姿態解算8.tlog\link.command.1.tlog

?????文件??????????2??2018-03-15?09:51??姿態解算8--成功測距\姿態解算8\Debug\姿態解算8.tlog\link.read.1.tlog

?????文件??????????2??2018-03-15?09:51??姿態解算8--成功測距\姿態解算8\Debug\姿態解算8.tlog\link.write.1.tlog

?????文件??????????0??2018-03-15?09:51??姿態解算8--成功測距\姿態解算8\Debug\姿態解算8.tlog\unsuccessfulbuild

?????文件????????227??2018-03-15?09:51??姿態解算8--成功測距\姿態解算8\Debug\姿態解算8.tlog\姿態解算8.lastbuildstate

?????文件???????7395??2018-03-15?12:22??姿態解算8--成功測距\姿態解算8\main.cpp

?????文件????1097569??2018-03-15?12:22??姿態解算8--成功測距\姿態解算8\x64\Debug\main.obj

?????文件????1158144??2018-03-15?12:22??姿態解算8--成功測距\姿態解算8\x64\Debug\vc140.idb

?????文件????2265088??2018-03-15?12:22??姿態解算8--成功測距\姿態解算8\x64\Debug\vc140.pdb

?????文件????????800??2018-03-15?12:22??姿態解算8--成功測距\姿態解算8\x64\Debug\姿態解算8.log

?????文件????????650??2018-03-15?12:22??姿態解算8--成功測距\姿態解算8\x64\Debug\姿態解算8.tlog\CL.command.1.tlog

?????文件??????33568??2018-03-15?12:22??姿態解算8--成功測距\姿態解算8\x64\Debug\姿態解算8.tlog\CL.read.1.tlog

?????文件????????556??2018-03-15?12:22??姿態解算8--成功測距\姿態解算8\x64\Debug\姿態解算8.tlog\CL.write.1.tlog

?????文件???????2710??2018-03-15?12:22??姿態解算8--成功測距\姿態解算8\x64\Debug\姿態解算8.tlog\link.command.1.tlog

?????文件???????7806??2018-03-15?12:22??姿態解算8--成功測距\姿態解算8\x64\Debug\姿態解算8.tlog\link.read.1.tlog

?????文件????????542??2018-03-15?12:22??姿態解算8--成功測距\姿態解算8\x64\Debug\姿態解算8.tlog\link.write.1.tlog

?????文件????????225??2018-03-15?12:22??姿態解算8--成功測距\姿態解算8\x64\Debug\姿態解算8.tlog\姿態解算8.lastbuildstate

?????文件???????7383??2018-03-11?19:14??姿態解算8--成功測距\姿態解算8\姿態解算8.vcxproj

?????文件????????945??2018-03-11?19:14??姿態解算8--成功測距\姿態解算8\姿態解算8.vcxproj.filters

............此處省略18個文件信息