meanshift_tracker.c

学习跟踪的好程序

  short int *dx1, *dx2;
  int r,c,rr,cc,hnrows,hncols, srow, erow, scol, ecol, count;
  int firsttime;
  float *fptr, laccum, raccum, score, minscore, maxscore;
  unsigned char *dptr;

  hnrows = nrows/2;
  hncols = ncols/2;

  if (startrow < 0) startrow = 0;
  if (startcol < 0) startcol = 0;
  if (endrow >= nrows) endrow = (nrows-1);
  if (endcol >= ncols) endcol = (ncols-1);

  firsttime = 1;

  for (r=startrow; r <= endrow; r++) {
    fptr = NGIftemp+r*ncols+startcol;
    for (c=startcol; c <= endcol; c++) {

      /*note, must take into account that gradDx and gradDy are halfsize
	images, i.e. of size nrows/2 X ncols/2 */

      srow = (r - hrow)/2; if (srow < 0) srow = 0;
      erow = (r + hrow)/2; if (erow >= hnrows) erow = (hnrows-1);
      scol = (c - hcol)/2; if (scol < 0) scol = 0;
      ecol = (c + hcol)/2; if (ecol >= hncols) ecol = (hncols-1);

      laccum = 0; raccum = 0; count = 0;
      for (rr = srow; rr <= erow; rr++) {
	dx1 = gradDx + rr*hncols + scol;
	dx2 = gradDx + rr*hncols + ecol;

	for (cc = scol; cc <= ecol; cc++) {
	  laccum += ((*dx1) < 0? -(*dx1) : (*dx1));
	  raccum += ((*dx2) < 0? -(*dx2) : (*dx2));
	  count++;
	  dx1 += hncols;
	  dx2 += hncols;
	}
      }

//      accum = *(gradDx + (r/2)*hncols + (c/2)); 
//      if (accum < 0) accum = -accum;
//      count = 1;

     if (laccum < raccum)
       *fptr++ = score = laccum / (float)count;
     else
       *fptr++ = score = raccum / (float)count;

     if (firsttime) {
       minscore = maxscore = score;
       firsttime = 0;
     }
     if (score < minscore) minscore = score;
     if (score > maxscore) maxscore = score;
    }
  }
  
  memset(edgepix,0,nrows*ncols);
  for (r=startrow; r <= endrow; r++) {
    fptr = NGIftemp+r*ncols+startcol;
    dptr = edgepix+r*ncols+startcol;
    for (c=startcol; c <= endcol; c++) {
      *dptr++ = (unsigned char)(((*fptr++ - minscore)/(maxscore-minscore))*255);
    }
  }
  *minval = minscore;
  *maxval = maxscore;
  return;
}


void bhatblock(float *modelhist, int histlen,
	       unsigned char *indeximage, int nrows, int ncols,
	       int startrow, int endrow, int startcol, int endcol,
	       int hrow, int hcol, unsigned char *bhatpix, float *minval, float *maxval);
//========================================

void bhatblock(float *modelhist, int histlen,
	       unsigned char *indeximage, int nrows, int ncols,
	       int startrow, int endrow, int startcol, int endcol,
	       int hrow, int hcol, unsigned char *bhatpix, float *minval, float *maxval)
     
{
  int i, h, sor, sir, eor, eir, soc, sic, eoc, eic, midc, midr, c, r;
  int inhist[256], outhist[256], *hiptr, *hoptr, insqlen, outsqlen;
  float fscore, *fptr, *mptr, indotprod, outdotprod;
  double fminscore, fmaxscore;
  unsigned char *dptr, *dsor, *dsir, *deor, *deir, *dstart, *dend;
  h = ((hrow < hcol) ? hrow/2 : hcol/2);
  if (h < 2) h = 2;

  if (startrow < 0) startrow = 0;
  if (startcol < 0) startcol = 0;
  if (endrow > nrows) endrow = nrows;
  if (endcol > ncols) endcol = ncols;

  fminscore = 99999.9; fmaxscore = -99999.9;
  dstart = indeximage;
  dend = indeximage + nrows * ncols;

  for (midc=startcol; midc < endcol; midc++) {
    fptr = NGIftemp+startrow*ncols+midc;
    sic = midc - hcol;         if (sic < 0) sic = 0;
    soc = midc - hcol - h;     if (soc < 0) soc = 0;
    eic = midc + hcol + 1;     if (eic > ncols) eic = ncols;
    eoc = midc + hcol + h + 1; if (eoc > ncols) eoc = ncols;
    midr = startrow;
    sir = midr - hrow;         if (sir < 0) sir = 0;
    sor = midr - hrow - h;     if (sor < 0) sor = 0;
    eir = midr + hrow + 1;     if (eir > nrows) eir = nrows;
    eor = midr + hrow + h + 1; if (eor > nrows) eor = nrows;

    for (i=0, hiptr=inhist, hoptr=outhist; i < histlen; i++, hiptr++, hoptr++) 
      *hiptr = *hoptr = 0;

    dsor = indeximage + sor * ncols + soc;
    for (r=sor; r < sir; r++, dsor += ncols) {
      for (dptr = dsor, c = soc; c < eoc; c++, dptr++) { 
		outhist[*dptr]++;
      }
     }
    for (r=sir; r < eir; r++, dsor += ncols) {
      dptr = dsor;
      for (c = soc; c < sic; c++, dptr++) {
		outhist[*dptr]++;
      }
      for (c = sic; c < eic; c++, dptr++) { 
		inhist[*dptr]++;
      }
      for (c = eic; c < eoc; c++, dptr++) {
		outhist[*dptr]++;
      }
    }
    for (r=eir; r < eor; r++, dsor += ncols) {
      for (dptr = dsor, c = soc; c < eoc; c++, dptr++) {
		outhist[*dptr]++;
      }
    }
    
    indotprod = outdotprod = 0.0;
    insqlen = outsqlen = 0;
    for (i=0, hiptr=inhist, hoptr=outhist, mptr=modelhist; i < histlen; i++, hiptr++, hoptr++, mptr++) {
      if (*hiptr) {
		insqlen += (*hiptr) * (*hiptr);
		indotprod += (float)(*hiptr) * (*mptr);
      }
      if (*hoptr) {
		outsqlen += (*hoptr) * (*hoptr);
		outdotprod += (float)(*hoptr) * (*mptr);
      }
    }
    
    fscore = *fptr = (float)(indotprod/sqrt((float)insqlen) -  outdotprod/sqrt((float)outsqlen));
    if (fscore > fmaxscore) fmaxscore = fscore;
    if (fscore < fminscore) fminscore = fscore;
    fptr += ncols;

    midr = startrow;
    sir = midr - hrow;
    sor = midr - hrow - h;
    eir = midr + hrow + 1;
    eor = midr + hrow + h + 1;
    dsor = indeximage + sor * ncols + soc;
    deor = indeximage + eor * ncols + soc;
    dsir = indeximage + sir * ncols + sic;
    deir = indeximage + eir * ncols + sic;

    for (midr = startrow+1; midr < endrow; midr++) {
      if (dsor > dstart) {
		for (dptr=dsor, c=soc; c < eoc; c++, dptr++) {
		  outdotprod -= modelhist[*dptr];
		  outsqlen -= (2 * outhist[*dptr] - 1);
		  outhist[*dptr]--;
		}
      }
      if (dsir > dstart) {
		for (dptr=dsir, c=sic; c < eic; c++, dptr++) {
		  indotprod -= modelhist[*dptr];
		  insqlen -= (2 * inhist[*dptr] - 1);
		  inhist[*dptr]--;
		  outdotprod += modelhist[*dptr];
		  outsqlen += (2 * outhist[*dptr] + 1);
		  outhist[*dptr]++;
		}
      }
      if (deir < dend) {
		for (dptr=deir, c=sic; c < eic; c++, dptr++) {
		  indotprod += modelhist[*dptr];
		  insqlen += (2 * inhist[*dptr] + 1);
		  inhist[*dptr]++;
		  outdotprod -= modelhist[*dptr];
		  outsqlen -= (2 * outhist[*dptr] - 1);
		  outhist[*dptr]--;
		}
      }
      if (deor < dend) {
		for (dptr=deor, c=soc; c < eoc; c++, dptr++) {
		  outdotprod += modelhist[*dptr];
		  outsqlen += (2 * outhist[*dptr] + 1);
		  outhist[*dptr]++;
		}
      }

 	  fscore = *fptr = (float)(indotprod/sqrt((float)insqlen) -  outdotprod/sqrt((float)outsqlen));
	  if (fscore > fmaxscore) fmaxscore = fscore;
	  if (fscore < fminscore) fminscore = fscore;
	  fptr += ncols;
	  dsor += ncols; deor += ncols; 
	  dsir += ncols; deir += ncols;
    }
  }

  dptr = bhatpix;
  memset(bhatpix,0,nrows*ncols);
  for (r=startrow; r < endrow; r++) {
    fptr = NGIftemp+r*ncols+startcol;
    dptr = bhatpix+r*ncols+startcol;
    for (c=startcol; c < endcol; c++){
      //*dptr++ = (unsigned char)(((*fptr++ - fminscore)/(fmaxscore-fminscore))*255);
		*dptr = (unsigned char)(((*fptr++ - fminscore)/(fmaxscore-fminscore))*255);
		dptr++;
	}
  }
  *minval = (float)fminscore;
  *maxval = (float)fmaxscore;
  return;
}



void histblock(unsigned char *indexim, int nrows, int ncols, 
	       int srow, int erow, int scol, int ecol,
	       int *histogram, int histlen, int *histsum)
{
  int i,r,c;
  unsigned char *dptr;
  for (i=0; i < histlen; i++) histogram[i] = 0;
  *histsum = 0;
  if (srow < 0) srow = 0;
  if (scol < 0) scol = 0;
  if (erow > nrows) erow = nrows;
  if (ecol > ncols) ecol = ncols;
  for (r = srow; r < erow; r++) {
    dptr = indexim + r*ncols + scol;
    for (c = scol; c < ecol; c++, dptr++) {
      histogram[*dptr]++;
      (*histsum)++;
    }
  }  
  return;
}


void ratblock(float *modelhist, int *datahist, int histlen,
	      unsigned char *indeximage, int nrows, int ncols,
	      int startrow, int endrow, int startcol, int endcol,
	      int hrow, int hcol, unsigned char *bhatpix, float *minval, float *maxval)
     
{
  int i, h, c, r;
  float fscore, fminscore, fmaxscore, *fptr;
  float rathist[256], accum;
  unsigned char *dptr;
  h = ((hrow < hcol) ? hrow/2 : hcol/2);
  if (h < 2) h = 2;

  if (startrow < 0) startrow = 0;
  if (startcol < 0) startcol = 0;
  if (endrow > nrows) endrow = nrows;
  if (endcol > ncols) endcol = ncols;

  //make norm(datahist) = 1
  accum = 0;
  for (i=0; i < histlen; i++)
    accum += (float)(datahist[i]*datahist[i]);
  accum = (float)sqrt(accum);
  printf("accum is %f\n",accum);

  fminscore = 0; fmaxscore = 0;
  for (i=0; i < histlen; i++){ 
    if (datahist[i] && modelhist[i]) {
      fscore = rathist[i] = (float)sqrt(modelhist[i] / (datahist[i] / accum));
//      fscore = rathist[i] = modelhist[i] / (datahist[i] / accum);
//      fscore = rathist[i] = modelhist[i] * (datahist[i] / accum);
//      fscore = rathist[i] = modelhist[i];
      if (fscore > fmaxscore) fmaxscore = fscore;
      if (fscore < fminscore) fminscore = fscore;
    }
	else
		rathist[i] = 0;
  }

  //for every entry in indeximg, get histogram ratio (as weight)
  for (r=startrow; r < endrow; r++) {
    fptr = NGIftemp+r*ncols + startcol;
    dptr = indeximage+r*ncols + startcol;
    for (c=startcol; c < endcol; c++) {
      *fptr = rathist[*dptr];
      fptr++;
      dptr++;
    }
  }
  printf("min %f max %f\n",fminscore,fmaxscore);

  //normalize NGIftemp to 0~255, output bhatpix
  dptr = bhatpix;
  memset(bhatpix,0,nrows*ncols);
  for (r=startrow; r < endrow; r++) {
    fptr = NGIftemp+r*ncols+startcol;
    dptr = bhatpix+r*ncols+startcol;
    for (c=startcol; c < endcol; c++){
      //*dptr++ = (unsigned char)(((*fptr++ - fminscore)/(fmaxscore-fminscore))*255);
		*dptr = (unsigned char)(((*fptr++ - fminscore)/(fmaxscore-fminscore))*255);
		dptr++;
	}
  }
  *minval = fminscore;
  *maxval = fmaxscore;
  return;
}



float ratmeanshift(float *modelhist, int *datahist, int histlen,
		   unsigned char *indeximage, short int *gradDx, short int *gradDy, 
		   int nrows, int ncols, int row, int col, int hrow, int hcol,
		   float stepepsilon, int *bestrow, int *bestcol)
{
	float score, scoreB, scoreE, w;
	unsigned char *bhatim, *edgeim, *dptr, *eptr;
	float accum, accumB, accumE, sumr, sumc, dr, dc, eps2, stepdist2;
	float bhatmin, bhatmax;
	int count,r,c,srow,erow,scol,ecol,deltarow,deltacol;
	int newrow,newcol,histsum,ntimes,border;

	eps2 = stepepsilon*stepepsilon;

	bhatim = NGItemp;
	border = ((hcol < hrow) ? hcol : hrow);

	histblock(indeximage, nrows, ncols, 
	  row-hrow, row+hrow, col-hcol, col+hcol,
	  datahist, histlen, &histsum);

	ratblock(modelhist, datahist, histlen, indeximage, nrows, ncols, 
	   row-hrow-border,row+hrow+border,col-hcol-border,col+hcol+border,
	   hrow, hcol, bhatim, &bhatmin, &bhatmax);

	memcpy(NGIbhatim, bhatim, nrows*ncols);
	edgeim = NGItemp+nrows*ncols;
	//edgeblock(gradDx, gradDy, nrows, ncols, 
	//row-2*hrow,row+2*hrow,col-2*hcol,col+2*hcol,
	//hrow, hcol, edgeim, &edgemin, &edgemax);

//	//debug print out images
//	if(0)
//	{
//		FILE *file;
//		char *filename;
//		int row, col;
//		int offset;
//	
//		filename = "result/bhatim0.txt";
//		file = fopen(filename, "w");
//		for (row=0; row<243; row++){			
//			for (col=0; col<320; col++){
//				offset = row*320+col;
//				fprintf(file, "%3d ", *(bhatim+offset));
//			}
//			fprintf(file, "\n");
//		}
//	}

	stepdist2 = eps2+1;
	ntimes = 0;
	while ((stepdist2 >= eps2)&&(ntimes < 10)) {
		accum = accumB = accumE = sumr = sumc = 0.0;  count = 0;
		srow = row - hrow; if (srow < 0) srow = 0;  
		scol = col - hcol; if (scol < 0) scol = 0;  
		erow = row + hrow; if (erow >= nrows) erow = nrows-1;  
		ecol = col + hcol; if (ecol >= ncols) ecol = ncols-1;    
		for (r = srow; r <= erow; r++) {
		  dptr = bhatim + r*ncols + scol;
		  eptr = edgeim + r*ncols + scol;
		  dr = (float)(r - row);
		  for (c = scol; c <= ecol; c++, dptr++, eptr++) {
			dc = (float)(c - col);
			//	w = *dptr + *eptr;
			w = *dptr;
			sumr += w*dr;
			sumc += w*dc;
			accum += w;
			accumB += *dptr;
			accumE += *eptr;
			count++;
		  }
		}
		scoreB = accumB / (float)count;
		scoreE = accumE / (float)count;
		newrow = (int)(row + floor(sumr/accum+0.5));
		newcol = (int)(col + floor(sumc/accum+0.5));    
		deltarow = newrow - row;
		deltacol = newcol - col;
	//    fprintf(stdout,"  step row %d col %d\n",newrow,newcol);

		stepdist2 = (float)(deltarow*deltarow + deltacol*deltacol);
		row = newrow; 
		col = newcol;


		histblock(indeximage, nrows, ncols, 
			  row-hrow, row+hrow, col-hcol, col+hcol,
			  datahist, histlen, &histsum);
		ratblock(modelhist, datahist, histlen, indeximage, nrows, ncols, 
			 row-hrow-border,row+hrow+border,col-hcol-border,col+hcol+border,
			 hrow, hcol, bhatim, &bhatmin, &bhatmax);
		memcpy(NGIbhatim, bhatim, nrows*ncols);
		ntimes++;
	}

	//  fprintf(stdout,"  final row %d col %d\n",row,col);
	*bestrow = row;
	*bestcol = col;

	histblock(indeximage, nrows, ncols, 
		row-hrow, row+hrow, col-hcol, col+hcol,
		datahist, histlen, &histsum);