ctrack.cpp

国外一个大牛人写的MEAN-SHIFT目标跟踪算法

#include "CTrack.h"
#include "definitions.h"

//---------------------------------------------------------------------------

#define TRACKING_OUTPUT 1

CTrack::CTrack ()
{
}

CTrack::~CTrack ()
{
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Kronecker delta function's implementation
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
float CTrack::kronecker_delta_function
(float color1,float color2)
{
 	return color1==color2;
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Eponechnikov profile's implementation for 2D (x>=0)
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
float CTrack::eponechnikov_profile
(float x)
{
	float volume_of_unit_d_dimensional_sphere=M_PI; //since d=2
	if (x<1) return 2.0*(1-x)/(volume_of_unit_d_dimensional_sphere);
    else	 return 0.0;
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Uniform profile's implementation for 2D (x>=0)
// derivative of eponechnikov profile	(don't put minus)
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
float CTrack::derivative_of_eponechnikov_profile
(float x)
{
	float volume_of_unit_d_dimensional_sphere=M_PI; //since d=2
	if (x<1) return 2.0/(volume_of_unit_d_dimensional_sphere);
    else	 return 0.0;
}


//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Calculate constant c used in model density function
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
float CTrack::constant_C()
{
	int x,y;
    CPixel pixel;
    float sum=0.0;
    for (y=-model_window_radius;y<=model_window_radius;y++)
    	for (x=-model_window_radius;x<=model_window_radius;x++)
        {	pixel.x=(float)x/(float)model_window_radius;
        	pixel.y=(float)y/(float)model_window_radius;
            sum+=eponechnikov_profile( pow(pixel.length(),2) );
        }
    return 1/(3*sum);
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Calculate constant ch used in candidate density function
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
float CTrack::constant_Ch()
{
	int x,y;
    CPixel pixel;
    float sum=0.0;
    for (y=-candidate_window_radius;y<=candidate_window_radius;y++)
    	for (x=-candidate_window_radius;x<=candidate_window_radius;x++)
        {	pixel.x=(float)x/(float)candidate_window_radius;
        	pixel.y=(float)y/(float)candidate_window_radius;
            sum+=eponechnikov_profile( pow(pixel.length(),2) );
        }
    return 1/(3*sum);
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Calculate model's density estimation q
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void CTrack::model_density_function_q
( )
{
	int 	x,y,u;
    CPixel 	pixel;
    int 	r_color,g_color,b_color;
    float 	color_dividor = 256/number_of_bins;
    float 	epanechnikov_number;
    float 	constant_c=constant_C(); //normalization constant
	int 	rows=FLIR_image.Nrows(),cols=FLIR_image.Ncols();

    for (u=0;u<number_of_bins;u++)
        FLIR_model_color_probabilities[u][0]=
        FLIR_model_color_probabilities[u][1]=
        FLIR_model_color_probabilities[u][2]=0.0;

    for (y=-model_window_radius;y<=model_window_radius&&y+target_center.y<rows;y++)
    	for (x=-model_window_radius;x<=model_window_radius&&x+target_center.x<cols;x++)
        {	pixel.x=(float)x/(float)model_window_radius;
        	pixel.y=(float)y/(float)model_window_radius;
            r_color =(int)(FLIR_image.R(y+(int)target_center.y,x+(int)target_center.x)/color_dividor);
            g_color =(int)(FLIR_image.G(y+(int)target_center.y,x+(int)target_center.x)/color_dividor);
            b_color =(int)(FLIR_image.B(y+(int)target_center.y,x+(int)target_center.x)/color_dividor);

			epanechnikov_number=eponechnikov_profile( pow(pixel.length(),2) );
            FLIR_model_color_probabilities[r_color][0]+=epanechnikov_number*constant_c;
            FLIR_model_color_probabilities[g_color][1]+=epanechnikov_number*constant_c;
            FLIR_model_color_probabilities[b_color][2]+=epanechnikov_number*constant_c;
        }
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Calculate candidate's density estimation q
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void CTrack::candidate_density_function_p
( )
{
	int 	x,y,u;
    CPixel 	pixel;
    int 	r_color,g_color,b_color;
    float 	color_dividor = 256/number_of_bins;
    float 	epanechnikov_number;
    float 	constant_ch=constant_Ch(); //normalization constant
	int 	rows=FLIR_image.Nrows(),cols=FLIR_image.Ncols();

    for (u=0;u<number_of_bins;u++)
        FLIR_candidate_color_probabilities[u][0]=
        FLIR_candidate_color_probabilities[u][1]=
        FLIR_candidate_color_probabilities[u][2]=0.0;

    for (y=-candidate_window_radius;y<=candidate_window_radius;y++)
    	for (x=-candidate_window_radius;x<=candidate_window_radius;x++)
        {   if (y+floor(candidate_center.y+0.5)<rows&&y+floor(candidate_center.y+0.5)>=0 &&x+floor(candidate_center.x+0.5)<cols&&x+floor(candidate_center.x+0.5)>=0)
        	{
        		pixel.x=(float)x/(float)candidate_window_radius;
        		pixel.y=(float)y/(float)candidate_window_radius;
            	r_color=(int)(FLIR_image.R(y+(int)floor(candidate_center.y+0.5),x+(int)floor(candidate_center.x+0.5))/color_dividor);
            	g_color=(int)(FLIR_image.G(y+(int)floor(candidate_center.y+0.5),x+(int)floor(candidate_center.x+0.5))/color_dividor);
            	b_color=(int)(FLIR_image.B(y+(int)floor(candidate_center.y+0.5),x+(int)floor(candidate_center.x+0.5))/color_dividor);
				epanechnikov_number=eponechnikov_profile( pow(pixel.length(),2) )*constant_ch;
                FLIR_candidate_color_probabilities[r_color][0]+=epanechnikov_number;
                FLIR_candidate_color_probabilities[g_color][1]+=epanechnikov_number;
                FLIR_candidate_color_probabilities[b_color][2]+=epanechnikov_number;
            }
        }
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// calculate the bhattacharyya coefficient
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
float CTrack::bhattacharyya_coefficient
( )
{
	int u;
    float flir_sum=0.0;
    for (u=0;u<number_of_bins;u++)
    	flir_sum+=sqrt(	FLIR_model_color_probabilities[u][0]*
        				FLIR_candidate_color_probabilities[u][0] )+
        		  sqrt(	FLIR_model_color_probabilities[u][1]*
        				FLIR_candidate_color_probabilities[u][1] )+
        		  sqrt(	FLIR_model_color_probabilities[u][2]*
        				FLIR_candidate_color_probabilities[u][2] );
    return (   flir_sum  ) ;
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// determine the weights for calculating the mean shift
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void CTrack::calculate_weights
( )
{   int x,y;
    float flir_weight;
    CPixel pixel;
	float r_color,g_color,b_color;
    float color_dividor=256/number_of_bins;
	int rows=FLIR_image.Nrows(),cols=FLIR_image.Ncols();

    for (y=-candidate_window_radius;y<=candidate_window_radius;y++)
    	for (x=-candidate_window_radius;x<=candidate_window_radius;x++)
        {   pixel.x=(float)x/(float)candidate_window_radius;
        	pixel.y=(float)y/(float)candidate_window_radius;
            flir_weight=0.0;
            if ( pixel.length()<1 &&
            	 y+floor(candidate_center.y+0.5)<rows&&y+floor(candidate_center.y+0.5)>=0 &&
                 x+floor(candidate_center.x+0.5)<cols&&x+floor(candidate_center.x+0.5)>=0)
            {
            	r_color=(int)( FLIR_image.R(y+(int)floor(candidate_center.y+0.5),x+(int)floor(candidate_center.x+0.5))/color_dividor);
            	g_color=(int)( FLIR_image.G(y+(int)floor(candidate_center.y+0.5),x+(int)floor(candidate_center.x+0.5))/color_dividor);
            	b_color=(int)( FLIR_image.B(y+(int)floor(candidate_center.y+0.5),x+(int)floor(candidate_center.x+0.5))/color_dividor);
            	flir_weight=(sqrt( FLIR_model_color_probabilities[(int)r_color][0]/(FLIR_candidate_color_probabilities[(int)r_color][0]+1e-4))+
                    		 sqrt( FLIR_model_color_probabilities[(int)g_color][1]/(FLIR_candidate_color_probabilities[(int)g_color][1]+1e-4))+
                    		 sqrt( FLIR_model_color_probabilities[(int)b_color][2]/(FLIR_candidate_color_probabilities[(int)b_color][2]+1e-4)))/3.0;
            }
            FLIR_weights    [y+candidate_window_radius][x+candidate_window_radius]=flir_weight;
        }
}


//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// calculate the new location of the target
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void CTrack::determine_new_candidate_location
( )
{
    int 	x,y;
    CPixel 	pixel;
    float 	denominator=0.0,numarator_x=0.0,numarator_y=0.0;
    float 	weighted;
    for (y=-candidate_window_radius;y<=candidate_window_radius;y++)
    	for (x=-candidate_window_radius;x<=candidate_window_radius;x++)
        {   pixel.x=(float)x/(float)candidate_window_radius;
        	pixel.y=(float)y/(float)candidate_window_radius;
			weighted=
            		FLIR_weights[y+candidate_window_radius][x+candidate_window_radius]*
                	derivative_of_eponechnikov_profile( pow(pixel.length(),2) );
            denominator+=weighted;
            numarator_x+=weighted*(x+floor(candidate_center.x+0.5));
            numarator_y+=weighted*(y+floor(candidate_center.y+0.5));
        }
    if (numarator_x!=0 || numarator_y!=0)
    {
    candidate_center.x = numarator_x/denominator;
    candidate_center.y = numarator_y/denominator;
    }
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// algorithm for the estimated location of the target in two frames
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
float CTrack::track_target_in_consecutive_frames
( )
{
    int   i,iter_cnt=0,inner_iter_cnt;
    float distance;
    float bhattacharyya_coefficient_1;
    float bhattacharyya_coefficient_2;
    //initialize the location of the target in the current frame
    candidate_center=target_center;

    do
    {   //calculate the distrubution for the target at candidate location
    	candidate_density_function_p();
        //evaluate the bhattacharyya coefficient
        bhattacharyya_coefficient_1 = bhattacharyya_coefficient();
//--------------------------------------------------------------------
        FLIR_weights = new float*[2*candidate_window_radius+1];
        for (i=0;i<2*candidate_window_radius+1;i++)
        	FLIR_weights[i] = new float[2*candidate_window_radius+1];