meanshift_tracker.c

学习跟踪的好程序

	bhatblock(modelhist, histlen, indeximage, nrows, ncols,
		row, row+1, col, col+1,
		hrow, hcol, NGItemp, &bhatmin, &bhatmax);
	score = bhatmax;
	return score;
}

void meantrack_init(double *input_blob_rect, unsigned char *image_RGB, float *modelhist, int *datahist)
{
  float row, col, hrow, hcol, numpix;
  //int row, col, hrow, hcol, numpix;
  int n;

  NGIbbrow = row = (float)input_blob_rect[0];   /* center_row  */
  NGIbbcol = col = (float)input_blob_rect[1];   /* center_col  */
  NGIbbhrow = hrow = (float)input_blob_rect[2];  /* half_height */
  NGIbbhcol = hcol = (float)input_blob_rect[3];  /* half_width  */
  NGIbbscale = (float)input_blob_rect[4]; /* scale       */
  NGIbbox = 1;

  NGInumrows = NUMROWS;
  NGInumcols = NUMCOLS;
  NGInumbands = 3;
  numpix = (float)(NGInumrows*NGInumcols);

  separatePlanes(image_RGB, NGIimage, NGInumrows,NGInumcols);
  color_index_image(NGIimage, NGInumcols, NGInumrows, NGIindeximage,1);
  phistogram(NGIindeximage, NGInumrows, NGInumcols, (int)row, (int)col, (int)hrow, (int)hcol, modelhist, HISTLEN);
  for (n=0; n < HISTLEN; n++) {
	  datahist[n] = (int)(100000.0*modelhist[n]);
	  if (datahist[n] != 0)
		  datahist[n] = datahist[n];
  }
  
  //cv_Imwrite_C1(NGIindeximage, NGInumcols, NGInumrows, "result/NGIindeximage_rect.bmp");
 
}

void meantrack_run(double *input_blob_rect, double *output_blob_rect, unsigned char *image_RGB, float *modelhist, int *datahist, float *bhatt_coeff)
{
  float row, col, hrow, hcol, testrow, testcol;
  int newrow, newcol, dumrow, dumcol;
  int copydatahist[256], newdatahist[256], i;
  float scale, newscale, eps=1.0, score, score2;
  
  NGIbbrow = row = (float)input_blob_rect[0];   /* center_row  */
  NGIbbcol = col = (float)input_blob_rect[1];   /* center_col  */
  NGIbbhrow = hrow = (float)input_blob_rect[2];  /* half_height */
  NGIbbhcol = hcol = (float)input_blob_rect[3];  /* half_width  */
  NGIbbscale = scale = (float)input_blob_rect[4]; /* scale       */
  NGIbbox = 1;

/*
  printf("row = %d\n",row);
  printf("col = %d\n",col);
  printf("hrow = %d\n",hrow);
  printf("hcol = %d\n",hcol);
  printf("scale = %f\n",scale);
  */

    testrow = row; testcol = col;

    separatePlanes(image_RGB, NGIimage, NGInumrows,NGInumcols);

    color_index_image(NGIimage, NGInumcols, NGInumrows, NGIindeximage, 0);

	for (i=0; i < 256; i++){
	  newdatahist[i] = datahist[i];
	}

  newscale = scale;
  score = ratmeanshift(modelhist, newdatahist, HISTLEN,
                       NGIindeximage, NGIgradDX, NGIgradDY, NGInumrows, NGInumcols,
                       (int)row, (int)col, (int)(scale*hrow), (int)(scale*hcol),
                       eps, &newrow, &newcol);

  testrow = (float)newrow;
  testcol = (float)newcol;
  if (((0.9*scale*hrow) > 3)&&((0.9*scale*hcol) > 3)) {
    for (i=0; i < 256; i++) copydatahist[i] = datahist[i];
    score2 = ratmeanshift(modelhist, copydatahist, HISTLEN,
                          NGIindeximage, NGIgradDX, NGIgradDY, NGInumrows, NGInumcols,
                          (int)testrow, (int)testcol, (int)(0.9*scale*hrow), (int)(0.9*scale*hcol),
                          eps, &dumrow, &dumcol);

    if (score2 > score) {
      score = score2;
      newscale =  (float)(0.9*scale);
      newrow = dumrow;
      newcol = dumcol;
      for (i=0; i < 256; i++) newdatahist[i] = copydatahist[i];
    }
  }

  for (i=0; i < 256; i++) copydatahist[i] = datahist[i];
  score2 = ratmeanshift(modelhist, copydatahist, HISTLEN,
                        NGIindeximage, NGIgradDX, NGIgradDY, NGInumrows, NGInumcols,
                        (int)testrow, (int)testcol, (int)(1.1*scale*hrow), (int)(1.1*scale*hcol),
                        eps, &dumrow, &dumcol);

  if (score2 > score) {
    score = score2;
    newscale =  (float)(1.1*scale);
    newrow = dumrow;
    newcol = dumcol;
    for (i=0; i < 256; i++) newdatahist[i] = copydatahist[i];
  }

    scale = (float)(0.9*scale+0.1*newscale);
    fprintf(stdout,"score %f: newrow %d newcol %d: scale %f\n", score,newrow,newcol,scale);
  NGIbbrow = (float)newrow;
  NGIbbcol = (float)newcol;
  NGIbbscale = scale;
  for (i=0; i < 256; i++) datahist[i] = newdatahist[i];

    output_blob_rect[0] = newrow;
    output_blob_rect[1] = newcol;
    output_blob_rect[2] = hrow;;
    output_blob_rect[3] = hcol;;
    output_blob_rect[4] = scale;

	*bhatt_coeff = score;

/*
  printf("newrow = %d\n",newrow);
  printf("newcol = %d\n",newcol);
  printf("hrow = %d\n",hrow);
  printf("hcol = %d\n",hcol);
  printf("scale = %f\n",scale);
  */
    
}
//meantrack_run_new was added on 24 Jan 2003 by Raju Patil
//The only difference between meantrack_run_new and meantrack_run is that
//in meantrack_run_new, the size adaptation can be turned off or on
//by passing 1 or 0 to the parameter "size_adapt"
void meantrack_run_new(double *input_blob_rect, double *output_blob_rect, unsigned char *image_RGB, float *modelhist, int *datahist, float *bhatt_coeff, BOOL size_adapt)
{
  float row, col, hrow, hcol, testrow, testcol;
  int newrow, newcol, dumrow, dumcol;
  int copydatahist[256], newdatahist[256], i;
  float scale, newscale, eps=1.0, score, score2;
  
  NGIbbrow = row = (float)input_blob_rect[0];   /* center_row  */
  NGIbbcol = col = (float)input_blob_rect[1];   /* center_col  */
  NGIbbhrow = hrow = (float)input_blob_rect[2];  /* half_height */
  NGIbbhcol = hcol = (float)input_blob_rect[3];  /* half_width  */
  NGIbbscale = scale = (float)input_blob_rect[4]; /* scale       */
  NGIbbox = 1;


  /*
  printf("row = %d\n",row);
  printf("col = %d\n",col);
  printf("hrow = %d\n",hrow);
  printf("hcol = %d\n",hcol);
  printf("scale = %f\n",scale);
  */

    testrow = row; testcol = col;

    separatePlanes(image_RGB, NGIimage, NGInumrows,NGInumcols);

    color_index_image(NGIimage, NGInumcols, NGInumrows, NGIindeximage, 0);

  for (i=0; i < 256; i++) newdatahist[i] = datahist[i];

  newscale = scale;
  score = ratmeanshift(modelhist, newdatahist, HISTLEN,
                       NGIindeximage, NGIgradDX, NGIgradDY, NGInumrows, NGInumcols,
                       (int)row, (int)col, (int)(scale*hrow), (int)(scale*hcol),
                       eps, &newrow, &newcol);

  testrow = (float)newrow;
  testcol = (float)newcol;

  if(size_adapt){ //only if size_adapt is TRUE, we check for different values of scale
	  if (((0.9*scale*hrow) > 3)&&((0.9*scale*hcol) > 3)) {
		  for (i=0; i < 256; i++) copydatahist[i] = datahist[i];
		  score2 = ratmeanshift(modelhist, copydatahist, HISTLEN,
			  NGIindeximage, NGIgradDX, NGIgradDY, NGInumrows, NGInumcols,
			  (int)testrow, (int)testcol, (int)(0.9*scale*hrow), (int)(0.9*scale*hcol),
			  eps, &dumrow, &dumcol);
		  
		  if (score2 > score) {
			  score = score2;
			  newscale =  (float)(0.9*scale);
			  newrow = dumrow;
			  newcol = dumcol;
			  for (i=0; i < 256; i++) newdatahist[i] = copydatahist[i];
		  }
	  }
	  
	  for (i=0; i < 256; i++) copydatahist[i] = datahist[i];
	  score2 = ratmeanshift(modelhist, copydatahist, HISTLEN,
		  NGIindeximage, NGIgradDX, NGIgradDY, NGInumrows, NGInumcols,
		  (int)testrow, (int)testcol, (int)(1.1*scale*hrow), (int)(1.1*scale*hcol),
		  eps, &dumrow, &dumcol);
	  
	  if (score2 > score) {
		  score = score2;
		  newscale =  (float)(1.1*scale);
		  newrow = dumrow;
		  newcol = dumcol;
		  for (i=0; i < 256; i++) newdatahist[i] = copydatahist[i];
	  }
	  
	  scale = (float)(0.9*scale+0.1*newscale);
	  
  }

  fprintf(stdout,"score %f: newrow %d newcol %d: scale %f\n", score,newrow,newcol,scale);
  NGIbbrow = (float)newrow;
  NGIbbcol = (float)newcol;
  NGIbbscale = scale;
  for (i=0; i < 256; i++) datahist[i] = newdatahist[i];

	output_blob_rect[0] = newrow;
	output_blob_rect[1] = newcol;
	output_blob_rect[2] = hrow;;
	output_blob_rect[3] = hcol;;
	output_blob_rect[4] = scale;

	*bhatt_coeff = score;

  /*
  printf("newrow = %d\n",newrow);
  printf("newcol = %d\n",newcol);
  printf("hrow = %d\n",hrow);
  printf("hcol = %d\n",hcol);
  printf("scale = %f\n",scale);
  */
}

//meantrack_run_scale2D was added on Jul 21st, 2003 by Xuhui Zhou
//The only difference between meantrack_run_scale2D and meantrack_run is that 
//1. the scale is in X, Y independently.
//2. pass image_weight as a parameter to display the weight image.
void meantrack_run_scale2D(double *input_blob_rect, double *output_blob_rect, unsigned char *image_RGB, float *modelhist, int *datahist, float *bhatt_coeff, unsigned char *image_weight)
{
  float row, col, hrow, hcol, testrow, testcol;
  int newrow, newcol, dumrow, dumcol;
  int copydatahist[256], i, newdatahist[256];
  //int *newdatahist;
  float scale, newscale, eps=1.0, score, score2;
  double scaleR, scaleC, newscaleR, newscaleC; //make Row, Col scale independently

  NGIbbrow = row = (float)input_blob_rect[0];   /* center_row  */
  NGIbbcol = col = (float)input_blob_rect[1];   /* center_col  */
  NGIbbhrow = hrow = (float)input_blob_rect[2];  /* half_height */
  NGIbbhcol = hcol = (float)input_blob_rect[3];  /* half_width  */
  NGIbbscale = scale = (float)input_blob_rect[4]; /* scale       */
  NGIbbox = 1;

  scale = 1;	//initialize input scale as 1. 
  newscaleR = 1;
  newscaleC = 1;
  /*
  printf("row = %d\n",row);
  printf("col = %d\n",col);
  printf("hrow = %d\n",hrow);
  printf("hcol = %d\n",hcol);
  printf("scale = %f\n",scale);
  */

    testrow = row; testcol = col;

    separatePlanes(image_RGB, NGIimage, NGInumrows,NGInumcols);
    color_index_image(NGIimage, NGInumcols, NGInumrows, NGIindeximage, 0);	

	//cv_Imwrite_C1(NGIindeximage, NGInumcols, NGInumrows, "result/NGIindeximage.bmp");

  for (i=0; i < 256; i++) newdatahist[i] = datahist[i];
//newdatahist = datahist;

  newscale = scale;
  score = ratmeanshift(modelhist, newdatahist, HISTLEN,
                       NGIindeximage, NGIgradDX, NGIgradDY, NGInumrows, NGInumcols,
                       (int)row, (int)col, (int)(scale*hrow), (int)(scale*hcol),
                       eps, &newrow, &newcol);

  testrow = (float)newrow;
  testcol = (float)newcol;

  memcpy(image_weight, NGIbhatim, NGInumrows*NGInumcols);

  scaleR = 1;
  scaleC = 1;
  if(1){ 
	  //scale down 0.9*0.9
	  if (((0.9*scale*hrow) > 3)&&((0.9*scale*hcol) > 3)) {
		  for (i=0; i < 256; i++) copydatahist[i] = datahist[i];
		  score2 = ratmeanshift(modelhist, copydatahist, HISTLEN,
			  NGIindeximage, NGIgradDX, NGIgradDY, NGInumrows, NGInumcols,
			  (int)testrow, (int)testcol, (int)(0.9*scale*hrow), (int)(0.9*scale*hcol),
			  eps, &dumrow, &dumcol);
		  
		  if (score2 > score) {
			  score = score2;
			  newscaleR = 0.9;
			  newscaleC = 0.9;
			  newrow = dumrow;
			  newcol = dumcol;
			  for (i=0; i < 256; i++) newdatahist[i] = copydatahist[i];
		  }
	  }

//	  //scale down 0.9*1
//	  if (((0.9*scale*hrow) > 3)) {
//		  for (i=0; i < 256; i++) copydatahist[i] = datahist[i];
//		  score2 = ratmeanshift(modelhist, copydatahist, HISTLEN,
//			  NGIindeximage, NGIgradDX, NGIgradDY, NGInumrows, NGInumcols,
//			  (int)testrow, (int)testcol, (int)(0.9*scale*hrow), (int)(scale*hcol),
//			  eps, &dumrow, &dumcol);
//		  
//		  if (score2 > score) {
//			  score = score2;
//			  //newscaleR = 0.9;	
//			  //newscaleC = 1;
//			  newrow = dumrow;
//			  newcol = dumcol;
//			  for (i=0; i < 256; i++) newdatahist[i] = copydatahist[i];
//		  }
//	  }
//
//	  //scale down 1*0.9
//	  if (((0.9*scale*hcol) > 3)) {
//		  for (i=0; i < 256; i++) copydatahist[i] = datahist[i];
//		  score2 = ratmeanshift(modelhist, copydatahist, HISTLEN,
//			  NGIindeximage, NGIgradDX, NGIgradDY, NGInumrows, NGInumcols,
//			  (int)testrow, (int)testcol, (int)(scale*hrow), (int)(0.9*scale*hcol),
//			  eps, &dumrow, &dumcol);
//		  
//		  if (score2 > score) {
//			  score = score2;
//			  //newscaleR = 1;
//			  //newscaleC = 0.9;			  
//			  newrow = dumrow;
//			  newcol = dumcol;
//			  for (i=0; i < 256; i++) newdatahist[i] = copydatahist[i];
//		  }
//	  }
	  
	  //scale up 1.1*1.1
	  for (i=0; i < 256; i++) copydatahist[i] = datahist[i];
	  score2 = ratmeanshift(modelhist, copydatahist, HISTLEN,
		  NGIindeximage, NGIgradDX, NGIgradDY, NGInumrows, NGInumcols,
		  (int)testrow, (int)testcol, (int)(1.1*scale*hrow), (int)(1.1*scale*hcol),
		  eps, &dumrow, &dumcol);
	  
	  if (score2 > score) {
		  score = score2;
		  newscaleR = 1.1;
		  newscaleC = 1.1;
		  newrow = dumrow;
		  newcol = dumcol;
		  for (i=0; i < 256; i++) newdatahist[i] = copydatahist[i];
	  }
	  
//	  //scale up 1.1*1
//	  for (i=0; i < 256; i++) copydatahist[i] = datahist[i];
//	  score2 = ratmeanshift(modelhist, copydatahist, HISTLEN,
//		  NGIindeximage, NGIgradDX, NGIgradDY, NGInumrows, NGInumcols,
//		  (int)testrow, (int)testcol, (int)(1.1*scale*hrow), (int)(scale*hcol),
//		  eps, &dumrow, &dumcol);
//	  
//	  if (score2 > score) {
//		  score = score2;
//		  //newscaleR = 1.1;		  
//		  //newscaleC = 1;
//		  newrow = dumrow;
//		  newcol = dumcol;
//		  for (i=0; i < 256; i++) newdatahist[i] = copydatahist[i];
//	  }
//
//	  //scale up 1*1.1
//	  for (i=0; i < 256; i++) copydatahist[i] = datahist[i];
//	  score2 = ratmeanshift(modelhist, copydatahist, HISTLEN,
//		  NGIindeximage, NGIgradDX, NGIgradDY, NGInumrows, NGInumcols,
//		  (int)testrow, (int)testcol, (int)(scale*hrow), (int)(1.1*scale*hcol),
//		  eps, &dumrow, &dumcol);
//	  
//	  if (score2 > score) {
//		  score = score2;
//		  //newscaleR = 1;
//		  //newscaleC = 1.1;
//		  newrow = dumrow;
//		  newcol = dumcol;
//		  for (i=0; i < 256; i++) newdatahist[i] = copydatahist[i];
//	  }
	  
	  //adjust scale
	  scaleR = (float)(newscaleR*0.1+1*0.9);
	  scaleC = (float)(newscaleC*0.1+1*0.9);
  }

  fprintf(stdout,"score %f: newrow %d newcol %d: scale %f\n", score,newrow,newcol,scale);
  NGIbbrow = (float)newrow;
  NGIbbcol = (float)newcol;
  NGIbbscale = scale;
  for (i=0; i < 256; i++) datahist[i] = newdatahist[i];

	output_blob_rect[0] = newrow;
	output_blob_rect[1] = newcol;
	output_blob_rect[2] = hrow;;
	output_blob_rect[3] = hcol;;
	output_blob_rect[4] = scaleR;
	output_blob_rect[5] = scaleC;

	*bhatt_coeff = score;

  /*
  printf("newrow = %d\n",newrow);
  printf("newcol = %d\n",newcol);
  printf("hrow = %d\n",hrow);
  printf("hcol = %d\n",hcol);
  printf("scale = %f\n",scale);
  */
}

void setMeanshiftSize(int rows, int cols)
{
	NUMROWS = rows;
	NUMCOLS = cols;
	NGInumrows = NUMROWS;
	NGInumcols = NUMCOLS;
	NGInumbands = 3;
}