fnmatrix.cpp

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

// FnMatrix.cpp: implementation of the FnMatrix class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "windows.h"
#include "stdio.h"
#include "FnMatrix.h"
#include "FnMath.h"
#include "FnMathematical.h"
#include <math.h>
#include <complex>

static FnMath fMath;


/*BOOL Create_Active_List(Active_List_elem* min_cost,Graph_elem *first)
{
 min_cost=new Active_List_elem;
 min_cost->not_expanded=first;
 return true;
}*/

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

Active_List_elem* Add_Active_List(Active_List_elem* arrayElem,CPoint pixel)
{
 Active_List_elem* temp=new Active_List_elem;
 temp->pixel=pixel;
 temp->down=arrayElem;
 return temp;
}

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

BOOL Remove(Active_List_elem* remove_this,Active_List_elem** index_array)
{
  remove_this->pixel=(*index_array)->pixel;
  (*index_array)=(*index_array)->down;
  return true;
}

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

int Remove_to_Expand(int start_index,Active_List_elem** list_pointer)
{
  int index=start_index;
  while((list_pointer[index]==NULL) && (index<=Max_cost))
  {
	  index++;
  }
  if(index<=Max_cost) return index;
  else
  {
	  index=0;
	  while((list_pointer[index]==NULL) && (index<start_index))
	  {
		  index++;
	  }		
  }
  
  if(index!=start_index) return index;
  else return Max_cost+1; 
}

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

CPoint CursorSnap(CPoint seed,matrix &GradientMagnitude)
{
	int i,j,x_max,y_max;
	CPoint real_seed;

	real_seed=seed;
	y_max=GradientMagnitude.ysize;
	x_max=GradientMagnitude.xsize;
	if((seed.x>=2) && (seed.x<=x_max-3) &&(seed.y>=2) && (seed.y<=y_max-3) )
	{
		for(i=seed.x-2;i<seed.x+3;i++)
			for(j=seed.y-2;j<seed.y+3;j++)
			{
				if(GradientMagnitude(j,i)>GradientMagnitude(real_seed.y,real_seed.x))
				{
					real_seed.y=j;
					real_seed.x=i;
				}
			}
	}
	return real_seed;
}

//------------------------------------------------------------------------
void Dijkstra(CPoint seed,matrix &imgNext,Graph_elem **graph_array,Active_List_elem **index_array)
{
	int i,j,m,index,k=0;
	long int temp_cost=0; 
	CString info, message = "";
	CPoint point_to_expand,point_activelist;
	//CClientDC  dc(this);
	
	//dc.SetPixel(point.x,point.y,RGB(0,255,0));
	imgNext(seed.y,seed.x,0)=0;
	imgNext(seed.y,seed.x,1)=255;
	imgNext(seed.y,seed.x,2)=0;
	for(i=0;i<imgNext.ysize;i++)
		for(j=0;j<imgNext.xsize;j++)
		{
			graph_array[i][j].expand=false;
			graph_array[i][j].parent=NULL;
			graph_array[i][j].listElem=NULL;
			graph_array[i][j].cumul_cost=-1;
		}
	for(i=0;i<Max_cost+1;i++) index_array[i]=NULL;
	graph_array[seed.y][seed.x].expand=true;
	graph_array[seed.y][seed.x].cumul_cost=0;
		
	point_to_expand=seed;
		
	do
	{
	  m=0;
	  for(i=(point_to_expand.y-1);i<(point_to_expand.y+2);i++)
		 for(j=(point_to_expand.x-1);j<(point_to_expand.x+2);j++)
		 {
		  if((i>=0) && (j>=0) && (i<=imgNext.ysize-1) && (j<=imgNext.xsize-1))
		  {
			point_activelist.y=i;
			point_activelist.x=j;
			if(graph_array[i][j].expand==false)
			{
				temp_cost=graph_array[point_to_expand.y][point_to_expand.x].cumul_cost;
				temp_cost+=graph_array[point_to_expand.y][point_to_expand.x].local_cost[m];
				if(graph_array[i][j].parent==NULL)
				{
					graph_array[i][j].cumul_cost=temp_cost;
					graph_array[i][j].parent=&graph_array[point_to_expand.y][point_to_expand.x];
					index=temp_cost & Max_cost;
						//message.Format("%d %d %d %d",index,k,i,j);
						//MessageBox(message);
					index_array[index]=Add_Active_List(index_array[index],point_activelist);
					graph_array[i][j].listElem=index_array[index];
				}
				else
				{
					if(temp_cost<graph_array[i][j].cumul_cost)
					{
						index=graph_array[i][j].cumul_cost & Max_cost;
						graph_array[i][j].cumul_cost=temp_cost;
						graph_array[i][j].parent=&graph_array[point_to_expand.y][point_to_expand.x];
						Remove(graph_array[i][j].listElem,&index_array[index]);
						point_activelist=graph_array[i][j].listElem->pixel;
						delete graph_array[point_activelist.y][point_activelist.x].listElem;
						graph_array[point_activelist.y][point_activelist.x].listElem=graph_array[i][j].listElem;
						graph_array[i][j].listElem=NULL;
						index=temp_cost & Max_cost;
							//message.Format("%d %d %d %d",index,k,i,j);
							//MessageBox(message);
						point_activelist.y=i;
						point_activelist.x=j;
						index_array[index]=Add_Active_List(index_array[index],point_activelist);
						graph_array[i][j].listElem=index_array[index];
					}
				}
			}
		  }m++;
		 }
		 k++;
		 index=(graph_array[point_to_expand.y][point_to_expand.x].cumul_cost & Max_cost);
		 if(point_to_expand!=seed){
		  index_array[index]=index_array[index]->down;
		  delete graph_array[point_to_expand.y][point_to_expand.x].listElem;
		 }
		 index=Remove_to_Expand(index,&index_array[0]);
		 if(index!=Max_cost+1){
		   point_to_expand=index_array[index]->pixel;
		   graph_array[point_to_expand.y][point_to_expand.x].expand=true;
		 }
		 else k=imgNext.ysize*(imgNext.xsize);
				
	}while(k<imgNext.ysize*(imgNext.xsize)-1);

}
//---------------------------------------------------------------------------------------
matrix LiveWireDrawer(CPoint seedPrev,CPoint seedCurrent,matrix &imgNext,Graph_elem **graph_array,Active_List_elem **index_array,Contour *&Last)
{
	struct Live_wire_elem* temp;
	static struct Live_wire_elem* new_elem;
	CPoint free_point,current_point;
	Live_wire_elem* first;
	CString message; 
	Contour* bounelem;

	free_point.x=seedCurrent.x;
	free_point.y=seedCurrent.y;
			//	free_point=CursorSnap(free_point,GradientMagnitude);
	new_elem=new Live_wire_elem; 
	first=new_elem;
	first->pixel=free_point;
	if(imgNext.numbands==1)
	{
		first->red=imgNext(free_point.y,free_point.x);
	}
	else
	{
		first->red=imgNext(free_point.y,free_point.x,0);
		first->green=imgNext(free_point.y,free_point.x,1);
		first->blue=imgNext(free_point.y,free_point.x,2);
	}
	first->next=NULL;
			
	current_point=free_point;
	temp=first;
	while(graph_array[current_point.y][current_point.x].parent!=NULL)
	{
			
		if(current_point!=free_point)
		{
			new_elem=new Live_wire_elem; 
			temp->next=new_elem;
			temp->next->pixel=current_point;
			if(imgNext.numbands==1)
			{
				temp->next->red=imgNext(current_point.y,current_point.x);
			}
			else
			{
				temp->next->red=imgNext(current_point.y,current_point.x,0);
				temp->next->green=imgNext(current_point.y,current_point.x,1);
				temp->next->blue=imgNext(current_point.y,current_point.x,2);
			}
			temp=temp->next;
		}
		//dc.SetPixel(scale*current_point.x+xPosWindow,scale*current_point.y+yPosWindow,RGB(0,255,0));
		imgNext(current_point.y,current_point.x,0)=0;
		imgNext(current_point.y,current_point.x,1)=255;
		imgNext(current_point.y,current_point.x,2)=0;

		bounelem=new Contour;
		bounelem->boundary=current_point;
		bounelem->next_point=NULL;
		Last->next_point=bounelem;
		Last=bounelem;

		current_point=graph_array[current_point.y][current_point.x].parent->pixel;					
	}
	temp->next=NULL;
	return imgNext;
}



//---------------------------------------------------------------
matrix operator*(const matrix &a, const matrix &b) // a*b
{
	ASSERT( a.xsize == b.ysize);
	ASSERT( a.numbands == 1 && b.numbands == 1);
	
	double sum;
	register double *t1,*t2;
	matrix res(a.ysize,b.xsize);
	for(int k=0; k<a.ysize; k++)
	{
		for(int l=0;l<b.xsize; l++)
		{
			sum=0;
			t1=a.rowptr[k];
			t2=b.elem+l;
			for(int q=0;q<a.xsize;q++,t2+=b.xsize) 
				sum+= *t1++ * *t2;
			res[k][l]=sum;
		}
	}
	return(res);
}
//----------------------------------------------------------------------------------
matrix operator&(const matrix &a,const matrix &b) // a*b element by element
{
	ASSERT( a.xsize == b.xsize && a.ysize == b.ysize);
	ASSERT( a.numbands ==1 && b.numbands == 1);

	register double *t1,*t2, *t3;
	matrix res(a.ysize,a.xsize);
	t1=a.elem;
	t2=b.elem;
	t3=res.elem;
	for(int q=0;q<a.xsize*a.ysize;q++) *t3++=*t1++ * *t2++;
	return(res);
}
//----------------------------------------------------------------------------------

matrix operator/(const matrix& a, const matrix& b)  // a./b
{
	ASSERT( a.xsize == b.xsize && a.ysize == b.ysize);
	ASSERT( a.numbands ==1 && b.numbands == 1);

	register double *t1,*t2, *t3;
	matrix res(a.ysize,a.xsize);
	t1=a.elem;
	t2=b.elem;
	t3=res.elem;
	for(int q=0;q<a.xsize*a.ysize;q++) 
	{
		if (*t2 != 0)
			*t3++=*t1++ / *t2++ ;
		else 
		{
			*t3++=*t1++ / (*t2 + 1e-20) ;
			++t2;
		}
	}
	return(res);
}
//----------------------------------------------------------------------------------

matrix 	operator+(const matrix &a, const matrix &b) // a+b element by element
{
	ASSERT( a.xsize == b.xsize && a.ysize == b.ysize);
	ASSERT( a.numbands == b.numbands);
			
	register double *t1,*t2, *t3;
	matrix res(a.ysize, a.xsize, a.numbands);
	t1 = a.elem;
	t2 = b.elem;
	t3 = res.elem;
	for(int q=0; q < a.xsize * a.ysize * a.numbands; q++) 
		*t3++ = *t1++ + *t2++;
	return(res);
}
//----------------------------------------------------------------------------------

matrix 	operator+(const matrix &a, double sc)
{
	matrix res(a.ysize, a.xsize, a.numbands);
	register double *t1,*t2;
	int i;
	for(i = 0, t1 = res.elem, t2 = a.elem; i < a.ysize * a.xsize * a.numbands; i++) 
		*t1++ = *t2++ + sc;
	return(res);
}
//----------------------------------------------------------------------------------

matrix 	operator+(double sc,const matrix &a)
{
	ASSERT( a.numbands ==1 );	
	return(a+sc);
}
//----------------------------------------------------------------------------------

matrix operator-(const matrix &a, const matrix &b) // a-b element by element
{
	ASSERT( a.xsize == b.xsize && a.ysize == b.ysize);
	//ASSERT( a.numbands ==1 && b.numbands == 1);
	
	register double *t1,*t2, *t3;
	matrix res(a.ysize,a.xsize);
	t1=a.elem;
	t2=b.elem;
	t3=res.elem;
	for(int q=0;q<a.xsize*a.ysize;q++) 
		*t3++ = *t1++ - *t2++;
	return(res);
}
//----------------------------------------------------------------------------------

matrix 	operator-(const matrix &a, double sc)
{
	ASSERT( a.numbands == 1) ;
	return(a+(-1.0*sc));
}
//----------------------------------------------------------------------------------

matrix 	operator-(double sc,const matrix &a)
{
	ASSERT( a.numbands == 1) ;
	return(a-sc);
}
//----------------------------------------------------------------------------------

matrix operator*(const matrix &a,double sc) 
{ 
	register double *t1,*t2;
	matrix res(a.ysize, a.xsize, a.numbands);
	int i;

	for(t1 = a.elem, t2 = res.elem, i = 0; i < a.ysize * a.xsize * a.numbands; i++)
		*t2++= sc* *t1++;
	return(res);
}
//----------------------------------------------------------------------------------

matrix operator*(double sc, const matrix &a) 
{
	return a*sc;
}
//----------------------------------------------------------------------------------

matrix operator/(const matrix &a,double sc) 
{
	ASSERT( a.numbands == 1);
	return a*(1.0/sc);
}
//----------------------------------------------------------------------------------

matrix 	operator/(double sc, const matrix &a) 
{
	ASSERT( a.numbands == 1);
	register double *t1,*t2;
	matrix res(a.ysize,a.xsize);
	int i;
	
	for(t1=a.elem,t2=res.elem,i=0;i<a.ysize*a.xsize;i++)
		*t2++= sc /  *t1++;
	return(res);
}
//----------------------------------------------------------------------------------
matrix filterGaussSmooth( double spread)
{
	ASSERT( spread >= 3.0);

	//double sigma = spread / 6.0;

	double sigma = (spread-1) / 6.0;

	double sigmaSquared = sigma*sigma;
	int radius = (int) (3*sigma+0.5);  // Half-size of kernel
	int size = 1+2*radius;  // Size of kernel
	float val, sum;
	int m;
	
	// create nx1 matrix to hold the smoothing filter
	// smoothing filter = exp(-(x*x)/(2*sigma*sigma))
	matrix coeff(size,1);
	
	sum = 0.0;
	for( m = 1; m <= radius; m++)
	{
		val = (float) exp(-((double)(m*m))/(2.0*sigmaSquared)); // Gaussian
		sum += val;
		coeff( radius + m) = val;
	}
	
	// Normalize smoothing kernel so that its coefficients add up to 1.
	sum = 2 * sum + 1;
	coeff( radius) = (float)(1.0 / sum);
	for( m = 1; m <= radius; m++)
	{
		coeff( radius + m) /= sum;
		coeff( radius - m) = coeff( radius + m);
	}
	
	return coeff;
}
//----------------------------------------------------------------------------------
matrix filterGaussDeriv( double spread)
{
	ASSERT( spread >= 3.0);

	//double sigma = spread / 6.0;
	double sigma = (spread - 1) / 6.0;
		
	double sigmaSquared = sigma*sigma;
	int radius = (int) (3*sigma+0.5);  // Half-size of kernel
	int size = 1+2*radius;  // Size of kernel
	float val, sum;
	int m;
	
	// create nx1 matrix to hold the differentiating filter
	// differentiating filter = x/(sigma*sigma) * exp(-(x*x)/(2*sigma*sigma))
	matrix coeff(size,1);
	
	sum = 0.0;
	for( m = 1; m <= radius; m++)
	{
		val = (float) exp(-((double)(m*m))/(2.0*sigmaSquared)); // Gaussian
		sum += val;
		coeff( radius + m) = (float) (val * m / sigmaSquared); // Gaussian
	}
	
	// Normalize differentiating filter.
	sum = 2 * sum + 1;
	coeff( radius) = 0.0;
	for( m = 1; m <= radius; m++)
	{
		coeff( radius + m) /= sum;