TEXT

//类的初始化
	QRCodeDetector QRCode;//opencv4二维码
	MonocularRanging mono("../QR_test/cameraParams.xml");//单目测距类
	ChooseTarget Chooser(20, 10);//选择目标
	ChooseTarget::Target target;
	BallisticCalculation Ballistic(5, 25000, 100, 9806);//弹道计算
	mono.SetTargetSize(62, 62);
	//图像获取
	VideoCapture CAP;
	CAP.open("/dev/video0", CAP_V4L2);
	CAP.set(CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G'));
	CAP.set(CAP_PROP_FRAME_HEIGHT, 480);
	CAP.set(CAP_PROP_FRAME_WIDTH, 640);
	CAP.set(CAP_PROP_AUTO_WB, 1);//白平衡
	//    CAP.set(CAP_PROP_AUTO_EXPOSURE, 0);
	//    CAP.set(CAP_PROP_EXPOSURE, 121);
	CAP.set(CAP_PROP_FPS, 120);

	if (!CAP.isOpened())
	{
		cout << "cap error" << endl;
		exit(0);
	}
	Mat Src;

	vector<Point2f> Points;
	SpatialLocation location;//6维空间坐标系类
	Sphe_COORD Spherical;//SphericalCoordiantes 球量
	Rect_COORD Trans;//SpaceRectangularCoordiantes 矩形空间量

	double time1 = cv::getTickCount();//时间
	int count = 0, _count = 0;

	while (true)
	{
		CAP >> Src;
		if (QRCode.detect(Src, Points))
		{
			line(Src, Points[0], Points[1], Scalar(0, 255, 0));
			line(Src, Points[1], Points[2], Scalar(0, 255, 0));
			line(Src, Points[2], Points[3], Scalar(0, 255, 0));
			line(Src, Points[3], Points[0], Scalar(0, 255, 0));

			mono.GetSpatialLocation(location, Points);//得到目标空间位置 6维空间坐标系类
			Spherical = location;//球量<<6维空间坐标系类
			Trans = Spherical;//矩形空间量<<球量

			/*getTickCount()：用于返回从操作系统启动到当前所经的计时周期数，看名字也很好理解，get Tick Count(s)。
			getTickFrequency()：用于返回CPU的频率。get Tick Frequency。这里的单位是秒，也就是一秒内重复的次数。
			*/
			//将目标压入链表
			Chooser.PushTarget(ChooseTarget::Target(location, ((double)getTickCount() / getTickFrequency())));


			//printf("X: %+3.02lf\tY: %+3.02lf\tZ: %+3.02lf\n", target.x, target.y, target.z);
			Points.clear();

			//            printf("X: %+3.02lf/%+3.02lf\tY: %+3.02lf/%+3.02lf\tZ: %+3.02lf/%+3.02lf\n",
			//                   location.x, Trans.x,
			//                   location.y, Trans.y,
			//                   location.z, Trans.z);

						//printf("phi: %+3.02lf theta: %+3.02lf r: %+3.02lf z: %+3.02lf\n", Ran2Deg(Spherical.phi), Ran2Deg(Spherical.theta), Spherical.r, location.z);

						//printf("P: %+3.02lf Y: %+3.02lf R: %+3.02lf\n", location.Pitch, location.Yaw, location.Roll);

			putText(Src, to_string(location.z), Points[0], FONT_HERSHEY_TRIPLEX, 1, Scalar(0, 0, 255), 1);
		}


		if (Chooser.GetTarget(target))//如果得到目标
		{
			double vx, vy, vz;//x轴，y轴，z轴的速度引用
			target.GetSpeed(vx, vy, vz);//得到x，y，z速度
			//printf("X: %+3.02lf/%+3.02lf\tY: %+3.02lf/%+3.02lf\tZ: %+3.02lf/%+3.02lf\n", target.x, vx, target.y, vy, target.z, vz);
			Ballistic.GetAngle(target);//弹道的预测及计算
		}

		putText(Src, "FPS:" + to_string(_count), Point(0, 25), FONT_HERSHEY_TRIPLEX, 1, Scalar(0, 0, 255), 1);
		imshow("Src", Src);
		waitKey(1);

		//计算FPS
		count++;
		if ((cv::getTickCount() - time1) / cv::getTickFrequency() >= 1)
		{
			time1 = cv::getTickCount();
			_count = count;
			count = 0;
		}
	}
	return 0;