optical_flow_klt.cpp

KLT方法对特征点进行跟踪

//////////////////////////////////////////////////
//实现特征点的跟踪
//利用函数cvGoodFeaturesToTrack
///////////////////////////////////////////////////
#include <stdio.h>
#include <cv.h>
#include <highgui.h>
#include <math.h>

/******************************/
//注意：这里N为要跟踪的特征点个数，通过修改N的值即可
static const int N= 100;
/******************************/

static const double pi = 3.14159265358979323846;

inline static double square(int a)
{
	return a * a;
}

//定义内联函数，减少栈内存的消耗 定义的图像的大小，深度和信道（channels）
inline static void allocateOnDemand( IplImage **img, CvSize size, int depth, int channels )
{
	if ( *img != NULL )	return;

	*img = cvCreateImage( size, depth, channels );
	if ( *img == NULL )
	{
		fprintf(stderr, "Error: Couldn't allocate image.  Out of memory?\n");
		exit(-1);
	}
}

int main(void)
{

//	CvCapture *input_video = cvCaptureFromCAM(0);
    CvCapture *input_video = cvCaptureFromFile("urban.avi");
//	CvCapture *input_video = cvCaptureFromFile("SampleVideo.avi");

	if (input_video == NULL)
	{
		fprintf(stderr, "Error: Can't open video.\n");
		return -1;
	}

	/* 读取视频（AVI）帧图像的大小 */
	CvSize frame_size;
	frame_size.height =
		(int) cvGetCaptureProperty( input_video, CV_CAP_PROP_FRAME_HEIGHT );
	frame_size.width =
		(int) cvGetCaptureProperty( input_video, CV_CAP_PROP_FRAME_WIDTH );

	/* 设定视频AVI的帧数 */
	long number_of_frames;

	/* Go to the end of the AVI (ie: the fraction is "1") */
	cvSetCaptureProperty( input_video, CV_CAP_PROP_POS_AVI_RATIO, 1. );
	//CV_CAP_PROP_POS_AVI_RATIO表示视频文件的相对位置(0 - 影片的开始，1 - 影片的结尾)

	/* Now that we're at the end, read the AVI position in frames */
	number_of_frames = (int) cvGetCaptureProperty( input_video, CV_CAP_PROP_POS_FRAMES );
	//CV_CAP_PROP_POS_FRAMES - 将被下一步解压／获取的帧索引，以0为起点
	
	/* Return to the beginning */
	/*CV_CAP_PROP_POS_FRAMES - 单位为帧数的位置（只对视频文件有效）*/
	cvSetCaptureProperty( input_video, CV_CAP_PROP_POS_FRAMES, 0. );

	/* Create a windows called "Optical Flow KLT" for visualizing the output.*/
	cvNamedWindow("Optical Flow KLT", CV_WINDOW_AUTOSIZE);

	long current_frame = 0;
	while(true)
	{
		static IplImage *frame = NULL, *frame1 = NULL, *frame1_1C = NULL, *frame2_1C = NULL,
			*eig_image = NULL, *temp_image = NULL, *pyramid1 = NULL, *pyramid2 = NULL;

	
		cvSetCaptureProperty( input_video, CV_CAP_PROP_POS_FRAMES, current_frame );

		/* Get the next frame of the video*/
		frame = cvQueryFrame( input_video );
		if (frame == NULL)
		{
			fprintf(stderr, "Error: Hmm. The end came sooner than we thought.\n");
			return -1;
		}
		/* Allocate another image if not already allocated.
		 * Image has ONE channel of color (ie: monochrome) with 8-bit "color" depth.
		 */
		allocateOnDemand( &frame1_1C, frame_size, IPL_DEPTH_8U, 1 );
		/* Convert whatever the AVI image format is into OpenCV's preferred format.
		 * AND flip the image vertically.  Flip is a shameless hack.  OpenCV reads
		 * in AVIs upside-down by default. 
		 */
		//CV_CVTIMG_FLIP - 垂直翻转图像
		cvConvertImage(frame, frame1_1C, CV_CVTIMG_FLIP);

		/* We'll make a full color backup of this frame so that we can draw on it.
		 * (It's not the best idea to draw on the static memory space of cvQueryFrame().)
		 */
		allocateOnDemand( &frame1, frame_size, IPL_DEPTH_8U, 3 );
		cvConvertImage(frame, frame1, CV_CVTIMG_FLIP);

		/* Get the second frame of video.  Same principles as the first. */
		frame = cvQueryFrame( input_video );
		if (frame == NULL)
		{
			fprintf(stderr, "Error: Hmm. The end came sooner than we thought.\n");
			return -1;
		}
		allocateOnDemand( &frame2_1C, frame_size, IPL_DEPTH_8U, 1 );
		cvConvertImage(frame, frame2_1C, CV_CVTIMG_FLIP);

		/* Shi and Tomasi Feature Tracking! */

		/* Preparation: Allocate the necessary storage. */
		allocateOnDemand( &eig_image, frame_size, IPL_DEPTH_32F, 1 );
		allocateOnDemand( &temp_image, frame_size, IPL_DEPTH_32F, 1 );

		/* Preparation: This array will contain the features found in frame 1. */
		//设置第一帧的特征点数目
		CvPoint2D32f frame1_features[N];

		int number_of_features;		
		number_of_features = N;

		/* Actually run the Shi and Tomasi algorithm!!
		 * "frame1_1C" is the input image.
		 * "eig_image" and "temp_image" are just workspace for the algorithm.
		 * The first ".01" specifies the minimum quality of the features (based on the eigenvalues).
		 * The second ".01" specifies the minimum Euclidean distance between features.
		 * "NULL" means use the entire input image.  You could point to a part of the image.
		 * WHEN THE ALGORITHM RETURNS:
		 * "frame1_features" will contain the feature points.
		 * "number_of_features" will be set to a value <= 100 indicating the number of feature points found.
		 */
		/* quality_level =.02
			最大最小特征值的乘法因子。定义可接受图像角点的最小质量因子。 
			min_distance =10.0
			限制因子。得到的角点的最小距离。使用 Euclidian 距离 
		*/
		//number_of_features参数值为小于frame1_features[100]
		//函数 cvGoodFeaturesToTrack 在图像中寻找具有大特征值的角点
		
		cvGoodFeaturesToTrack(frame1_1C, eig_image, temp_image, frame1_features, 
			&number_of_features, .02, 10.0, NULL);

		/* Pyramidal Lucas Kanade Optical Flow! */

		/* This array will contain the locations of the points from frame 1 in frame 2. */
		//设置第一帧的特征点数目
		CvPoint2D32f frame2_features[N];

		char optical_flow_found_feature[N];

		/* The i-th element of this array is the error in the optical flow for the i-th feature
		 * of frame1 as found in frame 2. */
		float optical_flow_feature_error[N];

		/* This is the window size to use to avoid the aperture problem  */
		CvSize optical_flow_window = cvSize(3,3);
		
		/* This termination criteria tells the algorithm to stop when it has either done 20 iterations or when
		 * epsilon is better than .3.  You can play with these parameters for speed vs. accuracy but these values
		 * work pretty well in many situations.
		 */
		CvTermCriteria optical_flow_termination_criteria
			= cvTermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, .3 );

		/* This is some workspace for the algorithm.
		 * (The algorithm actually carves the image into pyramids of different resolutions.)
		 */
		allocateOnDemand( &pyramid1, frame_size, IPL_DEPTH_8U, 1 );
		allocateOnDemand( &pyramid2, frame_size, IPL_DEPTH_8U, 1 );

		/* Actually run Pyramidal Lucas Kanade Optical Flow!!
		 * "frame1_1C" is the first frame with the known features.
		 * "frame2_1C" is the second frame where we want to find the first frame's features.
		 * "pyramid1" and "pyramid2" are workspace for the algorithm.
		 * "frame1_features" are the features from the first frame.
		 * "frame2_features" is the (outputted) locations of those features in the second frame.//下一帧特征点
		 * "number_of_features" is the number of features in the frame1_features array.
		 * "optical_flow_window" is the size of the window to use to avoid the aperture problem.//孔径问题
		 * "5" is the maximum number of pyramids to use.  0 would be just one level.//图像金字塔
		 * "optical_flow_found_feature" is as described above (non-zero if feature found by the flow).
		 * "optical_flow_feature_error" is as described above (error in the flow for this feature).
		 * "optical_flow_termination_criteria" is as described above (how long the algorithm should look).
		 * "0" means disable enhancements.  (For example, the second array isn't pre-initialized with guesses.)
		 */
		//光流参数设置（重点看！！！）
		cvCalcOpticalFlowPyrLK(frame1_1C, frame2_1C, pyramid1, pyramid2, frame1_features, frame2_features,
			number_of_features, optical_flow_window, 5, optical_flow_found_feature, 
			optical_flow_feature_error, optical_flow_termination_criteria, 0 );
		
		/* For fun , let's draw the flow field. */
		for(int i = 0; i < number_of_features; i++)
		{
			/* If Pyramidal Lucas Kanade didn't really find the feature, skip it. */
			//optical_flow_found_feature数组
			//如果对应特征的光流被发现，数组中的每一个元素都被设置为1，否则设置为0 
			if ( optical_flow_found_feature[i] == 0 )	continue;

			int line_thickness;				line_thickness = 1;
			/* CV_RGB(red, green, blue) is the red, green, and blue components
			 * of the color you want, each out of 255.*/	
			CvScalar line_color;			
			line_color = CV_RGB(255,0,0);
	
			/* Let's make the flow field look nice with arrows. */

			/* The arrows will be a bit too short for a nice visualization because of the high framerate
			 * (ie: there's not much motion between the frames).  So let's lengthen them by a factor of 3.
			 */
			CvPoint p,q;
			p.x = (int) frame1_features[i].x;
			p.y = (int) frame1_features[i].y;
			q.x = (int) frame2_features[i].x;
			q.y = (int) frame2_features[i].y;

			double angle;		angle = atan2( (double) p.y - q.y, (double) p.x - q.x );
			double hypotenuse;	hypotenuse = sqrt( square(p.y - q.y) + square(p.x - q.x) );

			/* Here we lengthen the arrow by a factor of three. */
			q.x = (int) (p.x - 3 * hypotenuse * cos(angle));
			q.y = (int) (p.y - 3 * hypotenuse * sin(angle));

			/* Now we draw the main line of the arrow. */
			/* "frame1" is the frame to draw on.
			 * "p" is the point where the line begins.
			 * "q" is the point where the line stops.
			 * "CV_AA" means antialiased drawing.
			 * "0" means no fractional bits in the center cooridinate or radius.
			 */
			cvLine( frame1, p, q, line_color, line_thickness, CV_AA, 0 );
			/* Now draw the tips of the arrow.  I do some scaling so that the
			 * tips look proportional to the main line of the arrow.
			 */	
			//设置箭头（没有什么实质性意义）
			p.x = (int) (q.x + 6 * cos(angle + pi / 4));
			p.y = (int) (q.y + 6 * sin(angle + pi / 4));
			cvLine( frame1, p, q, line_color, line_thickness, CV_AA, 0 );
			p.x = (int) (q.x + 6 * cos(angle - pi / 4));
			p.y = (int) (q.y + 6 * sin(angle - pi / 4));
			cvLine( frame1, p, q, line_color, line_thickness, CV_AA, 0 );
		}

		cvShowImage("Optical Flow KLT", frame1);
       
	    // 按键的设置，为了观察
		int key_pressed;
		key_pressed = cvWaitKey(0);

		/* 按b或B帧后退，按a或A帧前进，按s或S程序结束*/
		if (key_pressed == 'b' || key_pressed == 'B')	current_frame--;
		else if(key_pressed == 'a' || key_pressed == 'A')	{current_frame++; cvSaveImage("feature.jpg", frame1);}
		        else if(key_pressed == 's' || key_pressed == 'S')      break;
				
		/*限定帧的运行范围.*/
		if (current_frame < 0)						current_frame = 0;
		if (current_frame >= number_of_frames - 1)	current_frame = number_of_frames - 2;
	}
	return 0;
}