fnmatrix.cpp

在人脸检测的基础之上,对嘴部的运动表情进行分析,进行语音模拟.

		coeff( radius - m) = - coeff( radius + m);
	}
	
	return coeff;
	
}
//----------------------------------------------------------------------------------
matrix Laplacian(matrix &img,int kernel_size)
{
  int N=img.ysize;
  int M=img.xsize;     
  int lower=(kernel_size-1)/2;
  double sigma=kernel_size/5;//(1+0.5*(kernel_size-5))/3; //standard deviation
  double r_square=0;
  matrix coeff=matrix(kernel_size,kernel_size);
  int i,j,m,k,band_no;
  matrix A=matrix(img.ysize,img.xsize,img.numbands);
  
  for (i=0;i<kernel_size;i++)
     for (j=0;j<kernel_size;j++)  //kernel will be generated
	 {
       r_square=pow(lower-i,2)+pow(lower-j,2);
       coeff(i,j)=(-1*((r_square-2*pow(sigma,2))/pow(sigma,4)));
       coeff(i,j)*=	exp(-1*r_square/(2*pow(sigma,2))); 
	 }
  

  for(band_no=0;band_no<img.numbands;band_no++) //kernel will be convolved   
   for(i=lower;i<(N-lower);i++)                 //with each color band of image
      for(j=lower;j<(M-lower);j++)				//seperately.
         for(m=i-lower;m<=i+lower;m++)
            for(k=j-lower;k<=(j+lower);k++)
               A(i,j,band_no)=A(i,j,band_no)+(coeff(m-i+lower,k-j+lower)*img(m,k,band_no));

       
  matrix h=A;
  
  for(band_no=0;band_no<img.numbands;band_no++) //A binary local cost feature
     for(i=0;i<N;i++)							//will be created for each 
        for(j=0;j<M;j++)						//color band
           if(A(i,j,band_no)>0)
           {
		    for(m=i-1;m<=i+1;m++)
               for(k=j-1;k<=j+1;k++)
			   {
                  if((A(m,k,band_no)<0) && (abs(A(m,k,band_no))<abs(A(i,j,band_no))))
                     h(m,k,band_no)=0;
                  else if((A(m,k,band_no)<0) && (abs(A(m,k,band_no))>=abs(A(i,j,band_no)))) 
                     h(i,j,band_no)=0;
				}
           }
            
         
   for(band_no=0;band_no<img.numbands;band_no++)
	  for(i=0;i<N;i++)
         for(j=0;j<M;j++)
            if(h(i,j,band_no)!=0)   h(i,j,band_no)=1;
   
   
   if(img.numbands==1)return h; 
   else		
   {
    matrix f=matrix(img.ysize,img.xsize,1); //all elements of f are 
    for(i=0;i<N;i++)		//zero initially.If a point is not a zero	
         for(j=0;j<M;j++)	//crossing on any of the bands, it is 1.
			if((h(i,j,0)*h(i,j,1)*h(i,j,2))!=0)  f(i,j,0)=1;			
		 
	return f;
   
   }
}
//------------------------------------------------------------------
double** Gradient_Mag(matrix &img,matrix &GradientMagnitude,int kernel_size)
{
  int N=img.ysize;
  int M=img.xsize;     
  int lower=(kernel_size-1)/2;
  double sigma=(kernel_size)/5;
  double r_square=0;
  matrix coeff_x=matrix(kernel_size,kernel_size);
  matrix coeff_y=matrix(kernel_size,kernel_size);
  int i,j,m,k,band_no,min,max;
  matrix Ax=matrix(img.ysize,img.xsize,img.numbands);
  matrix Ay=matrix(img.ysize,img.xsize,img.numbands);
  matrix gradMag=matrix(img.ysize,img.xsize,img.numbands);
  static matrix cost=matrix(img.ysize,img.xsize,1);
  static matrix Ix=matrix(img.ysize,img.xsize,1);
  static matrix Iy=matrix(img.ysize,img.xsize,1);
  static double* grads[3];

  for (i=0;i<kernel_size;i++)
     for (j=0;j<kernel_size;j++)
	 {
       r_square=pow(lower-i,2)+pow(lower-j,2);
       coeff_x(i,j)=(lower-j)/pow(sigma,2);
       coeff_y(i,j)=(i-lower)/pow(sigma,2);
	   coeff_x(i,j)*=exp(-1*r_square/(2*pow(sigma,2))); 
	   coeff_y(i,j)*=exp(-1*r_square/(2*pow(sigma,2)));
	 }
  
  for(band_no=0;band_no<img.numbands;band_no++)    
     for(i=lower;i<(N-lower);i++)
        for(j=lower;j<(M-lower);j++)
           for(m=i-lower;m<=i+lower;m++)
              for(k=j-lower;k<=(j+lower);k++)
			  {
                 Ax(i,j,band_no)=Ax(i,j,band_no)+(coeff_x(m-i+lower,k-j+lower)*img(m,k,band_no));
                 Ay(i,j,band_no)=Ay(i,j,band_no)+(coeff_y(m-i+lower,k-j+lower)*img(m,k,band_no)); 
                 gradMag(i,j,band_no)=sqrt(pow(Ax(i,j,band_no),2)+pow(Ay(i,j,band_no),2));
			  }

   
  for(i=0;i<N;i++)
     for(j=0;j<M;j++)
	 {
		cost(i,j)=0;	 
	 }

  for(i=0;i<N;i++)
     for(j=0;j<M;j++)
		for(band_no=0;band_no<img.numbands;band_no++)
			if(cost(i,j)<gradMag(i,j,band_no)) 
			{
				cost(i,j)=gradMag(i,j,band_no);
				Ix(i,j)=(1/cost(i,j))*Ax(i,j,band_no);
				Iy(i,j)=(1/cost(i,j))*Ay(i,j,band_no);
			}

   /*for(i=0;i<N;i++)
     for(j=0;j<M;j++)
	     if(GradientMagnitude(i,j)<cost(i,j))
		 {
			GradientMagnitude(i,j)=cost(i,j); 
		 }*/
   
   min=MinFind(cost,0);
   for(i=0;i<N;i++)
      for(j=0;j<M;j++) 
		 cost(i,j)-=min;

   max=MaxFind(cost,0);
   for(i=0;i<N;i++)
      for(j=0;j<M;j++)
	  {
	   cost(i,j)/=max;
	   if(GradientMagnitude(i,j)<cost(i,j))
	   {
		  GradientMagnitude(i,j)=cost(i,j); 
	   }
       cost(i,j)=1-cost(i,j);
	  }

  grads[0]=(cost.elem);
  grads[1]=(Ix.elem);
  grads[2]=(Iy.elem);
  return &grads[0];
} 

//------------------------------------------------------------------
void Gradient_Dir(CPoint q,int N,int M,double **grads,double* local_cost,double coeff)
{    
  CPoint p;
  double temp_cost;
 

  if((q.x>0) && (q.y>0)) 
  {
	  p.x=q.x-1;
	  p.y=q.y-1;

	  if((grads[2][N*p.y+p.x]-grads[1][N*p.y+p.x])>=0)
	  {
		  temp_cost=acos(0.707*(grads[2][N*p.y+p.x]-grads[1][N*p.y+p.x]));
		  temp_cost+=acos(0.707*(grads[2][N*q.y+q.x]-grads[1][N*q.y+q.x]));
		  temp_cost=2*temp_cost*coeff/(3*PI);//fD(p,q)*m_editGradDir
		  temp_cost+=grads[0][N*q.y+q.x];//wN*fG(q) added
		  local_cost[0]+=temp_cost;
		  local_cost[0]=ceil(255*local_cost[0]);
	  }
	  else
	  {
		  temp_cost=acos(-0.707*(grads[2][N*p.y+p.x]-grads[2][N*p.y+p.x]));
		  temp_cost+=acos(-0.707*(grads[2][N*q.y+q.x]-grads[2][N*q.y+q.x]));
		  temp_cost=2*temp_cost*coeff/(3*PI);//fD(p,q)*m_editGradDir
		  temp_cost+=grads[0][N*q.y+q.x];//wN*fG(q) added
		  local_cost[0]+=temp_cost;
		  local_cost[0]=ceil(255*local_cost[0]);
	  }
  }
	
  if((q.y>0)) 
  {
	  p.x=q.x;
	  p.y=q.y-1;

	  if((grads[1][N*p.y+p.x])<=0)
	  {
		  temp_cost=acos(-1*grads[1][N*p.y+p.x]);
		  temp_cost+=acos(-1*grads[1][N*q.y+q.x]);
		  temp_cost=2*temp_cost*coeff/(3*PI);//fD(p,q)*m_editGradDir
		  temp_cost+=0.707*grads[0][N*q.y+q.x];//wN*fG(q) added
		  local_cost[1]+=temp_cost;
		  local_cost[1]=ceil(255*local_cost[1]);
	  }
	  else
	  {
		  temp_cost=acos(grads[1][N*p.y+p.x]);
		  temp_cost+=acos(grads[1][N*q.y+q.x]);
		  temp_cost=2*temp_cost*coeff/(3*PI);//fD(p,q)*m_editGradDir
		  temp_cost+=0.707*grads[0][N*q.y+q.x];//wN*fG(q) added
		  local_cost[1]+=temp_cost;  
		  local_cost[1]=ceil(255*local_cost[1]);
	  }
  }

  if((q.x<N-1) && (q.y>0)) 
  {
	  p.x=q.x+1;
	  p.y=q.y-1;

	  if((grads[1][N*p.y+p.x]+grads[2][N*p.y+p.x])<=0)
	  {
		  temp_cost=acos(-0.707*(grads[1][N*p.y+p.x]+grads[2][N*p.y+p.x]));
		  temp_cost+=acos(-0.707*(grads[1][N*q.y+q.x]+grads[2][N*q.y+q.x]));
		  temp_cost=2*temp_cost*coeff/(3*PI);//fD(p,q)*m_editGradDir
		  temp_cost+=grads[0][N*q.y+q.x];//wN*fG(q) added
		  local_cost[2]+=temp_cost;
		  local_cost[2]=ceil(255*local_cost[2]);
	  }
	  else
	  {
		  temp_cost=acos(0.707*(grads[1][N*p.y+p.x]+grads[2][N*p.y+p.x]));
		  temp_cost+=acos(0.707*(grads[1][N*q.y+q.x]+grads[2][N*q.y+q.x]));
		  temp_cost=2*temp_cost*coeff/(3*PI);//fD(p,q)*m_editGradDir
		  temp_cost+=grads[0][N*q.y+q.x];//wN*fG(q) added
		  local_cost[2]+=temp_cost;
		  local_cost[2]=ceil(255*local_cost[2]);
	  }
  }

  if((q.x>0)) 
  {
	  p.x=q.x-1;
	  p.y=q.y;

	  if((grads[2][N*p.y+p.x])>=0)
	  {
		  temp_cost=acos(grads[2][N*p.y+p.x]);
		  temp_cost+=acos(grads[2][N*q.y+q.x]);
		  temp_cost=2*temp_cost*coeff/(3*PI);//fD(p,q)*m_editGradDir
		  temp_cost+=0.707*grads[0][N*q.y+q.x];//wN*fG(q) added
		  local_cost[3]+=temp_cost;
		  local_cost[3]=ceil(255*local_cost[3]);
	  }
	  else
	  {
		  temp_cost=acos(-1*grads[2][N*p.y+p.x]);
		  temp_cost+=acos(-1*grads[2][N*q.y+q.x]);
		  temp_cost=2*temp_cost*coeff/(3*PI);//fD(p,q)*m_editGradDir
		  temp_cost+=0.707*grads[0][N*q.y+q.x];//wN*fG(q) added
		  local_cost[3]+=temp_cost;  
		  local_cost[3]=ceil(255*local_cost[3]);
	  }
  }
  
  if((q.x<N-1)) 
  {
	  p.x=q.x+1;
	  p.y=q.y;

	  if((grads[2][N*p.y+p.x])<=0)
	  {
		  temp_cost=acos(-1*grads[2][N*p.y+p.x]);
		  temp_cost+=acos(-1*grads[2][N*q.y+q.x]);
		  temp_cost=2*temp_cost*coeff/(3*PI);//fD(p,q)*m_editGradDir
		  temp_cost+=0.707*grads[0][N*q.y+q.x];//wN*fG(q) added
		  local_cost[5]+=temp_cost;
		  local_cost[5]=ceil(255*local_cost[5]);
	  }
	  else
	  {
		  temp_cost=acos(grads[2][N*p.y+p.x]);
		  temp_cost+=acos(grads[2][N*q.y+q.x]);
		  temp_cost=2*temp_cost*coeff/(3*PI);//fD(p,q)*m_editGradDir
		  temp_cost+=0.707*grads[0][N*q.y+q.x];//wN*fG(q) added
		  local_cost[5]+=temp_cost;  
		  local_cost[5]=ceil(255*local_cost[5]);
	  }
  }

  if((q.x>0) && (q.y<M-1)) 
  {
	  p.x=q.x-1;
	  p.y=q.y+1;

	  if((grads[2][N*p.y+p.x]+grads[1][N*p.y+p.x])>=0)
	  {
		  temp_cost=acos(0.707*(grads[2][N*p.y+p.x]+grads[1][N*p.y+p.x]));
		  temp_cost+=acos(0.707*(grads[2][N*q.y+q.x]+grads[1][N*q.y+q.x]));
		  temp_cost=2*temp_cost*coeff/(3*PI);//fD(p,q)*m_editGradDir
		  temp_cost+=grads[0][N*q.y+q.x];//wN*fG(q) added
		  local_cost[6]+=temp_cost;
		  local_cost[6]=ceil(255*local_cost[6]);
	  }
	  else
	  {
		  temp_cost=acos(-0.707*(grads[2][N*p.y+p.x]+grads[1][N*p.y+p.x]));
		  temp_cost+=acos(-0.707*(grads[2][N*q.y+q.x]+grads[1][N*q.y+q.x]));
		  temp_cost=2*temp_cost*coeff/(3*PI);//fD(p,q)*m_editGradDir
		  temp_cost+=grads[0][N*q.y+q.x];//wN*fG(q) added
		  local_cost[6]+=temp_cost;
		  local_cost[6]=ceil(255*local_cost[6]);
	  }
  }

  if((q.y<M-1)) 
  {
	  p.x=q.x;
	  p.y=q.y+1;

	  if((grads[1][N*p.y+p.x])>=0)
	  {
		  temp_cost=acos(grads[1][N*p.y+p.x]);
		  temp_cost+=acos(grads[1][N*q.y+q.x]);
		  temp_cost=2*temp_cost*coeff/(3*PI);//fD(p,q)*m_editGradDir
		  temp_cost+=0.707*grads[0][N*q.y+q.x];//wN*fG(q) added
		  local_cost[7]+=temp_cost;
		  local_cost[7]=ceil(255*local_cost[7]);
	  }
	  else
	  {
		  temp_cost=acos(-1*grads[1][N*p.y+p.x]);
		  temp_cost+=acos(-1*grads[1][N*q.y+q.x]);
		  temp_cost=2*temp_cost*coeff/(3*PI);//fD(p,q)*m_editGradDir
		  temp_cost+=0.707*grads[0][N*q.y+q.x];//wN*fG(q) added
		  local_cost[7]+=temp_cost;  
		  local_cost[7]=ceil(255*local_cost[7]);
	  }
  }

 if((q.x<N-1) && (q.y<M-1)) 
  {
	  p.x=q.x+1;
	  p.y=q.y+1;

	  if((grads[1][N*p.y+p.x]-grads[2][N*p.y+p.x])>=0)
	  {
		  temp_cost=acos(0.707*(grads[1][N*p.y+p.x]-grads[2][N*p.y+p.x]));
		  temp_cost+=acos(0.707*(grads[1][N*q.y+q.x]-grads[2][N*q.y+q.x]));
		  temp_cost=2*temp_cost*coeff/(3*PI);//fD(p,q)*m_editGradDir
		  temp_cost+=0.707*grads[0][N*q.y+q.x];//wN*fG(q) added
		  local_cost[8]+=temp_cost;
		  local_cost[8]=ceil(255*local_cost[8]);
	  }
	  else
	  {
		  temp_cost=acos(-0.707*(grads[1][N*p.y+p.x]-grads[2][N*p.y+p.x]));
		  temp_cost+=acos(-0.707*(grads[1][N*q.y+q.x]-grads[2][N*q.y+q.x]));
		  temp_cost=2*temp_cost*coeff/(3*PI);//fD(p,q)*m_editGradDir
		  temp_cost+=0.707*grads[0][N*q.y+q.x];//wN*fG(q) added
		  local_cost[8]+=temp_cost;
		  local_cost[8]=ceil(255*local_cost[8]);
	  }
  }
}
//------------------------------------------------------------------
matrix filterReduce()
{
	matrix reduce( 7, 1);
	
	reduce(0) = -0.125;
	reduce(1) = 0.0;
	reduce(2) = 0.375;
	reduce(3) = 0.5;
	reduce(4) = 0.375;
	reduce(5) = 0.0;
	reduce(6) = -0.125;
	
	return reduce;
}

//----------------------------------------------------------------------------------
matrix filterExpand()
{
	matrix expand( 7, 1);
	
	expand(0) = -0.25;
	expand(1) = 0.0;
	expand(2) = 0.75;
	expand(3) = 1.0;
	expand(4) = 0.75;
	expand(5) = 0.0;
	expand(6) = -0.25;
	
	return expand;
}

//----------------------------------------------------------------------------------
matrix  operator|(matrix &a, matrix &b) //convolve a and b
{
	ASSERT( b.numbands == 1);

	// a|b convolve matrix a with filter b.
	// if filter b is column, vertical filtering, else horizontal filtering.
	
	int i,j,k,l,flag,strt,strt1,strt2,indx,jndx;
	//int m,n;
	
	double multp;
	//register int *t1,*t2;
	
	int ysize=a.ysize;
	int xsize=a.xsize;
	int numbands = a.numbands;
	int n;
	if (b.xsize==1) 
	{ // vertical filter
		ASSERT( b.ysize%2 == 1);
		flag = 1;
		strt = (int)((b.ysize-1) / 2.0);
	} 
	else if (b.ysize==1) 
	{ // horizontal filter
		ASSERT( b.xsize%2 == 1);
		flag = 0;
		strt = (int)((b.xsize-1) / 2.0);
	} 
	else 
	{
		ASSERT( b.ysize%2 == 1);
		ASSERT( b.xsize%2 == 1);
		flag = 2;
		strt1 = (int)((b.ysize-1) / 2.0);
		strt2 = (int)((b.xsize-1) / 2.0);
	}
	matrix outfilt = zeros(ysize,xsize,numbands);
	
	
	if (flag==1) 
	{
		for( n = 0; n < numbands; n++)
		{
			for ( i=0; i<xsize; i++)
			{
				for ( j=0; j<ysize; j++) 
				{
					for ( k=-strt; k<=strt; k++) 
					{
						indx = j-k;
						if(indx<0)
						{
							indx = 0;
						}
						else if(indx >= a.ysize)
						{
							indx = a.ysize - 1;
						}
						multp = b(k+strt,0)*a(indx,i,n);
						outfilt(j,i,n) += multp;
					}
				}
			}
		}
	} 
	else if (flag==0)
	{
		for( n = 0; n < numbands; n++)
		{
			for ( j=0; j<ysize; j++) 
			{
				for ( i=0; i<xsize; i++)
				{
					for ( k=-strt; k<=strt; k++) 
					{
						indx = i-k;
						if(indx<0)
						{
							indx = 0;
						}
						else if(indx >= a.xsize)
						{
							indx = a.xsize - 1;
						}