mstracker.cpp

基于均值漂移理论的该进算法

//meanshift.cpp - a meanshift tracker
//Iain Wallace, 08/2005
//Based on the equations in "kernel-based object tracking"
//D. Comaniciu, V. Ramesh, P. Meer, IEEE Trans. Pattern
//Analysis and Machine Intelligence, vol25 (5),pp564-577,
//August 2003
//References to equations, and numbers, refer to this paper.
//Also used matlab code by Zsolt L. Husz from HWU for reference
//Some notes on terminology.
//"window" refers to the user-defined region covering the target.
//TODO put references in to the eqns in the paper

// Remodified by xdb 2007-03-19, the perform programs are all in one file of Mstracker.cpp. But it is not to 
// readed or modified. So I modify the codes and built an class named CMsTracker;

#include "stdafx.h"
#include "MsTracker.h"
#include "math.h"
using namespace std;


CMsTracker:: CMsTracker(void)
{	
	m_centerx	= 0;
	m_centery	= 0;
	m_sizex		= 0;
	m_sizey		= 0;

	weight_array	= NULL;
	kernel_array	= NULL;
	kernelDeriv_array	= NULL;	

	dx = dy = pdx = pdy = 0;
}
CMsTracker::~CMsTracker()
{
	int i = 0;

	if( weight_array != NULL )
	{
		for( i = 0;i < m_sizex ; i++)
		{
			delete[] weight_array[i];			
		}
		delete []weight_array;
	}
	if( kernel_array != NULL )
	{
		for( i = 0; i < m_sizex; i++ )
			delete []kernel_array[i];

		delete []kernel_array;
	}
	if( kernelDeriv_array != NULL )
	{
		for( i = 0; i < m_sizex; i++ )
			delete []kernelDeriv_array[i];

		delete []kernelDeriv_array;
	}
	
}
BOOL CMsTracker::InitTracker(CRect trackRect, BYTE *pImage, UINT bytesPerLine, UINT imageWidth,
								UINT imageHeight)
{
	if( trackRect.bottom - trackRect.top < 0 || trackRect.right - trackRect.left < 0 )
		return FALSE;
	
	m_imageHeight = imageHeight;
	m_imageWidth	= imageWidth;

	/*m_trackRect.bottom = trackRect.bottom;
	m_trackRect.top		= trackRect.top;
	m_trackRect.left	= trackRect.left;
	m_trackRect.right	= trackRect.right;*/

	//calculate the centre of the window, and the half-size (size from centre to edge)
	//Note this implicitly makes the window an odd size, which is required so there is
	//a centre pixel (not halfway)
	m_centerx	= (int)floor(( trackRect.right - trackRect.left)/2.0) + trackRect.left;
	m_centery	= (int)floor(( trackRect.bottom - trackRect.top)/2.0) + trackRect.top;
	m_halfsizex	= m_centerx - trackRect.left;
	m_halfsizey	= m_centery - trackRect.top;

	m_sizex		= 2 * m_halfsizex + 1;
	m_sizey		= 2 * m_halfsizey + 1;
	
	int i = 0;
	if( weight_array != NULL )
	{
		for ( i = 0; i < m_sizex; i++)
			delete[] weight_array[i];

		delete []weight_array;
		weight_array = NULL;
	}
	weight_array = new double*[m_sizex];
	ZeroMemory( weight_array, m_sizex * sizeof( double*));

	for ( i = 0; i < m_sizex; i++)
	{
		weight_array[i] = new double[m_sizey];
		ZeroMemory( weight_array[i], m_sizey* sizeof( double ));
	}
	//declare a 2d array the size of the window, and fill it with the kernel function
	if( kernel_array != NULL )
	{
		for( i = 0;i < m_sizex ; i++)
		{
			delete[] kernel_array[i];			
		}
		delete []kernel_array;
	}
	kernel_array = new double*[m_sizex];
	ZeroMemory(kernel_array, m_sizex * sizeof(double*));

	for ( i =  0; i < m_sizex;i++)
	{
		kernel_array[i] = new double[m_sizey];
		ZeroMemory( kernel_array[i], m_sizey * sizeof(double) );
	}
	//the value of the kernal is static, as the only variable is the position of the pixels
	//relative to the centre, and the window size is constant.
	evalKernel(&kernel_array,m_halfsizex, m_halfsizey);
	
	if( kernelDeriv_array != NULL )
	{
		for( i = 0; i < m_sizex; i++ )
			delete []kernelDeriv_array[i];

		delete []kernelDeriv_array;
	}
	kernelDeriv_array = new int*[m_sizex];
	ZeroMemory(kernelDeriv_array, m_sizex * sizeof( int* ));

	for ( i = 0; i < m_sizex; i++)
	{	
		kernelDeriv_array[i] = new int[m_sizey];
		ZeroMemory(kernelDeriv_array[i], m_sizey * sizeof(int) );
	}
	evalKernelDeriv(&kernelDeriv_array,m_halfsizex, m_halfsizey);


	Qu.updateModel(pImage, m_centerx,m_centery,m_halfsizex, m_halfsizey, &kernel_array, bytesPerLine);
	DrawFrame(pImage, m_centerx, m_centery, m_halfsizex, m_halfsizey, bytesPerLine);

	return TRUE;

}
BOOL CMsTracker::UpdateTracker( int loopMax, BYTE *pImage, UINT bytesPerLine, int tolerWidth )
{
	int loopCount = 0;
	bool exit = false;
	bool noexsit = false;

	while (exit == false)
	{
		pdx = dx;
		pdy = dy;
		loopCount++;
		Pu.updateModel(pImage, m_centerx, m_centery, m_halfsizex, m_halfsizey,&kernel_array, bytesPerLine);
		updateWeights(&weight_array, &Pu, &Qu, m_sizex,m_sizey);
		//now compute the displacement
		computeDisplacement(&weight_array, &kernelDeriv_array, m_centerx, m_centery, m_halfsizex, m_halfsizey, &dx, &dy);		
		m_centerx += dx;
		m_centery += dy;
		//Check if we've converged
		//Also, strictly a better convergence rule could be used
		// - see steps 4-6 on p567 in the paper.
		//There is a check for "oscillation" due to rounding errors.
		if (((dx == 0) && (dy == 0)) || (loopCount > loopMax) || ( (pdx + dx == 0) && (pdy+dy==0) ))
			exit = true;

		//if (((dx == 0) && (dy == 0)) || ( (pdx + dx == 0) && (pdy+dy==0) ))
		//	noexsit = true;	
		int tmpw = dx + m_halfsizex;
		int tmpH = dy + m_halfsizey;

		if( m_centerx - tmpw < tolerWidth || m_centerx + tmpw > m_imageWidth - tolerWidth
			|| m_centery - tmpH < tolerWidth || m_centery + tmpH > m_imageHeight - tolerWidth)
		{
			exit = TRUE;						
			noexsit = TRUE;
		}
	}
	//if ( !noexsit )
	//{
		DrawFrame(pImage, m_centerx, m_centery, m_halfsizex, m_halfsizey, bytesPerLine);
	//}
	return noexsit;
}
double CMsTracker::kernel(int x,int y,int half_x,int half_y)
{
	//This comes from a simplified version of eqn(12).
	//Note that this makes use of the fact that a lot of the kernel terms cancel out,
	//as it is primarily used in eqn(2) and eqn(3).
	//the distance to the point, normalised to unit radius from the centre
	double euclideanDistance = sqrt( pow( ( (double)(x)/(double)(half_x) ) ,2.0) +pow( ( (double)(y)/(double)(half_y) ) ,2.0) );
	if (euclideanDistance > 1)
		return( 0.0);
	else
		return(1.0-pow(euclideanDistance,2));
}
void CMsTracker::evalKernel (double*** kArray,int half_x,int half_y)
{
	//This function calculates the Epanechnikov kernel over
	//the size of the window.
	//x and y vary accoding to local co-ords with 0,0 at the centre
	for (int x = -half_x; x < half_x; x++)
	{
		for (int y = -half_y; y < half_y; y++)
		{
			(*kArray)[x+half_x][y+half_y] = kernel(x,y,half_x,half_y);
		}
	}
}
void CMsTracker::evalKernelDeriv (int*** kArray,int half_x,int half_y)
{
	//This function calculates the derivative of the Epanechnikov kernel over
	//the size of the window. Appears as "g" in the paper.
	//x and y vary accoding to local co-ords with 0,0 at the centre
	double euclideanDistance;
	for (int x = -half_x;x < half_x; x++)
	{
		for (int y = -half_y;y < half_y; y++)
		{
			euclideanDistance = sqrt( pow( ( (double)(x)/(double)(half_x) ) ,2.0) +pow( ( (double)(y)/(double)(half_y) ) ,2.0) );
			if (euclideanDistance > 1)
				(*kArray)[x+half_x][y+half_y] = 0;
			else
				(*kArray)[x+half_x][y+half_y] = 1;
		}
	}
}
// My program is not used to save the result, so I rewrite the function to realize highlight the tracking area.
/*
void CMsTracker::writeFrame(CImg<unsigned char>* frame,int centre_x,int centre_y,int half_size_x,int half_size_y,string name)
{
	const unsigned char colour[3]={255,0,0};
	int x1 = centre_x - half_size_x;
	int y1 = centre_y - half_size_y;
	int x2 = centre_x + half_size_x;
	int y2 = centre_y + half_size_y;
	//Highlight the target in a fetching transparent red
	(*frame).draw_rectangle(x1,y1,x2,y2,colour,0.4);
	//This code writes the frame (with box) to a file.
	//Filename will be the original, with "_out" appended before the extension.
	string fileName(name);
	fileName.insert((name.size()-4),"_out");
	(*frame).save(fileName.c_str());
}*/
void CMsTracker::DrawFrame( BYTE *pImage, int centre_x,int centre_y,int half_size_x,int half_size_y, UINT bytesPerLine )
{
	int y, x;
	y = x = 0;
	for(  y = centre_y - half_size_y ; y <= centre_y + half_size_y; y++ )
	{
		int p = y * bytesPerLine;
		for( x = centre_x -half_size_x, p += x * 3; x<= centre_x + half_size_x; x++, p += 3)
		{			
			*( pImage + p )	= 0;
			*( pImage + p + 1 )	= 0;
			*( pImage + p + 2 )	= 255;				
		}
	}
	/*int starty = centre_y - half_size_y;
	int	endy = centre_y + half_size_y;
	int startx = centre_x - half_size_x;
	int endx = centre_x + half_size_x;

	for( y = starty; y < endy; y++ )
	{
		int p = y * bytesPerLine;
		for( x = startx, p += startx * 3; x < endx; x++, p +=3 )
		{
			*(pImage + p) = 0;
			*(pImage + p + 1) = 0;
			*(pImage + p + 2) = 255;
		}
	}*/
}

void CMsTracker::updateWeights(double*** weights,colourModel *Pu,colourModel *Qu,unsigned int x_size,unsigned int y_size)
{
	//This calculates the pixel weights for the window, based on the colour model.
	//Described in eqn(10)
	double R[NUMBINS];
	for (int i = 0;i< NUMBINS;i++)
	{
		//Note if there're no pixels in a bin we'll never need to use the value,
		//so set corresponding R to 0
		//Necessary, as otherwise there will be divide-by-zero errors.
		if ((*Pu)[i]==0)
		{
			R[i] = 0.0;
		}
		else
		{
			R[i] = sqrt( ((*Qu)[i]/(*Pu)[i]) );
		}
	}
	for (unsigned int x = 0;x<x_size; x++)
	{
		for (unsigned int y = 0;y<y_size; y++)
		{
			(*weights)[x][y] = R[(*Pu).theBin(x,y)];
		}
	}
}
void CMsTracker::computeDisplacement(double*** weights,int*** kArray,unsigned int centre_x,unsigned int centre_y,int half_x,int half_y,int *dx,int *dy)
{
	double weight_sum = 0;
	double x_sum =0, y_sum=0;
	double curPixelWeight;
	for (int x = -half_x;x < half_x; x++)
	{
		for (int y = -half_y;y < half_y; y++)
		{
			//the bottom half of eqn(11) (sum the weights under the kernel)
		//	curPixelWeight = (*weights)[x+half_x+1][y+half_y+1]*(*kArray)[x+half_x+1][y+half_y+1];

			curPixelWeight = (*weights)[x+half_x][y+half_y]*(*kArray)[x+half_x][y+half_y];
			weight_sum += curPixelWeight;
			//The top half of Eqn(11)
			x_sum += x*curPixelWeight;
			y_sum += y*curPixelWeight;
		}
	}
	//do the division
	*dx = (int)floor(x_sum/weight_sum);
	*dy = (int)floor(y_sum/weight_sum);
}
void CMsTracker::ReleaseTracker()
{
	CMsTracker::~CMsTracker();
	weight_array	= NULL;
	kernel_array	= NULL;
	kernelDeriv_array	= NULL;	
}