snakeimage.h

opencv实现的人体运动跟踪源码

#define SNAKE_BIG 2.e+38f
#include "Image.h"
#include <math.h>
#define MIN(a,b)  ((a) > (b) ? (b) : (a))
#define MAX(a,b)  ((a) > (b) ? (a) : (b))

BOOL
SnakeImage(    CImage * img,
               CPoint * pt,
               int n,
               float alpha,
               float beta,
               float gamma,
               CSize win)
{    /* check bad arguments */
    if( img == NULL ){
		AfxMessageBox("图像未加载!");
        return false;
	}
	if( img->Bits() != 8){
		AfxMessageBox("非单通道图像");
		return false;
	}	
    if( pt == NULL ){
		AfxMessageBox("未初始化控制点!");
        return false;
	}
    if( n < 3 ){
		AfxMessageBox("控制点过少!");
        return false;
	}   
    if( alpha ==0 || beta ==0 || gamma == 0 ){
		AfxMessageBox("初始权重错误!");
        return false;
	}   
    if( (win.cx <= 0) || (!(win.cx & 1))||(win.cy <= 0) || (!(win.cy & 1))){
        AfxMessageBox("梯度窗口初始化失败!");
        return false;
	}
    else{

		int i, j, k;
		int neighbors = win.cx * win.cy;

		int centerx = win.cx >> 1;
		int centery = win.cy >> 1;

		float invn;
		int iteration = 0;
		int converged = 0;
		BYTE* src = img->Data();
		int width = img->Width();

		invn = 1 / ((float) n);
		float *Econt;
		float *Ecurv;
		float *Eimg;
		float *E;
		Econt = new float[neighbors * sizeof( float )];
		Ecurv = new float[neighbors * sizeof( float )];
		Eimg =  new float[neighbors * sizeof( float )];
		E =     new float[neighbors * sizeof( float )];

		while( !converged )
		{
			float ave_d = 0;
			int moved = 0;

			converged = 0;
			iteration++;
			/* compute average distance */
			for( i = 1; i < n; i++ )
			{
				int diffx = pt[i - 1].x - pt[i].x;
				int diffy = pt[i - 1].y - pt[i].y;

				ave_d += sqrt( (float) (diffx * diffx + diffy * diffy) );
			}
			ave_d += sqrt( (float) (  (pt[0].x - pt[n - 1].x) * (pt[0].x - pt[n - 1].x) +
									  (pt[0].y - pt[n - 1].y) * (pt[0].y - pt[n - 1].y)));

			ave_d *= invn;
			/* average distance computed */
			for( i = 0; i < n; i++ )
			{
				/* Calculate Econt */
				float maxEcont = 0;
				float maxEcurv = 0;
				float maxEimg = 0;
				float minEcont = SNAKE_BIG;
				float minEcurv = SNAKE_BIG;
				float minEimg = SNAKE_BIG;
				float Emin = SNAKE_BIG;

				int offsetx = 0;
				int offsety = 0;
				float tmp;

				/* compute bounds */
				int left =  MIN( pt[i].x, win.cx >> 1 );
				int right = MIN( img->Width() - 1 - pt[i].x, win.cx >> 1 );
				int upper = MIN( pt[i].y, win.cy >> 1 );
				int bottom =MIN( img->Height() - 1 - pt[i].y, win.cy >> 1 );

 
				for( j = -upper; j <= bottom; j++ )
				{
					for( k = -left; k <= right; k++ )
					{
						int diffx, diffy;
						float energy;

						if( i == 0 )
						{
							diffx = pt[n - 1].x - (pt[i].x + k);
							diffy = pt[n - 1].y - (pt[i].y + j);
						}
						else
						{
							diffx = pt[i - 1].x - (pt[i].x + k);
							diffy = pt[i - 1].y - (pt[i].y + j);
						}
						Econt[(j + centery) * win.cx + k + centerx] = energy =
							(float) fabs( ave_d -
										  sqrt( (float) (diffx * diffx + diffy * diffy) ));

						maxEcont = MAX( maxEcont, energy );
						minEcont = MIN( minEcont, energy );
					}
				}
				tmp = maxEcont - minEcont;
				tmp = (tmp == 0) ? 0 : (1 / tmp);
				//归一化
				for( k = 0; k < neighbors; k++ )
				{
					Econt[k] = (Econt[k] - minEcont) * tmp;
				}

				/*  Calculate Ecurv */
				for( j = -upper; j <= bottom; j++ )
				{
					for( k = -left; k <= right; k++ )
					{
						int tx, ty;
						float energy;

						if( i == 0 )
						{
							tx = pt[n - 1].x - 2 * (pt[i].x + k) + pt[i + 1].x;
							ty = pt[n - 1].y - 2 * (pt[i].y + j) + pt[i + 1].y;
						}
						else if( i == n - 1 )
						{
							tx = pt[i - 1].x - 2 * (pt[i].x + k) + pt[0].x;
							ty = pt[i - 1].y - 2 * (pt[i].y + j) + pt[0].y;
						}
						else
						{
							tx = pt[i - 1].x - 2 * (pt[i].x + k) + pt[i + 1].x;
							ty = pt[i - 1].y - 2 * (pt[i].y + j) + pt[i + 1].y;
						}
						Ecurv[(j + centery) * win.cx + k + centerx] = energy =
							(float) (tx * tx + ty * ty);
						maxEcurv = MAX( maxEcurv, energy );
						minEcurv = MIN( minEcurv, energy );
					}
				}
				tmp = maxEcurv - minEcurv;
				tmp = (tmp == 0) ? 0 : (1 / tmp);
				for( k = 0; k < neighbors; k++ )
				{
					Ecurv[k] = (Ecurv[k] - minEcurv) * tmp;
				}


				/* Calculate Eimg */
				for( j = -upper; j <= bottom; j++ )
				{
					for( k = -left; k <= right; k++ )
					{
						float energy;
						int position;
						int dx,dy;
						position= (pt[i].y+j)*width + pt[i].x + k;
						dx = src[position - 1] - src[position +1];
						dy = src[position - width] - src[position + width];

						//Eimg[(j + centery) * win.cx + k + centerx] = energy =
						//      (float)(dx * dx + dy * dy);
						Eimg[(j + centery) * win.cx + k + centerx] = energy =
							   (float)( fabs(dx) + fabs(dy) );   

						maxEimg = MAX( maxEimg, energy );
						minEimg = MIN( minEimg, energy );
					}
				}

				tmp = (maxEimg - minEimg);
				tmp = (tmp == 0) ? 0 : (1 / tmp);
				for( k = 0; k < neighbors; k++ )
				{
					Eimg[k] = (minEimg - Eimg[k]) * tmp;
				}

				/* Find Minimize point in the neighbors */
				for( k = 0; k < neighbors; k++ )
				{
					E[k] = alpha * Econt[k] + beta * Ecurv[k] + gamma * Eimg[k];
				}
				Emin = SNAKE_BIG;
				for( j = -upper; j <= bottom; j++ )
				{
					for( k = -left; k <= right; k++ )
					{
						float energy = E[(j + centery) * win.cx + k + centerx];
						if(  energy< Emin )
						{
							Emin = energy;
							offsetx = k;
							offsety = j;
						}
					}
				}

				if( offsetx || offsety )
				{
					pt[i].x += offsetx;
					pt[i].y += offsety;
					moved++;
				}
			}
			converged = (moved == 0);
			if( iteration >= 500) //经验值 
				converged = 1;
		}

		delete []Econt ;
		delete []Ecurv ;
		delete []Eimg ;
		delete []E ;
		return true;
	}
}