objecttracker.cpp

学习meanshift跟踪算法的好程序

/*******************************************************************
Bilkent University: 

Mean-shift Tracker based Moving Object Tracker in Video

Version: 1.0

Compiler: Microsoft Visual C++ 6.0 (tested in both debug and release 
          mode)

**********************************************************************/
#include "ObjectTracker.h"
#include "utils.h"
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
/*
#define GetRValue(rgb)   ((UBYTE8) (rgb)) 
#define GetGValue(rgb)   ((UBYTE8) (((ULONG_32) (rgb)) >> 8)) 
#define GetBValue(rgb)   ((UBYTE8) ((rgb) >> 16)) 
*/
//#define RGB(r, g ,b)  ((ULONG_32) (((UBYTE8) (r) | ((UBYTE8) (g) << 8)) | (((ULONG_32) (UBYTE8) (b)) << 16))) 

#define min(a, b)  (((a) < (b)) ? (a) : (b))

#define max(a, b)  (((a) > (b)) ? (a) : (b)) 


#define  MEANSHIFT_ITARATION_NO 5
#define  DISTANCE_ITARATION_NO 1
#define  ALPHA 1
#define  EDGE_DETECT_TRESHOLD  32



CObjectTracker::CObjectTracker(INT32 imW,INT32 imH,IMAGE_TYPE eImageType)
{
 
  m_nImageWidth = imW;
  m_nImageHeight = imH;
  m_eIMAGE_TYPE = eImageType;
  m_cSkipValue = 0;

  for (UBYTE8 i=0;i<MAX_OBJECT_TRACK_NUMBER;i++)
  {
	  m_sTrackingObjectTable[i].Status = false;
      for(SINT16 j=0;j<HISTOGRAM_LENGTH;j++)
		  m_sTrackingObjectTable[i].initHistogram[j] = 0;
  }

  m_nFrameCtr = 0;	
  m_uTotalTime = 0;
  m_nMaxEstimationTime = 0;
  m_cActiveObject = 0;
	TotalDist=0.0;
	LastDist=0.0;

  switch (eImageType)
  {
   case MD_RGBA:
    m_cSkipValue = 4 ;
	break ;
   case MD_RGB:
	m_cSkipValue = 3 ;
   break ;
  };
};

CObjectTracker::~CObjectTracker()
{
 

}
//returns pixel values in format |0|B|G|R| wrt to (x.y)
/*
ULONG_32 CObjectTracker::GetPixelValues(UBYTE8 *frame,SINT16 x,SINT16 y)
{
 ULONG_32 pixelValues = 0;

 pixelValues = *(frame+(y*m_nImageWidth+x)*m_cSkipValue+2)|//0BGR
               *(frame+(y*m_nImageWidth+x)*m_cSkipValue+1) << 8|
			   *(frame+(y*m_nImageWidth+x)*m_cSkipValue) << 16;


 return(pixelValues);

}*/

//set RGB components wrt to (x.y)
void CObjectTracker::SetPixelValues(IplImage *r,IplImage *g,IplImage *b,ULONG_32 pixelValues,SINT16 x,SINT16 y)
{
//  *(frame+(y*m_nImageWidth+x)*m_cSkipValue+2) = UBYTE8(pixelValues & 0xFF);
//  *(frame+(y*m_nImageWidth+x)*m_cSkipValue+1) = UBYTE8((pixelValues >> 8) & 0xFF);
//  *(frame+(y*m_nImageWidth+x)*m_cSkipValue) = UBYTE8((pixelValues >> 16) & 0xFF); 
	//setpix32f
	setpix8c(r, y, x, UBYTE8(pixelValues & 0xFF));
	setpix8c(g, y, x, UBYTE8((pixelValues >> 8) & 0xFF));
	setpix8c(b, y, x, UBYTE8((pixelValues >> 16) & 0xFF));
}

// returns box color 
ULONG_32 CObjectTracker::GetBoxColor()
{
 ULONG_32 pixelValues = 0;

 switch(m_cActiveObject)
 {
 case 0:
	 pixelValues = RGB(255,0,0);
	 break;
 case 1:
	 pixelValues = RGB(0,255,0);
	 break;
 case 2:
	 pixelValues = RGB(0,0,255);
	 break;
 case 3:
	 pixelValues = RGB(255,255,0);
	 break;
 case 4:
	 pixelValues = RGB(255,0,255);
	 break;
 case 5:
	 pixelValues = RGB(0,255,255);
	 break;
 case 6:
	 pixelValues = RGB(255,255,255);
	 break;
 case 7:
	 pixelValues = RGB(128,0,128);
	 break;
 case 8:
	 pixelValues = RGB(128,128,0);
	 break;
 case 9:
	 pixelValues = RGB(128,128,128);
	 break;
 case 10:
	 pixelValues = RGB(255,128,0);
	 break;
 case 11:
	 pixelValues = RGB(0,128,128);
	 break;
 case 12:
	 pixelValues = RGB(123,50,10);
	 break;
 case 13:
	 pixelValues = RGB(10,240,126);
	 break;
 case 14:
	 pixelValues = RGB(0,128,255);
	 break;
 case 15:
	 pixelValues = RGB(128,200,20);
	 break;
 default:
	 break;
 }

 return(pixelValues);


}
//initializes the object parameters 
void CObjectTracker::ObjectTrackerInitObjectParameters(SINT16 x,SINT16 y,SINT16 Width,SINT16 Height)
{

   m_cActiveObject = 0;

   m_sTrackingObjectTable[m_cActiveObject].X = x;
   m_sTrackingObjectTable[m_cActiveObject].Y = y;
   m_sTrackingObjectTable[m_cActiveObject].W = Width;
   m_sTrackingObjectTable[m_cActiveObject].H = Height;

   m_sTrackingObjectTable[m_cActiveObject].vectorX = 0;
   m_sTrackingObjectTable[m_cActiveObject].vectorY = 0;

  
   m_sTrackingObjectTable[m_cActiveObject].Status = true;
   m_sTrackingObjectTable[m_cActiveObject].assignedAnObject = false;
}

void CObjectTracker::ObjeckTrackerHandlerByUser(IplImage *frame)
{
   m_cActiveObject = 0;

   if (m_sTrackingObjectTable[m_cActiveObject].Status)
   {
	   if (!m_sTrackingObjectTable[m_cActiveObject].assignedAnObject)
	   {
		   FindHistogram(frame,m_sTrackingObjectTable[m_cActiveObject].initHistogram);
           m_sTrackingObjectTable[m_cActiveObject].assignedAnObject = true;
	   }
	   else
	   {
		   FindNextLocation(frame);    

		   DrawObjectBox(frame);
	   }
   }

}
//Extracts the histogram of box
void CObjectTracker::FindHistogram(IplImage *frame, FLOAT32 (*histogram))
{
  SINT16  i = 0;
  SINT16  x = 0;
  SINT16  y = 0;
  UBYTE8  E = 0;
  UBYTE8  qR = 0,qG = 0,qB = 0;
//  ULONG_32 pixelValues = 0;
  UINT32 numberOfPixel = 0;
  IplImage* r, * g, * b;

	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 (i=0;i<HISTOGRAM_LENGTH;i++) //reset all histogram
	  histogram[i] = 0.0;

  //for all the pixels in the region
  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;//quantize R component
			qG = (UBYTE8)pixval8c( g, y, x )/16;//quantize G component
			qB = (UBYTE8)pixval8c( b, y, x )/16;//quantize B component

		  histogram[4096*E+256*qR+16*qG+qB] += 1;	//HISTOGRAM_LENGTH=8192

		  numberOfPixel++;

   }

  for (i=0;i<HISTOGRAM_LENGTH;i++) //normalize
	  histogram[i] = histogram[i]/numberOfPixel;

	cvReleaseImage(&r);
	cvReleaseImage(&g);
	cvReleaseImage(&b);

}
 //Draw box around object
void CObjectTracker::DrawObjectBox(IplImage *frame)
{
  SINT16 x_diff = 0;
  SINT16 x_sum  = 0;
  SINT16 y_diff = 0;
  SINT16 y_sum  = 0;
  SINT16  x = 0;
  SINT16  y = 0;
  ULONG_32 pixelValues = 0;
  IplImage* r, * g, * b;

	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 );

  pixelValues = GetBoxColor();

  //the x left and right bounds
  x_sum =  min(m_sTrackingObjectTable[m_cActiveObject].X+m_sTrackingObjectTable[m_cActiveObject].W/2+1,m_nImageWidth-1);