/* * 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 rightvoid 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 downvoid 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){