main.c

不错的particle filter的程序

/* File: main.c
 * ------------------
 * call the tracking procedures.
 */

#include <time.h>
#include "track.h"

/***************************** Function Prototypes ***************************/
int getopt(int Argc, char **Argv, char *Str);
void usage( char* );
void arg_parse( int, char** );

/********************************** Globals **********************************/

char* pname;                      /* program name */
char* vid_file;                   /* input video file name */
int num_particles = PARTICLES;    /* number of particles */
int show_all = FALSE;             /* TRUE to display all particles */
int export = FALSE;               /* TRUE to exported tracking sequence */

int optind = 1;
char *optarg;

/* Main */

int main( int argc, char** argv )
{
	gsl_rng* rng;
	IplImage* frame, *hsv_frame, * frames[MAX_FRAMES], *last_frame, *diff_frame, *b_frame, *c_frame;
//  IplImage** hsv_ref_imgs;
	histogram **ref_histos, **test_histos;
	CvCapture* video;
	particle* particles, * new_particles;
	CvScalar color;
	CvRect *regions, *test_regions, test_rect;
	CvSize size;
	CvVideoWriter *out, *motion;	//export
	float weight, sum;
	double test_dis, test_l_dis;
	FILE *maskfile, *distfile;

	time_t t;
	int num_objects = 0;
	float sw, sh;
	int i, j, i_update=0, test_i;
	CvPoint vector;
	CvRect recta, rectb;
	char name[100];

  /* parse command line and initialize random number generator */
	arg_parse( argc, argv );
//  gsl_rng_env_setup();	//including this sentance disable debugging
	rng = gsl_rng_alloc( gsl_rng_mt19937 );
	t=time(NULL);
	printf("t=%ld\n", t);
	gsl_rng_set( rng, t);

	video = cvCaptureFromFile( vid_file );
	if( ! video )
    fatal_error("couldn't open video file %s", vid_file);
    
	i = 0;
	while( frame = cvQueryFrame( video ) )
	{
		printf("frame %d\n", i);
		hsv_frame = bgr2hsv( frame );	//convert RGB to HSV, as the observasion is based on HSV
		frames[i] = (IplImage *)cvClone( frame );

		/* allow user to select object to be tracked in the first frame */
		if( i == 0 )	//the first few frame
		{
			maskfile=fopen("mask.y", "wb");
			distfile=fopen("distance.txt", "w");
			size=cvGetSize(frame);
			b_frame=cvCreateImage(cvGetSize(frame), 8, 1);
			c_frame=cvCreateImage(cvGetSize(frame), 8, 1);
			diff_frame=cvCreateImage(size, 8, 1);
			last_frame=cvCreateImage(size, 8, 3);
			diff_frame->origin=1;
			c_frame->origin=1;
			if( export )
			{
				out=cvCreateVideoWriter("out.avi", -1, 25, size, 1);
				motion=cvCreateVideoWriter("motion.avi", -1, 25, size, 1);
			}
			fprintf( stderr, "Select object region to track\n" );
			while( num_objects == 0 )
			{
				num_objects = get_regions( frame, &regions );
				if( num_objects == 0 )
				{
					fprintf( stderr, "Please select a object\n" );
				}
			}

			/* compute reference histograms and distribute particles */
			ref_histos = compute_ref_histos( hsv_frame, regions, num_objects );
//			if( export )
//			export_ref_histos( ref_histos, num_objects );
			export_histogram(ref_histos[0], "ref_histos.txt");
			particles = init_distribution( regions, ref_histos,	num_objects, num_particles );
		}
		else
		{
			//motion cue
			cvCvtColor(last_frame, b_frame, CV_BGR2GRAY);
			cvCvtColor(frame, c_frame, CV_BGR2GRAY);
			cvSetZero(diff_frame);
			vector=MotionCompensation(b_frame, c_frame, size.width, size.height, L, H);
			InitRect(vector.x, vector.y, size.width, size.height, &recta, &rectb);
			cvSetImageROI(b_frame, recta);
			cvSetImageROI(c_frame, rectb);
			cvSetImageROI(diff_frame, rectb);
			cvAbsDiff(c_frame, b_frame, diff_frame);
			Concentrate(diff_frame, c_frame, recta.width, recta.height);//size.width, size.height);//
			cvResetImageROI(b_frame);
			cvResetImageROI(c_frame);
			cvResetImageROI(diff_frame);
//			cvWaitKey(0);

			/* perform prediction and measurement for each particle */
			for( j = 0; j < num_particles; j++ )
			{
				particles[j] = transition( particles[j], size.width, size.height, rng );
				sw = particles[j].sw;
				sh = particles[j].sh;
				weight=likelihood( hsv_frame, cvRound(particles[j].y), cvRound(particles[j].x),
							cvRound(particles[j].width*sw), cvRound(particles[j].height*sh), particles[j].histo, c_frame);
				particles[j].w=weight;
                //become larger quickly, become small slowly
				//particles[j].w+=weight/particles[j].w*(weight-particles[j].w);
			}

			/* normalize weights and resample a set of unweighted particles */
			sum=normalize_weights( particles, num_particles );

//			if(i+1==502 || i+1==514 || i+1==520 || i+1==524)
//				DisplayDistribution(particles, sum, num_particles, "Distribution", i);

/*			if(i-i_update>UPDATE_FRAMES)	//update
			{
				if(TryUpdateHistogram(particles, num_particles, ref_histos[0])==1)	//update one histogram
				{
					test_i=1;
					test_l_dis=0;
					test_rect=cvRect(0,0,1,1);
//					display_particle( frames[i], particles[0], color );	//i'th frame with p[0]
					while(1)	//test for histogram
					{
//						cvReleaseImage(&frame);
						frame=(IplImage *)cvClone(frames[i]);
						cvRectangle(frame, cvPoint(test_rect.x, test_rect.y),
							cvPoint(test_rect.x+test_rect.width, test_rect.y+test_rect.height), CV_RGB(255, 255, 0), 1, 8, 0 );	//most likely
						test_i=get_regions(frame, &test_regions);
						if(test_i==0) break;
						test_histos=compute_ref_histos(hsv_frame, test_regions, 1);
						DisplayHistogram(ref_histos[0], test_histos[0], "test_histogram");
						export_histogram(test_histos[0], "test_histogram.txt");
						test_dis=histo_dist_sq(ref_histos[0], test_histos[0]);
						printf("dist^2=%f\n", test_dis);
						test_dis=exp( -LAMBDA * test_dis);
						printf("weight=%f\n", test_dis);
						if(test_dis>test_l_dis)
						{
							test_l_dis=test_dis;
							test_rect=test_regions[0];
						}
					}//end test
					i_update=i;
					UpdateHisto(hsv_frame, cvRound(particles[0].y), cvRound(particles[0].x),
								cvRound(particles[0].width*particles[0].sw), cvRound(particles[0].height*particles[0].sh),
								particles[0].histo);
					sprintf(name, "update%d_histos.txt", i);
					export_histogram(ref_histos[0], name);

					printf("Update, i=%d\n", i);
				}
			}//end update
*/
			new_particles = resample( particles, num_particles );
			free( particles );
			particles = new_particles;
		}

		/* display all particles if requested */
		qsort( particles, num_particles, sizeof( particle ), &particle_cmp );	//sort according to weight of particles
		DisplayParticleHistogram(ref_histos[0], hsv_frame, cvRound(particles[0].y), cvRound(particles[0].x),
								cvRound(particles[0].width*particles[0].sw), cvRound(particles[0].height*particles[0].sh),
								"Histogram", distfile);
		if( show_all )
			for( j = num_particles - 1; j > 0; j-- )
			{
				color = CV_RGB(0,0,255);
				display_particle( frames[i], particles[j], color );
			}
	    
		/* display most likely particle */
		color = CV_RGB(255,0,0);
		display_particle( frames[i], particles[0], color );	//according to particle_cmp, particles[0] is the most likely
		cvNamedWindow( "Video", 1 );
		cvShowImage( "Video", frames[i] );
		cvCopyImage(frame, last_frame);
			display_particle(c_frame, particles[0], cvScalar(250,250,250,250));
			cvNamedWindow("difference", 1);
			cvShowImage("difference", c_frame);
		if( export )
		{
//			MaskParticleImage(frame, particles[0], maskfile);	//export
			cvWriteFrame(out, frames[i]);
			cvWriteFrame(motion, c_frame);
//			cvNamedWindow( "Mask", 1 );
//			cvShowImage( "Mask", frame);
		}
		cvWaitKey( 5 );

		cvReleaseImage( &hsv_frame );
		cvReleaseImage( &frames[i] );
		i++;	//count number of frames
	}//end while
	cvReleaseCapture( &video );
	cvReleaseImage(&b_frame);
	cvReleaseImage(&c_frame);
	cvReleaseImage(&diff_frame);
	fclose(maskfile);
	fclose(distfile);
  if( export )
  {
	  cvReleaseVideoWriter(&out);
	  cvReleaseVideoWriter(&motion);
  }
}



/* print usage for this program */
void usage( char* name )
{
  fprintf(stderr, "%s: track a single object using particle filtering\n\n",
	  name);
  fprintf(stderr, "Usage: %s [options] <vid_file>\n\n", name);
  fprintf(stderr, "Arguments:\n");
  fprintf(stderr, "  <vid_file>          A clip of video in which " \
	  "to track an object\n");
  fprintf(stderr, "\nOptions:\n");
  fprintf(stderr, "  -h                  Display this message and exit\n");
  fprintf(stderr, "  -a                  Display all particles, not just " \
	  "the most likely\n");
  fprintf(stderr, "  -o                  Output tracking sequence frames as " \
	  "%s*%s\n", EXPORT_BASE, EXPORT_EXTN);
  fprintf(stderr, "  -p <particles>      Number of particles (default %d)\n",
	  PARTICLES);
}



/*
  arg_parse() parses the command line arguments, setting appropriate globals.
  
  argc and argv should be passed directly from the command line
*/
void arg_parse( int argc, char** argv )
{
//  int i = 0;
  int end=0;
  /*extract program name from command line (remove path, if present) */
  pname = remove_path( argv[0] );

  /*parse commandline options */
  while( !end )
    {
      char* arg_check;
      int arg = getopt( argc, argv, OPTIONS );
      if( arg == -1 )
	break;

      switch( arg )
	{
	  /* user asked for help */
	case 'h':
	  usage( pname );
	  exit(0);
	  break;
	  
	case 'a':
	  show_all = TRUE;
	  break;

	  /* user wants to output tracking sequence */
	case 'o':
	  export = TRUE;
	  break;

	  /* user wants to set number of particles */
	case 'p':
	  if( ! optarg )
	    fatal_error( "error parsing arguments at -%c\n"	\
			 "Try '%s -h' for help.", arg, pname );
	  num_particles = strtol( optarg, &arg_check, 10 );
	  if( arg_check == optarg  ||  *arg_check != '\0' )
	    fatal_error( "-%c option requires an integer argument\n"	\
			 "Try '%s -h' for help.", arg, pname );
	  break;
	case '?': optind--; end=1; break;	//argument begin without '-', is the input out put name, by Waks
	  
	  /* catch invalid arguments */
	default:
	  fatal_error( "-%c: invalid option\nTry '%s -h' for help.",
		       arg/*optopt*/, pname );
	}
    }
  
  /* make sure input and output files are specified */
  if( argc - optind < 1 )
    fatal_error( "no input image specified.\nTry '%s -h' for help.", pname );
  if( argc - optind > 2 )
    fatal_error( "too many arguments.\nTry '%s -h' for help.", pname );
 
  /* record video file name */
  vid_file = argv[optind];
}

///////////////////////////

int getopt(int Argc, char **Argv, char *Str)
{
    int Optchar;
    char *Option;

    if ( optind >= Argc ) return EOF;

    Option = Argv[optind++];

    if ( *Option++ != '-' ) return '?';

    Optchar = *Option++;

    while ( *Str && *Str != Optchar ) Str++;
    if ( ! *Str ) return '?';

    if ( *++Str == ':' )
    {
	if ( *Option ) optarg = Option;
	else
	if ( optind < Argc ) optarg = Argv[optind++];
	else
	Optchar = '?';
    }

    return Optchar;
}