objecttracker.cpp

学习meanshift跟踪算法的好程序

  x_diff = max(m_sTrackingObjectTable[m_cActiveObject].X-m_sTrackingObjectTable[m_cActiveObject].W/2,0);
  //the y upper and lower bounds
  y_sum =  min(m_sTrackingObjectTable[m_cActiveObject].Y+m_sTrackingObjectTable[m_cActiveObject].H/2+1,m_nImageHeight-1);
  y_diff = max(m_sTrackingObjectTable[m_cActiveObject].Y-m_sTrackingObjectTable[m_cActiveObject].H/2,0);

  for (y=y_diff;y<=y_sum;y++)
  {
	  SetPixelValues(r, g, b,pixelValues,x_diff,y);
	  SetPixelValues(r, g, b,pixelValues,x_diff+1,y);

      SetPixelValues(r, g, b,pixelValues,x_sum-1,y);
      SetPixelValues(r, g, b,pixelValues,x_sum,y);
  }
  for (x=x_diff;x<=x_sum;x++)
  {
	  SetPixelValues(r, g, b,pixelValues,x,y_diff);
      SetPixelValues(r, g, b,pixelValues,x,y_diff+1);

      SetPixelValues(r, g, b,pixelValues,x,y_sum-1);
      SetPixelValues(r, g, b,pixelValues,x,y_sum);
  }
	cvCvtPlaneToPix(b, g, r, NULL, frame);

	cvReleaseImage(&r);
	cvReleaseImage(&g);
	cvReleaseImage(&b);
}
// Computes weights and drives the new location of object in the next frame
void CObjectTracker::FindWightsAndCOM(IplImage *frame, FLOAT32 (*histogram))
{
  SINT16  i = 0;
  SINT16  x = 0;
  SINT16  y = 0;
  UBYTE8  E = 0;
  FLOAT32  sumOfWeights = 0;
  SINT16  ptr  = 0;
  UBYTE8  qR = 0,qG = 0,qB = 0;
  FLOAT32   newX = 0.0;
  FLOAT32   newY = 0.0;
//  ULONG_32 pixelValues = 0;
  IplImage* r, * g, * b;


  FLOAT32 *weights = new FLOAT32[HISTOGRAM_LENGTH];

  for (i=0;i<HISTOGRAM_LENGTH;i++)
  {
	  if (histogram[i] >0.0 )
		  weights[i] = m_sTrackingObjectTable[m_cActiveObject].initHistogram[i]/histogram[i];	//qu/pu(y0)
	  else
		  weights[i] = 0.0;
  }

	r = cvCreateImage( cvGetSize(frame), frame->depth, 1 );
	g = cvCreateImage( cvGetSize(frame), frame->depth, 1 );
	b = cvCreateImage( cvGetSize(frame), frame->depth, 1 );
	cvCvtPixToPlane( frame, b, g, r, NULL );	//divide color image into separate planes r, g, b. The exact sequence doesn't matter.

  for (y=max(m_sTrackingObjectTable[m_cActiveObject].Y-m_sTrackingObjectTable[m_cActiveObject].H/2,0);y<=min(m_sTrackingObjectTable[m_cActiveObject].Y+m_sTrackingObjectTable[m_cActiveObject].H/2,m_nImageHeight-1);y++)
	  for (x=max(m_sTrackingObjectTable[m_cActiveObject].X-m_sTrackingObjectTable[m_cActiveObject].W/2,0);x<=min(m_sTrackingObjectTable[m_cActiveObject].X+m_sTrackingObjectTable[m_cActiveObject].W/2,m_nImageWidth-1);x++)
	  {
		  E = CheckEdgeExistance(r, g, b,x,y);

			qR = (UBYTE8)pixval8c( r, y, x )/16;
			qG = (UBYTE8)pixval8c( g, y, x )/16;
			qB = (UBYTE8)pixval8c( b, y, x )/16;

		  ptr = 4096*E+256*qR+16*qG+qB;	//some recalculation here. The bin number of (x, y) can be stroed somewhere in fact.

		  newX += (weights[ptr]*x);
		  newY += (weights[ptr]*y);

		  sumOfWeights += weights[ptr];
		}

   if (sumOfWeights>0)
   {
	   m_sTrackingObjectTable[m_cActiveObject].X = SINT16((newX/sumOfWeights) + 0.5);	//update location
	   m_sTrackingObjectTable[m_cActiveObject].Y = SINT16((newY/sumOfWeights) + 0.5);
   }

	cvReleaseImage(&r);
	cvReleaseImage(&g);
	cvReleaseImage(&b);
   delete[] weights, weights = 0;
}
// Returns the distance between two histograms.
FLOAT32 CObjectTracker::FindDistance(FLOAT32 (*histogram))
{
  SINT16  i = 0;
  FLOAT32 distance = 0;

  
  for(i=0;i<HISTOGRAM_LENGTH;i++)
	  distance += FLOAT32(sqrt(DOUBLE64(m_sTrackingObjectTable[m_cActiveObject].initHistogram[i]
                  *histogram[i])));

  return(sqrt(1-distance));
}
//An alternative distance measurement 
FLOAT32 CObjectTracker::CompareHistogram(UBYTE8 (*histogram))
{
  SINT16  i	 = 0;
  FLOAT32 distance = 0.0;
  FLOAT32 difference = 0.0;

  
  for (i=0;i<HISTOGRAM_LENGTH;i++)
  {
	  difference = FLOAT32(m_sTrackingObjectTable[m_cActiveObject].initHistogram[i]
                         -histogram[i]);

	  if (difference>0)
		  distance += difference;
	  else
  		  distance -= difference;
  }
  return(distance);
}
// Returns the edge insformation of a pixel at (x,y), assume a large jump of value around edge pixels
UBYTE8 CObjectTracker::CheckEdgeExistance(IplImage *r, IplImage *g, IplImage *b, SINT16 _x,SINT16 _y)
{
  UBYTE8  E = 0;
  SINT16  GrayCenter = 0;
  SINT16  GrayLeft = 0;
  SINT16  GrayRight = 0;
  SINT16  GrayUp = 0;
  SINT16  GrayDown = 0;
//  ULONG_32 pixelValues = 0;

//  pixelValues = GetPixelValues(frame,_x,_y);
  GrayCenter = SINT16(3*pixval8c( r, _y, _x )+6*pixval8c( g, _y, _x )+pixval8c( b, _y, _x ));

  if (_x>0)
  {
//	  pixelValues = GetPixelValues(frame,_x-1,_y);
  
	  GrayLeft = SINT16(3*pixval8c( r, _y, _x-1 )+6*pixval8c( g, _y, _x-1 )+pixval8c( b, _y, _x-1 ));
  }

  if (_x < (m_nImageWidth-1))
  {
//	  pixelValues = GetPixelValues(frame,_x+1,_y);
  
      GrayRight = SINT16(3*pixval8c( r, _y, _x+1 )+6*pixval8c( g, _y, _x+1 )+pixval8c( b, _y, _x+1 ));
  }

  if (_y>0)
  {
//	  pixelValues = GetPixelValues(frame,_x,_y-1);
  
      GrayUp = SINT16(3*pixval8c( r, _y-1, _x )+6*pixval8c( g, _y-1, _x )+pixval8c( b, _y-1, _x ));
  }

  if (_y<(m_nImageHeight-1))
  {
//	  pixelValues = GetPixelValues(frame,_x,_y+1);

	  GrayDown = SINT16(3*pixval8c( r, _y+1, _x )+6*pixval8c( g, _y+1, _x )+pixval8c( b, _y+1, _x ));
  }

  if (abs((GrayCenter-GrayLeft)/10)>EDGE_DETECT_TRESHOLD)
	  E = 1;

  if (abs((GrayCenter-GrayRight)/10)>EDGE_DETECT_TRESHOLD)
	  E = 1;

  if (abs((GrayCenter-GrayUp)/10)>EDGE_DETECT_TRESHOLD)
      E = 1;

  if (abs((GrayCenter-GrayDown)/10)>EDGE_DETECT_TRESHOLD)
      E = 1;

  return(E);
}
// Alpha blending: used to update initial histogram by the current histogram 
void CObjectTracker::UpdateInitialHistogram(UBYTE8 (*histogram))
{
  SINT16  i = 0;
  
  for (i=0; i<HISTOGRAM_LENGTH; i++)
	  m_sTrackingObjectTable[m_cActiveObject].initHistogram[i] = ALPHA*m_sTrackingObjectTable[m_cActiveObject].initHistogram[i]
                                                            +(1-ALPHA)*histogram[i];

}
// Mean-shift iteration 
void CObjectTracker::FindNextLocation(IplImage *frame)
{
	int i, j, opti, optj;
	SINT16 scale[3]={-3, 3, 0};
	FLOAT32 dist, optdist;
	SINT16 h, w, optX, optY;

	//try no-scaling
	FindNextFixScale(frame);
	optdist=LastDist;
	optX=m_sTrackingObjectTable[m_cActiveObject].X;
	optY=m_sTrackingObjectTable[m_cActiveObject].Y;

	//try one of the 9 possible scaling
	i=rand()*2/RAND_MAX;
	j=rand()*2/RAND_MAX;
	h=m_sTrackingObjectTable[m_cActiveObject].H;
	w=m_sTrackingObjectTable[m_cActiveObject].W;
	if(h+scale[i]>10 && w+scale[j]>10 && h+scale[i]<m_nImageHeight/2 && w+scale[j]<m_nImageWidth/2)
	{
		m_sTrackingObjectTable[m_cActiveObject].H=h+scale[i];
		m_sTrackingObjectTable[m_cActiveObject].W=w+scale[j];
		FindNextFixScale(frame);
		if( (dist=LastDist) < optdist )	//scaling is better
		{
			optdist=dist;
//			printf("Next%f->\n", dist);
		}
		else	//no scaling is better
		{
			m_sTrackingObjectTable[m_cActiveObject].X=optX;
			m_sTrackingObjectTable[m_cActiveObject].Y=optY;
			m_sTrackingObjectTable[m_cActiveObject].H=h;
			m_sTrackingObjectTable[m_cActiveObject].W=w;
		}
	};
	TotalDist+=optdist;	//the latest distance
//	printf("\n");
}

void CObjectTracker::FindNextFixScale(IplImage *frame)
{
  UBYTE8  iteration = 0;
  SINT16 optX, optY;

  FLOAT32 *currentHistogram = new FLOAT32[HISTOGRAM_LENGTH];
  FLOAT32 dist, optdist=1.0;

  for (iteration=0; iteration<MEANSHIFT_ITARATION_NO; iteration++)
  {
	  FindHistogram(frame,currentHistogram); //current frame histogram, use the last frame location as starting point
   
      FindWightsAndCOM(frame,currentHistogram);//derive weights and new location
   
      //FindHistogram(frame,currentHistogram);   //uptade histogram
   
      //UpdateInitialHistogram(currentHistogram);//uptade initial histogram
	  if( ((dist=FindDistance(currentHistogram)) < optdist) || iteration==0 )
	  {
		  optdist=dist;
		  optX=m_sTrackingObjectTable[m_cActiveObject].X;
		  optY=m_sTrackingObjectTable[m_cActiveObject].Y;
//	  	  printf("%f->", dist);
	  }
	  else	//bad iteration, then find a better start point for next iteration
	  {
		m_sTrackingObjectTable[m_cActiveObject].X=(m_sTrackingObjectTable[m_cActiveObject].X+optX)/2;
		m_sTrackingObjectTable[m_cActiveObject].Y=(m_sTrackingObjectTable[m_cActiveObject].Y+optY)/2;
	  }
  }//end for
  m_sTrackingObjectTable[m_cActiveObject].X=optX;
  m_sTrackingObjectTable[m_cActiveObject].Y=optY;
  LastDist=optdist;	//the latest distance
//  printf("\n");

  delete[] currentHistogram, currentHistogram = 0;
}
 
float CObjectTracker::GetTotalDist(void)
{
	return(TotalDist);
}