opencv Farneback光流法

#include  
#include "opencv2/opencv.hpp" 

using namespace cv;  
using namespace std;

#define UNKNOWN_FLOW_THRESH 1e9  

static void makecolorwheel(vector &colorwheel)  
{  
	int RY = 15;  
	int YG = 6;  
	int GC = 4;  
	int CB = 11;  
	int BM = 13;  
	int MR = 6;  

	int i;  

	for (i = 0; i < RY; i++) colorwheel.push_back(Scalar(255,       255*i/RY,     0));  
	for (i = 0; i < YG; i++) colorwheel.push_back(Scalar(255-255*i/YG, 255,       0));  
	for (i = 0; i < GC; i++) colorwheel.push_back(Scalar(0,         255,      255*i/GC));  
	for (i = 0; i < CB; i++) colorwheel.push_back(Scalar(0,         255-255*i/CB, 255));  
	for (i = 0; i < BM; i++) colorwheel.push_back(Scalar(255*i/BM,      0,        255));  
	for (i = 0; i < MR; i++) colorwheel.push_back(Scalar(255,       0,        255-255*i/MR));  
}  

static void motionToColor(Mat flow, Mat &color)  
{  
	static vector colorwheel; //Scalar r,g,b  
	if (colorwheel.empty())  
		makecolorwheel(colorwheel);  

	// determine motion range:  
	float maxrad = -1;  

	// Find max flow to normalize fx and fy  
	for (int i= 0; i < flow.rows; ++i)   
	{  
		for (int j = 0; j < flow.cols; ++j)   
		{  
			Vec2f flow_at_point = flow.at(i, j);  //flow:x,y
			float fx = flow_at_point[0];  
			float fy = flow_at_point[1];  
			if ((fabs(fx) >  UNKNOWN_FLOW_THRESH) || (fabs(fy) >  UNKNOWN_FLOW_THRESH))  
				continue;  
			float rad = sqrt(fx * fx + fy * fy);  
			maxrad = maxrad > rad ? maxrad : rad;  
		}  
	}  

	for (int i= 0; i < flow.rows; ++i)   
	{  
		for (int j = 0; j < flow.cols; ++j)   
		{  
			uchar *data = color.data + color.step[0] * i + color.step[1] * j;  
			Vec2f flow_at_point = flow.at(i, j);  

			float fx = flow_at_point[0] / maxrad;  
			float fy = flow_at_point[1] / maxrad;	//归一化
			if ((fabs(fx) >  UNKNOWN_FLOW_THRESH) || (fabs(fy) >  UNKNOWN_FLOW_THRESH))  
			{  
				data[0] = data[1] = data[2] = 0;	//BGR
				continue;  
			}  
			float rad = sqrt(fx * fx + fy * fy);	//速率

			float angle = atan2(-fy, -fx) / CV_PI;	//夹角 
			float fk = (angle + 1.0) / 2.0 * (colorwheel.size()-1);  
			int k0 = (int)fk;  
			int k1 = (k0 + 1) % colorwheel.size();  
			float f = fk - k0;  
			//f = 0; // uncomment to see original color wheel  

			for (int b = 0; b < 3; b++)   
			{  
				float col0 = colorwheel[k0][b] / 255.0;  
				float col1 = colorwheel[k1][b] / 255.0;  
				float col = (1 - f) * col0 + f * col1;  
				if (rad <= 1)  
					col = 1 - rad * (1 - col); // increase saturation with radius  
				else  
					col *= .75; // out of range  

				data[2 - b] = (int)(255.0 * col);  
			}  
		}  
	}  
}  

int main()
{
	const char* videoFileName = "D:/FarnebackInGPU/myProject/myProject/dataset/MAH00054.MP4";
	//const char* videoFileName = "optical_flow_input.avi";
	VideoCapture cap;
	//string strFileName(videoFileName);
	//cap.open(0);  
	cap.open(videoFileName);  

	if( !cap.isOpened() )  
		return 1;  

	Mat prevgray, gray, flow, cflow, frame;  
	namedWindow("flow", 1); 

	bool isFirstFrame = true;
	Mat motion2color; 

	while (true)
	{
		double t = (double)cvGetTickCount();  
		cap >> frame;  

		if (!frame.data) break;

		while (frame.cols > 800)
		{
			resize(frame, frame, Size(frame.cols / 2, frame.rows / 2));
		}

		cvtColor(frame, gray, CV_BGR2GRAY); 
		imshow(videoFileName, frame);		//显示当前帧

		if (isFirstFrame)
		{
			motion2color.create(gray.rows, gray.cols, CV_8UC3);  //BGR
			isFirstFrame = false;
		}
		else
		{
			calcOpticalFlowFarneback(prevgray, gray, flow, 0.5, 3, 15, 3, 5, 1.2, 0);  
			motionToColor(flow, motion2color); 
			imshow("flow", motion2color); 

			t = (double)cvGetTickCount() - t;  
			cout << "cost time: " << t / ((double)cvGetTickFrequency() * 1000 * 1000)<<"s." << endl; 
		}
		std::swap(prevgray, gray);

		char ch = waitKey(1);
		if(ch == 27) return 1;
		else if(ch > -1) waitKey(0);
	}

	return 0;
}