void arg_parse( int argc, char** argv )
{int i = 0;/*extract program name from command line (remove path, if present) */pname = remove_path( argv[0] );/*parse commandline options */while( TRUE ){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;      /* catch invalid arguments */default:fatal_error( "-%c: invalid option/nTry '%s -h' for help.",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];
}

IplImage* bgr2hsv( IplImage* bgr )
{
IplImage* bgr32f, * hsv;bgr32f = cvCreateImage( cvGetSize(bgr), IPL_DEPTH_32F, 3 );
hsv = cvCreateImage( cvGetSize(bgr), IPL_DEPTH_32F, 3 );
cvConvertScale( bgr, bgr32f, 1.0 / 255.0, 0 );
cvCvtColor( bgr32f, hsv, CV_BGR2HSV );
cvReleaseImage( &bgr32f );
return hsv;
}

/*Computes a reference histogram for each of the object regions defined bythe user@param frame video frame in which to compute histograms; should have beenconverted to hsv using bgr2hsv in observation.h@param regions regions of /a frame over which histograms should be computed@param n number of regions in /a regions@param export if TRUE, object region images are exported@return Returns an /a n element array of normalized histograms correspondingto regions of /a frame specified in /a regions.
*/
histogram** compute_ref_histos( IplImage* frame, CvRect* regions, int n )
{histogram** histos = malloc( n * sizeof( histogram* ) );IplImage* tmp;int i;/* extract each region from frame and compute its histogram */for( i = 0; i < n; i++ ){cvSetImageROI( frame, regions[i] );//set the region of interesttmp = cvCreateImage( cvGetSize( frame ), IPL_DEPTH_32F, 3 );cvCopy( frame, tmp, NULL );cvResetImageROI( frame );//free the ROIhistos[i] = calc_histogram( &tmp, 1 );//calculate the hisrogramnormalize_histogram( histos[i] );//Normalizes a histogram so all bins sum to 1.0cvReleaseImage( &tmp );}return histos;
}

/*Calculates a cumulative histogram as defined above for a given arrayof images@param img an array of images over which to compute a cumulative histogram;each must have been converted to HSV colorspace using bgr2hsv()@param n the number of images in imgs@return Returns an un-normalized HSV histogram for /a imgs
*/
histogram* calc_histogram( IplImage** imgs, int n )
{IplImage* img;histogram* histo;IplImage* h, * s, * v;float* hist;int i, r, c, bin;histo = malloc( sizeof(histogram) );histo->n = NH*NS + NV;hist = histo->histo;memset( hist, 0, histo->n * sizeof(float) );for( i = 0; i < n; i++ ){/* extract individual HSV planes from image */img = imgs[i];h = cvCreateImage( cvGetSize(img), IPL_DEPTH_32F, 1 );s = cvCreateImage( cvGetSize(img), IPL_DEPTH_32F, 1 );v = cvCreateImage( cvGetSize(img), IPL_DEPTH_32F, 1 );cvCvtPixToPlane( img, h, s, v, NULL );/* increment appropriate histogram bin for each pixel */for( r = 0; r < img->height; r++ )for( c = 0; c < img->width; c++ ){bin = histo_bin( pixval32f( h, r, c ),pixval32f( s, r, c ),pixval32f( v, r, c ) );hist[bin] += 1;}cvReleaseImage( &h );cvReleaseImage( &s );cvReleaseImage( &v );}return histo;
}

/*
Creates an initial distribution of particles at specified locations@param regions an array of regions describing player locations around
which particles are to be sampled
@param histos array of histograms describing regions in /a regions
@param n the number of regions in /a regions
@param p the total number of particles to be assigned@return Returns an array of /a p particles sampled from around regions in
/a regions
*/
particle* init_distribution( CvRect* regions, histogram** histos, int n, int p)
{
particle* particles;
int np;
float x, y;
int i, j, width, height, k = 0;particles = malloc( p * sizeof( particle ) );
np = p / n;/* create particles at the centers of each of n regions */
for( i = 0; i < n; i++ )
{
width = regions[i].width;
height = regions[i].height;
x = regions[i].x + width / 2;
y = regions[i].y + height / 2;
for( j = 0; j < np; j++ )
{
particles[k].x0 = particles[k].xp = particles[k].x = x;
particles[k].y0 = particles[k].yp = particles[k].y = y;
particles[k].sp = particles[k].s = 1.0;
particles[k].width = width;
particles[k].height = height;
particles[k].histo = histos[i];
particles[k++].w = 0;
}
}/* make sure to create exactly p particles */
i = 0;
while( k < p )
{
width = regions[i].width;
height = regions[i].height;
x = regions[i].x + width / 2;
y = regions[i].y + height / 2;
particles[k].x0 = particles[k].xp = particles[k].x = x;
particles[k].y0 = particles[k].yp = particles[k].y = y;
particles[k].sp = particles[k].s = 1.0;
particles[k].width = width;
particles[k].height = height;
particles[k].histo = histos[i];
particles[k++].w = 0;
i = ( i + 1 ) % n;
}return particles;
}

/*Computes the likelihood of there being a player at a given location inan image@param img image that has been converted to HSV colorspace using bgr2hsv()@param r row location of center of window around which to compute likelihood@param c col location of center of window around which to compute likelihood@param w width of region over which to compute likelihood@param h height of region over which to compute likelihood@param ref_histo reference histogram for a player; must have beennormalized with normalize_histogram()@return Returns the likelihood of there being a player at location(/a r, /a c) in /a img
*/
float likelihood( IplImage* img, int r, int c,int w, int h, histogram* ref_histo )
{IplImage* tmp;histogram* histo;float d_sq;/* extract region around (r,c) and compute and normalize its histogram */cvSetImageROI( img, cvRect( c - w / 2, r - h / 2, w, h ) );tmp = cvCreateImage( cvGetSize(img), IPL_DEPTH_32F, 3 );cvCopy( img, tmp, NULL );cvResetImageROI( img );histo = calc_histogram( &tmp, 1 );cvReleaseImage( &tmp );normalize_histogram( histo );/* compute likelihood as e^{/lambda D^2(h, h^*)} */d_sq = histo_dist_sq( histo, ref_histo );free( histo );return exp( -LAMBDA * d_sq );
}

/*Computes squared distance metric based on the Battacharyya similaritycoefficient between histograms.@param h1 first histogram; should be normalized@param h2 second histogram; should be normalized@return Returns a squared distance based on the Battacharyya similaritycoefficient between /a h1 and /a h2
*/
float histo_dist_sq( histogram* h1, histogram* h2 )
{float* hist1, * hist2;float sum = 0;int i, n;n = h1->n;hist1 = h1->histo;hist2 = h2->histo;/*According the the Battacharyya similarity coefficient,D = /sqrt{ 1 - /sum_1^n{ /sqrt{ h_1(i) * h_2(i) } } }*/for( i = 0; i < n; i++ )sum += sqrt( hist1[i]*hist2[i] );return 1.0 - sum;
}

/*Samples a transition model for a given particle@param p a particle to be transitioned@param w video frame width@param h video frame height@param rng a random number generator from which to sample@return Returns a new particle sampled based on <EM>p</EM>'s transitionmodel
*/
particle transition( particle p, int w, int h, gsl_rng* rng )
{float x, y, s;particle pn;/* sample new state using second-order autoregressive dynamics */x = A1 * ( p.x - p.x0 ) + A2 * ( p.xp - p.x0 ) +B0 * gsl_ran_gaussian( rng, TRANS_X_STD ) + p.x0;pn.x = MAX( 0.0, MIN( (float)w - 1.0, x ) );y = A1 * ( p.y - p.y0 ) + A2 * ( p.yp - p.y0 ) +B0 * gsl_ran_gaussian( rng, TRANS_Y_STD ) + p.y0;pn.y = MAX( 0.0, MIN( (float)h - 1.0, y ) );s = A1 * ( p.s - 1.0 ) + A2 * ( p.sp - 1.0 ) +B0 * gsl_ran_gaussian( rng, TRANS_S_STD ) + 1.0;pn.s = MAX( 0.1, s );pn.xp = p.x;pn.yp = p.y;pn.sp = p.s;pn.x0 = p.x0;pn.y0 = p.y0;pn.width = p.width;pn.height = p.height;pn.histo = p.histo;pn.w = 0;return pn;
}

/*Re-samples a set of particles according to their weights to produce anew set of unweighted particles@param particles an old set of weighted particles whose weights have beennormalized with normalize_weights()@param n the number of particles in /a particles@return Returns a new set of unweighted particles sampled from /a particles
*/
particle* resample( particle* particles, int n )
{particle* new_particles;int i, j, np, k = 0;qsort( particles, n, sizeof( particle ), &particle_cmp );new_particles = malloc( n * sizeof( particle ) );for( i = 0; i < n; i++ ){np = cvRound( particles[i].w * n );for( j = 0; j < np; j++ ){new_particles[k++] = particles[i];if( k == n )goto exit;}}while( k < n )new_particles[k++] = particles[0];exit:return new_particles;
}

/*Normalizes particle weights so they sum to 1@param particles an array of particles whose weights are to be normalized@param n the number of particles in /a particles
*/
void normalize_weights( particle* particles, int n )
{float sum = 0;int i;for( i = 0; i < n; i++ )sum += particles[i].w;for( i = 0; i < n; i++ )particles[i].w /= sum;
}