ctrack.cpp

国外一个大牛人写的MEAN-SHIFT目标跟踪算法

        //derive the weigths
        calculate_weights( );
        //Calculate the new location of the target
        determine_new_candidate_location();

        for (i=0;i<2*candidate_window_radius+1;i++)
        	delete FLIR_weights[i];
        delete FLIR_weights;
//--------------------------------------------------------------------
        //update the distrubution for the target at the new location
        candidate_density_function_p();
        //evaluate the updated bhattacharyya coefficient
        bhattacharyya_coefficient_2 = bhattacharyya_coefficient();

        inner_iter_cnt=0;
        while (bhattacharyya_coefficient_2-bhattacharyya_coefficient_1 < -0.01 && inner_iter_cnt<30)
        {
			candidate_center = (candidate_center+target_center) / 2;
        	//update the distrubution for the target at the new candidate location
        	candidate_density_function_p();
        	//evaluate the updated bhattacharyya coefficient
        	bhattacharyya_coefficient_2 = bhattacharyya_coefficient();
            inner_iter_cnt++;
        }
        //Calculate the distance between two vectors
    	distance = candidate_center.distance(target_center);
    	target_center = candidate_center;
        iter_cnt++;
    }
    while ( distance >= 1 && iter_cnt<30);
    return bhattacharyya_coefficient_2;
}


int frameNumber = 0;
int code = 0;
bool pause = false;
int window_radiusDynamic;
int centerXMouse, centerYMouse;
bool changeCenter;

//this module allows mouse input
//if left button clicked then if number typed the left mouse
//coordinates are used as new center location
//if after left mouse button the r key is pressed then you can type a new radius
//size in the command prompt
//the right mouse button toggles between pause and unpause
//while paused you may press the - or = key to move backwards or forwards manually
void on_mouse( int event, int x, int y, int flags )
{

	char command[500];
	int code;

    switch( event )
    {
    case CV_EVENT_LBUTTONDOWN:
        {
			
			//now get event code
			
			code = cvWaitKey(0);
			if ( isdigit(code) ) {
			
				//store mouse coordinates
				changeCenter = true;
				centerXMouse = x;
				centerYMouse = y;

			}
			else if ( code == 'r' ) {
				
				scanf("%d",&window_radiusDynamic);
				
				
			}
        }
        break;
	case CV_EVENT_RBUTTONDOWN:
        {
			
            pause = !pause;
			
        }
        break;
    }
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// algorithm for the estimated location of the target in video
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void CTrack::track_target
( 	char	*input_file,
	char	*output_directory,
	char	*frame_name,
	char	*frame_type,
	int 	file_start_index,
    int 	file_end_index,
    CPixel 	center_of_target,
    int		window_radius,
    int		number_of_color_bins
)
{
	//create open cv window
	changeCenter = false;
	window_radiusDynamic = window_radius;
	char* m_windowName = (char*) (malloc(sizeof(char)*100));
	strcpy(m_windowName,"Output Window");
	cvNamedWindow(m_windowName,CV_WINDOW_AUTOSIZE);

	cvSetMouseCallback( m_windowName, on_mouse );
	
	char 	filename[1000];
    int     frame,i;
    int 	min_scale,scale;
    float	min_bhattacharyya;
    float 	bhattacharyya_distastance;
    float	temp_candidate_window_radius;
    CPixel 	temp_center,correct_center;
	//initialize the variables
	target_center 			= center_of_target;
    model_window_radius		= window_radiusDynamic;
	number_of_bins			= number_of_color_bins;
    //allocate resources
	FLIR_model_color_probabilities    	 =new float*[number_of_bins];
	for (i=0;i<number_of_bins;i++)
		FLIR_model_color_probabilities[i]=new float[3];
    FLIR_candidate_color_probabilities	 =new float*[number_of_bins];
	for (i=0;i<number_of_bins;i++)
		FLIR_candidate_color_probabilities[i]=new float[3];
    //Generate the target model from the first frame
	
    
	
	CvCapture* capture= cvCaptureFromAVI(input_file);
	
	IplImage* image;
	int frameCount = cvGetCaptureProperty( capture, CV_CAP_PROP_FRAME_COUNT);
	cvSetCaptureProperty(capture,CV_CAP_PROP_POS_FRAMES,file_start_index);
	image= cvQueryFrame(  capture );
	cvFlip(image,NULL,0);
	
	FLIR_image.convert(image);
	
    
    //calculate the distrubution for the model ahead of time
    model_density_function_q();
#if (TRACKING_OUTPUT)
	
	cvCircle(image,cvPoint((int)target_center.x,(int)target_center.y),model_window_radius,255);
    sprintf(filename,"%s%s%05d.%s",output_directory,frame_name,file_start_index,frame_type);
    
	cvFlip(image,NULL,0);
	cvSaveImage(filename,image);
	FILE *cmd_out;
	cmd_out = fopen("cmd_output.txt", "w");
#endif

	
	//track the target in the other frames
    for (frameNumber=file_start_index;frameNumber<file_end_index;)
    {
    	
		
    
		//handle mouse IO
		if ( changeCenter ) {
			target_center.x = centerXMouse;
			target_center.y = centerYMouse;
			model_density_function_q();
			changeCenter = false;
		}
		if ( !pause ) {
			image= cvQueryFrame(  capture );
			if ( image == 0 ) {
				//no more frames
				break;
			}
			cvFlip(image,NULL,0);
			FLIR_image.convert(image);
			frameNumber++;
		}
		else 
		{
			cvFlip(image,NULL,0);
			code = cvWaitKey(1000);
			if ( code == '-' ) {
				frameNumber--;
				cvSetCaptureProperty(capture,CV_CAP_PROP_POS_FRAMES,frameNumber-1);
				image= cvQueryFrame(  capture );
				cvFlip(image,NULL,0);
				FLIR_image.convert(image);
			}
			else if ( code == '=' ) {
				frameNumber++;
				cvSetCaptureProperty(capture,CV_CAP_PROP_POS_FRAMES,frameNumber-1);
				image= cvQueryFrame(  capture );
				cvFlip(image,NULL,0);
				FLIR_image.convert(image);
			}
		
		}
    	model_window_radius = window_radiusDynamic;    	
    	candidate_window_radius	= model_window_radius;
        //track the target for 3 scales
        temp_candidate_window_radius=candidate_window_radius;

		min_scale=0;
    	min_bhattacharyya=1e+10;
        temp_center = target_center;

        for (scale=0;scale<=0;scale++)
        {   target_center = temp_center;
        	if (temp_candidate_window_radius+scale>2 &&
            	temp_candidate_window_radius+scale<120)
            {   candidate_window_radius   = (int)temp_candidate_window_radius+scale;
        		bhattacharyya_distastance = exp(-.3*abs(scale))*sqrt(1-track_target_in_consecutive_frames());
            	if (bhattacharyya_distastance<min_bhattacharyya)
            	{   min_scale 			= scale;
                	min_bhattacharyya	= bhattacharyya_distastance;
                    correct_center		= target_center;
            	}
            }
		}
        target_center		 = correct_center;
        model_window_radius  = (int)(temp_candidate_window_radius+min_scale);

#if (TRACKING_OUTPUT)
		
		cvCircle(image,cvPoint((int)target_center.x,(int)target_center.y),model_window_radius,255);
        
		cvCircle(image,cvPoint((int)target_center.x,(int)target_center.y),3,CV_RGB(255,0,0));
    	sprintf(filename,"%s%s%05d.%s",output_directory,frame_name,frameNumber,frame_type);        
		cvFlip(image,NULL,0);
		cvShowImage(m_windowName,image);
		cvWaitKey(1);
		cvSaveImage(filename,image);        
		fprintf(cmd_out, "%05d %f %f %d\n", frameNumber, target_center.x, target_center.y,model_window_radius);

#endif

	
	}

	//delete allocated resources
	cvReleaseCapture(&capture);
	delete FLIR_model_color_probabilities;
    delete FLIR_candidate_color_probabilities;
#if (TRACKING_OUTPUT)
	fclose(cmd_out);
#endif
}