meanshift_tracker.c

学习跟踪的好程序

             /*
 * meantrack.c - top level for mean-shift tracking
 * Original implementation by 
 * Bob Collins, Carnegie Mellon University, July 31, 2001
 * Matlab mex file written by
 * Raju Patil, Carnegie Mellon University, Nov 8, 2001
 */
//#include "stdafx.h"
#include "globaldata.h"
#include "imageProcessing.h"
#include "meanshift_tracker.h"
//#include "../Utility.h"

#define INIT_MODE 0
#define RUN_MODE  1

//#define NUMROWS 243
//#define NUMCOLS 320

//#define NUMROWS 480
//#define NUMCOLS 720
int NUMROWS=480;
int NUMCOLS=720;

#define SPEED_DELTA 10
#define FBD_DELTA 12
#define SIZE_DELTA  6

#define HISTLEN 8*8*4

#define PI 3.14159265358979


/*************************************/
/* Start of meanshift.c routines       */
/*************************************/
/*
 * Functions for performing mean-shift tracking
 * motivation is paper by Comanesciu, Ramesh and Meer in CVPR'00
 * Bob Collins  Carnegie Mellon University  Aug 1, 2001
 */
// acquire normalized weighted index histogram
// weights are according to Epanechnikov profile (4/(2 * pi)) * (1 - radiusq);
// since we are normalizing at end, we drop the constant 4/(2pi)

void phistogram(unsigned char *indeximage, int nrows, int ncols,
  		int row, int col, int hrow, int hcol, 
	        float *histogram, int histlen)
{
  unsigned char *dptr;
  float radiusq, accum, dr, dc;
  int i,r,c,srow,erow,scol,ecol;

  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 (i=0; i < histlen; i++) histogram[i] = 0.0;
  for (r = srow; r <= erow; r++) {
    dptr = indeximage + r*ncols + scol;
    dr = (float)(r - row)/(float)hrow;
    for (c = scol; c <= ecol; c++, dptr++) {
      dc = (float)(c - col)/(float)hcol;
      radiusq = dr*dr + dc*dc;
      if (radiusq < 1.0){      
		histogram[*dptr] += (float)((4/(2*PI))*(float)(1.0-radiusq));
	  }
	  //histogram[*dptr] ++;
    }
  }

  // Original code from Bob, to make norm(histogram) = 1 //
  accum=0.0;
  for  (i=0; i < histlen; i++)
    accum += (histogram[i] * histogram[i]);
  accum = (float)sqrt(accum);
  
  // orginal end//

  /* above code modified as follows by Raju Patil: 3 Oct 02, to make sum(histogram) = 1 */
//  accum=0.0;
//  for  (i=0; i < histlen; i++)
//  accum += histogram[i];
  /* code modification ends*/

  for  (i=0; i < histlen; i++)
    histogram[i] /= accum;
 
  return;
}


// Bhatacharyya coefficient for comparing two normalized histograms
// weights are according to Epanechnikov profile (4/(2 * pi)) * (1 - radiusq);
// since we are normalizing at end, we drop the constant 4/(2pi)

float bhatcoeff(float *modelhist, float *phist, int histlen)
{
  float accum;
  int i;
  accum = 0;
  for (i=0; i < histlen; i++, phist++, modelhist++) {
    accum += (float)sqrt((*phist) * (*modelhist));
  }

  return(accum);
}




//======================================================================
// FAST COMPUTATION OF BHATACHARYYA COEFF
// NOTE: NO CHECKING FOR OUT OF BOUND PIXELS IS DONE, BE CAREFUL!


//void histblock(unsigned char *indexim, int nrows, int ncols, 
//	       int srow, int erow, int scol, int ecol,
//	       int *histogram, int histlen, int *histsum);

void oldhistblock(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;
  for (r = srow; r <= erow; r++) {
    dptr = indexim + r*ncols + scol;
    for (c = scol; c <= ecol; c++, dptr++) {
      histogram[*dptr]++;
      (*histsum)++;
    }
  }  
  return;
}

//update results based on shifting one column to right
void update_hist_col(unsigned char *indexim, int nrows, int ncols, 
		     int srow, int erow, int scol, int ecol,
		     int *histogram)
{
  int r;
  unsigned char *addptr, *subptr;
  subptr = indexim + srow*ncols + scol;
  addptr = indexim + srow*ncols + ecol + 1;
  for (r = srow; r <= erow; r++) {
    histogram[*subptr]--;
    histogram[*addptr]++;
    subptr += ncols;
    addptr += ncols;
  }
  return;
}

//update results based on shifting one row down
void update_hist_row(unsigned char *indexim, int nrows, int ncols, 
		     int srow, int erow, int scol, int ecol,
		     int *histogram)
{
  int c;
  unsigned char *addptr, *subptr;
  subptr = indexim + srow*ncols + scol;
  addptr = indexim + (erow+1)*ncols + scol;
  for (c = scol; c <= ecol; c++) {
    histogram[*subptr]--;
    histogram[*addptr]++;
    subptr++;
    addptr++;
  }
  return;
}


float fastbhat(float *modelhist, int histlen, int *histogram, int histsum)
{
  float accum;
  int i;

  for (i=0, accum=0.0; i < histlen; i++)
    if (modelhist[i])
      accum += (float)sqrt((float)modelhist[i] * (float)(histogram[i]));
  accum /= (float)sqrt((float)histsum);
  return(accum);
}


float fullbhat(float *modelhist, int histlen,
	       unsigned char *indeximage, int nrows, int ncols,
	       int row, int col, int hrow, int hcol)
{
  float score;
  int srow, scol, erow, ecol, histsum, histogram[256];

  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;    
  histblock(indeximage, nrows, ncols, srow, erow, scol, ecol, histogram, histlen, &histsum);
  score = fastbhat(modelhist, histlen, histogram, histsum);
  return(score);
}


void bhatimage(float *modelhist, int histlen,
	       unsigned char *indeximage, int nrows, int ncols,
	       int hrow, int hcol, unsigned char *bhatpix)

{
  float score, *fptr, minscore, maxscore;
  unsigned char *dptr;
  int i,r,c,srow,erow,scol,ecol, numpix, histsum;
  int Isrow, Ierow, Iscol, Iecol;
  int histogram[256];
  int firsttime;

  numpix = nrows*ncols;
  fptr = NGIftemp;
  for (i=0; i < numpix; i++)
    *fptr++ = 0;

  Isrow = hrow; Ierow = nrows-hrow;
  Iscol = hcol; Iecol = ncols-hcol;

  firsttime = 1;

  for (c=Iscol; c < Iecol; c++) {
    fptr = NGIftemp+Isrow*ncols+c;
    srow = Isrow - hrow; 
    erow = Isrow + hrow;
    scol = c - hcol;
    ecol = c + hcol;
    histblock(indeximage, nrows, ncols, srow, erow, scol, ecol, histogram, histlen, &histsum);
    *fptr = score = fastbhat(modelhist, histlen, histogram, histsum);
    fptr+=ncols;
    if (firsttime) {
      minscore = maxscore = score;
      firsttime = 0;
    }
    if (score < minscore) minscore = score;
    if (score > maxscore) maxscore = score;
    for (r=Isrow; r < Ierow; r++) {
      update_hist_row(indeximage, nrows, ncols, srow, erow, scol, ecol, histogram);
      *fptr = score = fastbhat(modelhist, histlen, histogram, histsum);
      fptr+=ncols;
      if (score < minscore) minscore = score;
      if (score > maxscore) maxscore = score;
      srow++; erow++;
    }
  }

  dptr = bhatpix;
  fptr = NGIftemp;
  for (i=0; i < numpix; i++)
    *dptr++ = (unsigned char)(((*fptr++ - minscore)/(maxscore-minscore))*255);

  return;
}

void histsubtract(int *hist1, int *hist2, int histlen, int *histout, int *histsum)
{
  int i, sum;
  for (i=0, sum=0; i < histlen; i++) {
    histout[i] = hist1[i] - hist2[i];
    sum += histout[i];
  }
  *histsum = sum;
  return;
}

void oldbhatblock(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)

{
  float score, *fptr, minscore, maxscore, outerscore;
  unsigned char *dptr;
  int r,c,srow,erow,scol,ecol, histsum;
  int Isrow, Ierow, Iscol, Iecol, h;
  int histogram[256];
  int firsttime;

  h = hrow / 2;
  if (h > (hcol / 2)) h = hcol / 2;
  if (h < 2) h = 2;

  if (startrow < hrow+h) startrow = hrow+h;
  if (startcol < hcol+h) startcol = hcol+h;
  if (endrow > (nrows-hrow-h)) endrow = (nrows-hrow-h);
  if (endcol > (ncols-hcol-h)) endcol = (ncols-hcol-h);

  Isrow = startrow; Ierow = endrow;
  Iscol = startcol; Iecol = endcol;

  firsttime = 1;

  for (c=Iscol; c < Iecol; c++) {
    fptr = NGIftemp+Isrow*ncols+c;
    srow = Isrow - hrow; 
    erow = Isrow + hrow;
    scol = c - hcol;
    ecol = c + hcol;
    histblock(indeximage, nrows, ncols, srow, erow, scol, ecol, histogram, histlen, &histsum);
//    histblock(indeximage, nrows, ncols, srow-h, erow+h, scol-h, ecol+h, bighist, histlen, &bigsum);
//    histsubtract(bighist,histogram,histlen,outerhist,&outersum);

    score = fastbhat(modelhist, histlen, histogram, histsum);
//    outerscore = fastbhat(modelhist, histlen, outerhist, outersum);
    outerscore = 0;
    *fptr = score = score - outerscore;
    fptr+=ncols;
    if (firsttime) {
      minscore = maxscore = score;
      firsttime = 0;
    }
    if (score < minscore) minscore = score;
    if (score > maxscore) maxscore = score;
    for (r=Isrow; r < Ierow; r++) {
      update_hist_row(indeximage, nrows, ncols, srow, erow, scol, ecol, histogram);
//      update_hist_row(indeximage, nrows, ncols, srow-h, erow+h, scol-h, ecol+h, bighist);
//      histsubtract(bighist,histogram,histlen,outerhist,&outersum);
      score = fastbhat(modelhist, histlen, histogram, histsum);
//      outerscore = fastbhat(modelhist, histlen, outerhist, outersum);
      outerscore = 0;
      *fptr = score = score - outerscore;
      fptr+=ncols;
      if (score < minscore) minscore = score;
      if (score > maxscore) maxscore = score;
      srow++; erow++;
    }
  }

  dptr = bhatpix;
  fptr = NGIftemp;
  memset(bhatpix,0,nrows*ncols);
  for (r=Isrow; r < Ierow; r++) {
    fptr = NGIftemp+r*ncols+Iscol;
    dptr = bhatpix+r*ncols+Iscol;
    for (c=Iscol; c < Iecol; c++)
      *dptr++ = (unsigned char)(((*fptr++ - minscore)/(maxscore-minscore))*255);
  }
  *minval = minscore;
  *maxval = maxscore;
  return;
}

void bhatblock2(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)

{
  float score, *fptr, minscore, maxscore, outerscore;
  unsigned char *dptr;
  int r,c,srow,erow,scol,ecol, histsum,bigsum,outersum;
  int Isrow, Ierow, Iscol, Iecol, h;
  int histogram[256],bighist[256],outerhist[256];
  int firsttime;

  h = hrow / 2;
  if (h > (hcol / 2)) h = hcol / 2;
  if (h < 2) h = 2;

  if (startrow < hrow+h) startrow = hrow+h;
  if (startcol < hcol+h) startcol = hcol+h;
  if (endrow > (nrows-hrow-h)) endrow = (nrows-hrow-h);
  if (endcol > (ncols-hcol-h)) endcol = (ncols-hcol-h);

  Isrow = startrow; Ierow = endrow;
  Iscol = startcol; Iecol = endcol;

  firsttime = 1;

  for (c=Iscol; c < Iecol; c++) {
    fptr = NGIftemp+Isrow*ncols+c;
    srow = Isrow - hrow; 
    erow = Isrow + hrow;
    scol = c - hcol;
    ecol = c + hcol;
    histblock(indeximage, nrows, ncols, srow, erow, scol, ecol, histogram, histlen, &histsum);
    histblock(indeximage, nrows, ncols, srow-h, erow+h, scol-h, ecol+h, bighist, histlen, &bigsum);
    histsubtract(bighist,histogram,histlen,outerhist,&outersum);

    score = fastbhat(modelhist, histlen, histogram, histsum);
    outerscore = fastbhat(modelhist, histlen, outerhist, outersum);
    *fptr = score = score - outerscore;
    fptr+=ncols;
    if (firsttime) {
      minscore = maxscore = score;
      firsttime = 0;
    }
    if (score < minscore) minscore = score;
    if (score > maxscore) maxscore = score;
    for (r=Isrow; r < Ierow; r++) {
      update_hist_row(indeximage, nrows, ncols, srow, erow, scol, ecol, histogram);
      update_hist_row(indeximage, nrows, ncols, srow-h, erow+h, scol-h, ecol+h, bighist);
      histsubtract(bighist,histogram,histlen,outerhist,&outersum);
      score = fastbhat(modelhist, histlen, histogram, histsum);
      outerscore = fastbhat(modelhist, histlen, outerhist, outersum);
      *fptr = score = score - outerscore;
      fptr+=ncols;
      if (score < minscore) minscore = score;
      if (score > maxscore) maxscore = score;
      srow++; erow++;
    }
  }

  dptr = bhatpix;
  fptr = NGIftemp;
  memset(bhatpix,0,nrows*ncols);
  for (r=Isrow; r < Ierow; r++) {
    fptr = NGIftemp+r*ncols+Iscol;
    dptr = bhatpix+r*ncols+Iscol;
    for (c=Iscol; c < Iecol; c++)
      *dptr++ = (unsigned char)(((*fptr++ - minscore)/(maxscore-minscore))*255);
  }
  *minval = minscore;
  *maxval = maxscore;
  return;
}



void edgeblock(short int *gradDx, short int *gradDy, int nrows, int ncols,
	       int startrow, int endrow, int startcol, int endcol,
	       int hrow, int hcol, unsigned char *edgepix, float *minval, float *maxval)

{