云台控制

#include <opencv2/opencv.hpp>
using namespace cv;
using namespace std;

int histSize[1] = {256};
float hranges[2] = {0, 255};
const float* ranges[1] = {hranges};
int channels[1] = {0};

int num = 0;
int flagud = 0;
int flaglr = 0;

Point cpt, rcpt;

//OpenCV貌似也没有获取矩形中心点的功能，还是自己写一个
Point getCenterPoint(Rect rect)
{
	Point cpt;
	cpt.x = rect.x + cvRound(rect.width/2.0);
	cpt.y = rect.y + cvRound(rect.height/2.0);
	return cpt;
}

int main()
{
	VideoCapture cap(1);

	Mat frame, frameHSV, imgROI, dstHist, backproj;
	Rect rect(100, 100, 100, 100);

	while (true)
	{
		++num;
		cap >> frame;
		if (frame.empty()) break;

		if (num == 1)
		{
			cpt.x = cvRound(frame.cols/2.0);
			cpt.y = cvRound(frame.rows/2.0);

			cvtColor(frame, frameHSV, COLOR_RGB2HSV);
			imgROI = frameHSV(rect);
			calcHist(&imgROI, 1, channels, Mat(), dstHist, 1, histSize, ranges);
			normalize(dstHist, dstHist, 0.0, 1.0, NORM_MINMAX);

			rectangle(frame, rect, Scalar(0, 0, 255), 2);
			imshow("Frame", frame);
			waitKey(30);
		} 
		else
		{
			num = 1;
			cvtColor(frame, frameHSV, COLOR_RGB2HSV);
			calcBackProject(&frameHSV, 1, channels, dstHist, backproj, ranges, 255.0);
			imshow("Backproj", backproj);

			TermCriteria criteria(TermCriteria::MAX_ITER + TermCriteria::EPS, 1000, 0.001);
			meanShift(backproj, rect, criteria);

			imgROI = frameHSV(rect);
			calcHist(&imgROI, 1, channels, Mat(), dstHist, 1, histSize, ranges);
			normalize(dstHist, dstHist, 0.0, 1.0, NORM_MINMAX);

			rectangle(frame, rect, Scalar(0, 0, 255), 2);

			rcpt = getCenterPoint(rect);//调整云台
			if (rcpt.x < cpt.x-10)
				flaglr = 1;
			else if (rcpt.x > cpt.x+10)
				flaglr = -1;
			else
				flaglr = 0;

			if (rcpt.y > cpt.y+10)
				flagud = 1;
			else if (rcpt.y < cpt.y-10)
				flagud = -1;
			else
				flagud = 0;

			imshow("Frame", frame);
			waitKey(30);
		}
	}
	return 0;
}