correlation.cpp

提取超声回波信号中回波时移的互相关计算。

#include "optimi.h"
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include "bmptest.h"
#include <math.h>

#define FTOL 1.0e-5;
#define NDIM 6;
typedef unsigned char BYTE;

struct Point
{
	double x;
	double y;
	double z;

};

struct Point2D
{
	double x;
	double y;
};
//clare the moving image, float image, and source image, this includes 
//image size, center and so on. x stands for col, y stands for row and z stands for slice.


Point tcenter;
Point mcenter;
Point fcenter;

Point tsize;
Point msize;
Point fsize;

//the data memory to save the 3D image and one slice of the raw image
BYTE **Target;
BYTE **Float;
BYTE **Move;
BYTE ***TargetData;
BYTE ***FloatData;
BYTE ***MoveData;


int biInterpolate(Point2D t);
double objective2D(double p[]);
double objective3D(double p[]);
bool PointInF(Point Pt);
bool Point2DInF(Point2D Pt);
void resultimage(double p[]);
void result2dimage(double p[]);
void Transform(double p[], Point Tin, Point &Fout);
void Transform2D(double p[], Point Tin, Point &Fout);
int TriInterpolate(Point s);


   int targetx=181;
   int targety=217;
   int movex=181;
   int movey=217;
   int floatx=181;
   int floaty=217;

   int tband=181;
   int trow=217;
   int tcol=181;
   int fband=181;
   int frow=217;
   int fcol=181;
   int mband=181;
   int mrow=217;
   int mcol=181;

int main()
{

   tcenter.x=(float)tcol/2;
   tcenter.y=(float)trow/2;
   mcenter.x=(float)fcol/2;
   mcenter.y=(float)frow/2;
   fcenter.x=(float)fcol/2;
   fcenter.y=(float)frow/2;

   tsize.x=1.0;
   tsize.y=1.0;
   tsize.z=1.0;

   msize.x=1.0;
   msize.y=1.0;
   msize.z=1.0;

   fsize.x=1.0;
   fsize.y=1.0;
   fsize.z=1.0;

   int i,j;
//construct 2D image data space   
   Target=new BYTE *[trow];
   for(i=0;i<trow;i++)
   {
	   Target[i]=new BYTE [tcol];
   }

   Float=new BYTE *[frow];
   for(i=0;i<frow;i++)
   {
	   Float[i]=new BYTE [fcol];
   }
    
   Move=new BYTE *[mrow];
   for(i=0;i<mrow;i++)
   {
	   Move[i]=new BYTE [mcol];
   }
//construct 3D image data space
   TargetData=new BYTE **[tband];
   for(i=0;i<tband;i++)
   {
	   TargetData[i]=new BYTE *[trow];
	   for(j=0;j<trow;j++)
	   {
		   TargetData[i][j]=new BYTE [tcol];
	   }
   }

   FloatData=new BYTE **[fband];
   for(i=0;i<fband;i++)
   {
	   FloatData[i]=new BYTE *[frow];
	   for(j=0;j<frow;j++)
	   {
		   FloatData[i][j]=new BYTE [fcol];
	   }
   }

   MoveData=new BYTE **[mband];
   for(i=0;i<mband;i++)
   {
	   MoveData[i]=new BYTE *[mrow];
	   for(j=0;j<mrow;j++)
	   {
		   MoveData[i][j]=new BYTE [mcol];
	   }
   }
//open and read the data

   FILE *T;
   T=fopen("t1.img","rb");
   for(i=0;i<tband;i++)
   {
	   for(j=0;j<trow;j++)
	   {
		   fread(TargetData[i][j],sizeof(BYTE),tcol,T);
	   }
   }
   fclose(T);

   for(i=0;i<trow;i++)
   {
	   for(j=0;j<tcol;j++)
	   {
           Target[i][j]=TargetData[89][i][j];
	   }
   }

   FILE *F;
   F=fopen("3xt1.img","rb");
   for(i=0;i<fband;i++)
   {
	   for(j=0;j<frow;j++)
	   {
		   fread(FloatData[i][j],sizeof(BYTE),fcol,F);
	   }
   }
   fclose(F);

   for(i=0;i<frow;i++)
   {
	   for(j=0;j<fcol;j++)
	   {
		   Float[i][j]=FloatData[89][i][j];
	   }
   }

   for(i=0;i<mrow;i++)
   {
	   for(j=0;j<mcol;j++)
	   {
		   Move[i][j]=0;
	   }
   }

   double p[7];
   for(i=0;i<7;i++)
	   p[i]=0.0;

 
/*
   double p[4];
   for(i=0;i<4;i++)
	   p[i]=0.0;
*/

//optimize 
	//iter is the number of iterations taken
	int iter;
	//fret is the returned function value at p
	double fret;

	//xi is the then-current direction set
	double **six; 
	double **three;

	six=matrix(1,6,1,6);
	three=matrix(1,3,1,3);
	for (i=1;i<=6;i++)
	{
		for (j=1;j<=6;j++)
		{
			six[i][j]=(i == j ? 1.0 : 0.0);
		}
	}

	for(i=1;i<=3;i++)
	{
		for(j=1;j<=3;j++)
		{
			three[i][j]=(i==j?1.0:0.0);
		}
	}
    powell(p,six,6,1.0e-5,&iter,&fret,objective3D);
//	powell(p,three,3,1.0e-5,&iter,&fret,objective2D);
	free_matrix(six,1,6,1,6);
	free_matrix(three,1,3,1,3);
//save the result 2Dtransform
	FILE *result;
	result=fopen("transformresult.txt","w");
	for(i=1;i<7;i++)
	{
		fprintf(result,"p[%d]=%f\n",i,p[i]);
	}
/*
	for(i=1;i<4;i++)
	{
		fprintf(result,"p[%d]=%f\n",i,p[i]);
	}
*/
/*
    save the result 3Dtransform
    FILE *result;
	result=fopen("transformresult.txt","w");
	for(i=1;i<7;i++)
	{
	   fprintf(result,"p[%d]=%f\n",i,p[i]);
	}
*/
//free the memory

   for(i=0;i<trow;i++)
   {
	   delete[] Target[i];
   }
   delete[] Target;

   for(i=0;i<frow;i++)
   {
	   delete[] Float[i];
   }
   delete[] Float;

   for(i=0;i<mrow;i++)
   {
	   delete[] Move[i];
   }
   delete[] Move;
   
   for(i=0;i<tband;i++)
   {
	   for(j=0;j<trow;j++)
	   {
		   delete[] TargetData[i][j];
	   }
	   delete[] TargetData[i];

   }
   delete[] TargetData;

   for(i=0;i<fband;i++)
   {
	   for(j=0;j<frow;j++)
	   {
		   delete[] FloatData[i][j];
	   }
	   delete[] FloatData[i];
   }
   delete[] FloatData;

   for(i=0;i<mband;i++)
   {
	   for(j=0;j<mrow;j++)
	   {
		   delete[] MoveData[i][j];
	   }
   }

   return 0;

}


void Transform(double p[], Point Tin, Point &Fout)
{
    double tx,ty,tz;
	double rx,ry,rz;
	tx=p[1];
	ty=p[2];
	tz=p[3];
	rx=p[4];
	ry=p[5];
	rz=p[6];

	Tin.x-=tcenter.x;
	Tin.y-=tcenter.y;
	Tin.z-=tcenter.z;

	Tin.x*=tsize.x;
	Tin.y*=tsize.y;
	Tin.z*=tsize.z;

	Fout.x=(Tin.x+tx)*cos(ry)*cos(rz)+(Tin.y+ty)*cos(ry)*sin(rz)+(Tin.z+tz)*sin(ry);
	Fout.y=(Tin.x+tx)*(sin(rx)*sin(ry)*cos(rz)-cos(rx)*sin(rz))+
		   (Tin.y+ty)*(sin(rx)*sin(ry)*sin(rz)+cos(rx)*cos(rz))+
		   (Tin.z+tz)*sin(rx)*cos(ry);
	Fout.z=(Tin.x+tx)*(cos(rx)*sin(ry)*cos(rz)+sin(rx)*sin(rz))+
		   (Tin.y+ty)*(cos(rx)*sin(ry)*sin(rz)-sin(rx)*cos(rz))+
		   (Tin.z+tz)*cos(rx)*cos(ry);

	Fout.x/=fsize.x;
	Fout.y/=fsize.y;
	Fout.z/=fsize.z;

	Fout.x+=fcenter.x;
	Fout.y+=fcenter.y;
	Fout.z+=fcenter.z;

}

void Transform2D(double p[], Point2D Tin, Point2D &Fout)
{
    double tx,ty;
	double rz;
	tx=p[1];
	ty=p[2];
	rz=p[3];
	
	Tin.x-=tcenter.x;
	Tin.y-=tcenter.y;
	Tin.x*=tsize.x;
	Tin.y*=tsize.y;

	Fout.x=(Tin.x+tx)*cos(rz)+(Tin.y+ty)*sin(rz);
	Fout.y=(Tin.x+tx)*(-sin(rz))+(Tin.y+ty)*cos(rz);
	
	Fout.x/=fsize.x;
	Fout.y/=fsize.y;
	Fout.x+=fcenter.x;
	Fout.y+=fcenter.y;

}

bool PointInF(Point Pt)
{
	double x=Pt.x;
	double y=Pt.y;
	double z=Pt.z;

	if(x<0||x>fcol-1||y<0||y>frow-1||z<0||z>fband-1)
		return false;
	else 
		return true;

}

bool Point2DInF(Point2D Pt)
{
	double x=Pt.x;
	double y=Pt.y;

	if(x<0||x>fcol-1||y<0||y>frow-1)
		return false;
	else
		return true;
}

int TriInterpolate(Point s)
{
   double x,y,z;
   x=s.x;
   y=s.y;
   z=s.z;
   int Ix,Iy,Iz;
   Ix=(int)x;
   Iy=(int)y;
   Iz=(int)z;

   double dx,dy,dz;
   dx=x-Ix;
   dy=y-Iy;
   dz=z-Iz;
   double mx,my,mz;
   mx=1-dx;
   my=1-dy;
   mz=1-dz;
   
   int finalinterpolate;

   if(x==fcol-1||y==frow-1||z==fband-1)
	   finalinterpolate=0;
   else
   {
       Point n[9];
  //Point n1,n2,n3,n4,n5,n6,n7,n8;
       n[0].x=0;
       n[0].y=0;
       n[0].z=0;
       n[1].x=Ix;
       n[1].y=Iy;
       n[1].z=Iz;
       n[2].x=Ix+1;
       n[2].y=Iy;
       n[2].z=Iz;
       n[3].x=Ix+1;
       n[3].y=Iy+1;
       n[3].z=Iz;
       n[4].x=Ix;
       n[4].y=Iy+1;
       n[4].z=Iz;
       n[5].x=Ix;
       n[5].y=Iy+1;
       n[5].z=Iz+1;
       n[6].x=Ix;
       n[6].y=Iy;
       n[6].z=Iz+1;
       n[7].x=Ix+1;
       n[7].y=Iy;
       n[7].z=Iz+1;
       n[8].x=Ix+1;
       n[8].y=Iy+1;
       n[8].z=Iz+1;

       int value[9];
       value[0]=0;
       int i;
       int m1,m2,m3;
       for(i=1;i<9;i++)
	   {
	     m1=(int)n[i].z;
	     m2=(int)n[i].y;
	     m3=(int)n[i].x;
	     value[i]=FloatData[m1][m2][m3];
	   }
       double w[9];
       w[0]=0;
// w[i] stands for the weight which is near the point i, just for the spatial position weight
       w[1]=dx*dy*dz;
       w[2]=mx*dy*dz;
       w[3]=mx*my*dz;
       w[4]=dx*my*dz;
       w[5]=dx*my*mz;
       w[6]=dx*dy*mz;
       w[7]=mx*dy*mz;
       w[8]=mx*my*mz;
   
       double result;
       result=value[1]*w[8]+value[2]*w[5]+value[3]*w[6]+value[4]*w[7]+
	          value[8]*w[1]+value[5]*w[2]+value[6]*w[3]+value[7]*w[4];
   
       finalinterpolate=(int)result;
   }
   
   return finalinterpolate;
}

int biInterpolate(Point2D t)
{
   double x,y;
   x=t.x;
   y=t.y;

   int finalInterpolate;
   
   if(x==fcol-1||y==frow-1)
	   finalInterpolate=0;
   else
   {
	   int Ix,Iy;
       Ix=(int)x;
       Iy=(int)y;
 
       double dx,dy;
       dx=x-Ix;
       dy=y-Iy;
       double mx,my;
       mx=1-dx;
       my=1-dy;

       Point2D n[5];
       n[0].x=0;
       n[0].y=0;
       n[1].x=Ix;
       n[1].y=Iy;
       n[2].x=Ix+1;
       n[2].y=Iy;
       n[3].x=Ix+1;
       n[3].y=Iy+1;
       n[4].x=Ix;
       n[4].y=Iy+1;

       int value[5];
       value[0]=0;
       int i;
       int m1,m2;
       for(i=1;i<5;i++)
	   {
	     m1=(int)n[i].y;
	     m2=(int)n[i].x;
  	     value[i]=(int)Float[m1][m2];
	   }
//the weight is just based on the position
      double w[5];
      w[0]=0.0;
      w[1]=dx*dy;
      w[2]=mx*dy;
      w[3]=mx*my;
      w[4]=dx*my;
   
     double result;
     result=value[1]*w[3]+value[2]*w[4]+value[3]*w[1]+value[4]*w[2];
     
     finalInterpolate=(int)result;
   }

     return finalInterpolate;
}

void resultimage(double p[])
{
   int i,j,k;
   for(i=0;i<tband;i++)
   {
	   for(j=0;j<trow;j++)
	   {
           for(k=0;k<tcol;k++)
		   {
               Point T,F;
			   T.x=k;
			   T.y=j;
			   T.z=i;
			   F.x=0;
			   F.y=0;
			   F.z=0;
			   Transform(p,T,F);
			   if(!PointInF(F))
				   MoveData[i][j][k]=0;
			   else
				   MoveData[i][j][k]=TriInterpolate(F);

		   }
	   }
   }

    FILE *result;
	result=fopen("newimage.img","wb");
	for(i=0;i<mband;i++)
	{
		for(j=0;j<mrow;j++)
		{
			fwrite(MoveData[i][j],sizeof(BYTE),mcol,result);
		}
	}

	fclose(result);   

}


void result2dimage(double p[])
{
    int i,j;
  
	for(i=0;i<trow;i++)
	{
       for(j=0;j<tcol;j++)
	   {
          Point2D T,F;
		  T.x=j;
		  T.y=i;
		  F.x=0;
		  F.y=0;
/*
		  p[0]=0.0;
		  p[1]=5.2;
		  p[2]=4.5;
		  p[3]=0.5;
*/
		  Transform2D(p,T,F);
		  if(!Point2DInF(F))
//			 MoveData[0][i][j]=0;
             Move[i][j]=0;
		  else
//		     MoveData[0][i][j]=biInterpolate(F);
             Move[i][j]=biInterpolate(F);

		}
	}
  
/*	
    for(i=0;i<trow;i++)
	{
		for(j=0;j<tcol;j++)
		{
			printf("Move [%d][%d]is %d\n",i,j,Move[i][j]);
		}
	}
*/

    FILE *result;
	result=fopen("new2Dimage.img","wb");
	
	for(j=0;j<mrow;j++)
	{
		fwrite(MoveData[0][j],sizeof(BYTE),mcol,result);
	}
	

	fclose(result); 
}



double objective3D(double p[])
{
	float tmean,Mmean;
	int tsum,Msum,tnum,Mnum;
	tsum=0;
	Msum=0;
	tnum=tband*trow*tcol;
	Mnum=mband*mrow*mcol;

	resultimage(p);

	int i,j,k;
	for(i=0;i<tband;i++)
	{
		for(j=0;j<trow;j++)
		{
			for(k=0;k<tcol;k++)
			{
				tsum+=TargetData[i][j][k];
				Msum+=MoveData[i][j][k];
			}
		}
	}

	tmean=(float)tsum/tnum;
	Mmean=(float)Msum/Mnum;

	double Ncc;
	Ncc=0;
	float numerator;
	numerator=0;
	float denominator1,denominator2;
	denominator1=0;
	denominator2=0;

    for(i=0;i<tband;i++)
	{
		for(j=0;j<trow;j++)
		{
			for(k=0;k<tcol;k++)
			{
				numerator+=(TargetData[i][j][k]-tmean)*(MoveData[i][j][k]-Mmean);
				denominator1+=(float)pow((TargetData[i][j][k]-tmean),2);
				denominator2+=(float)pow((MoveData[i][j][k]-Mmean),2);
			}
		}
	}

	Ncc=(double)numerator/(sqrt(denominator1)*sqrt(denominator2));

	printf("Ncc=%f\n",Ncc);
	return -Ncc;

}

double objective2D(double p[])
{
	float tmean,Mmean;
	int tsum,Msum,tnum,Mnum;
	tsum=0;
	Msum=0;
	tnum=trow*tcol;
	Mnum=mrow*mcol;

	result2dimage(p);

	int j,k;

	for(j=0;j<trow;j++)
	{
		for(k=0;k<tcol;k++)
		{
				tsum+=Target[j][k];
				Msum+=Move[j][k];
		}
	}
	
	tmean=(float)tsum/tnum;
	Mmean=(float)Msum/Mnum;

	double Ncc;
	Ncc=0;
	float numerator;
	numerator=0;
	float denominator1,denominator2;
	denominator1=0;
	denominator2=0;

	for(j=0;j<trow;j++)
	{
		for(k=0;k<tcol;k++)
		{
				numerator+=(Target[j][k]-tmean)*(Move[j][k]-Mmean);
				denominator1+=(float)pow((Target[j][k]-tmean),2);
				denominator2+=(float)pow((Move[j][k]-Mmean),2);
		}
	}
	

	Ncc=(double)numerator/(sqrt(denominator1)*sqrt(denominator2));

	printf("Ncc=%f\n",Ncc);
	return -Ncc;

}