gauss.h

高斯滤波算法源程序

#include<iostream.h>
#include<fstream.h>
#include<math.h>
class pinghuaCD
{
public:

class c10point
{
public:
	double x,y,z;
	double c0,c1,c2,c3,c4,c5,c6,c7,c8,c9;
};

class NearKpoint
{
public:
	double x,y,z,s;
};

class point
   {
   public:
	   double x,y,z;
   };
point *array; 
c10point *arrayc10;
int arraynumber;
//////////////////////////////////////////////////////////////////////////////
////////////////类函数
//////////////////////////////////////////////////////////////////////////////
int countnumber (const char *name);
void creatasc(const char *name,int arraysize,point *ptr);
void loaddata(const char *name,int arraysize);
point* NearK(point *ptr,point *neark,point *kpoint,int *r,int *n,int P,int arraysize,double a,double b,double c);
point* newNearK(point *ptr,point *neark,point *kpoint,int *r,int *n,int *nn,int P,int arraysize,double a,double b,double c,double cs);
void PX (int arraysize,point *ptr);

};
///////////////////////////////////////////////////////
///LoadData-------需要改进接口
///////////////////////////////////////////////////////
void pinghuaCD::loaddata(const char *name,int arraysize)
{
   double n;
   ifstream file(name);
   array=new point[arraysize]; 
   //开辟空间,读数   
   for(int i=0;i<arraysize;i++)
   {
       file>>n;
       array[i].x=n;
	   file>>n;
       array[i].y=n;
	   file>>n;
       array[i].z=n;
   }
   file.close();
}
///////////////////////////////////////////////////////
////number counting
//////////////////////////////////////////////////////
int  pinghuaCD::countnumber (const char *name)
{
   double a[3][3];
   double *p=&(a[0][0]);
   double n;
   static int Count;
   ifstream file(name);
   while(1)
   {
       for(int i=0;i<9;i++)
	   {	
		  file>>n;
          *(p+i)=n;
	   }
       Count++;
       Count++;
       Count++;
       if((a[0][0]==a[1][0])&&(a[0][1]==a[1][1])&&(a[0][2]==a[1][2]))
	   {
          Count--;
          Count--;
          Count--;
          break;
	   }
       if((a[2][0]==a[1][0])&&(a[2][1]==a[1][1])&&(a[2][2]==a[1][2]))
	   {
          Count--;
          Count--;
          break;
	   }
       if((a[2][0]==a[2][1])&&(a[2][1]==a[2][2])&&(a[2][2]==a[1][2]))
	   {
	      file>>n;
	      a[0][0]=n;
	      file>>n;
	      a[0][1]=n;
	      file>>n;
		  a[0][2]=n;
	      if((a[0][0]==a[2][0])&&(a[0][1]==a[2][1])&&(a[0][2]==a[2][2]))
		  {
		       Count--;
               break;
		  }
          else
		  {
			  Count++;
		  }
	   }
   }
  int count=Count;
  return count;
  file.close();
}
///////////////////////////////////////////////////////
////Creat txt
///////////////////////////////////////////////////////
void  pinghuaCD::creatasc(const char *name,int arraysize,point *ptr)
{  
   ofstream file(name);
   for(int k=0;k<arraysize;k++)
   {
       file<<ptr[k].x<<" ";
	   file<<ptr[k].y<<" ";
	   file<<ptr[k].z<<"\n";
   }
   file<<"**end cloud**"<<"\n";
   file.close();
}
/////////////////////////////////////////////////////////////////////////////////////////
/////先排序-希尔排序
//6,3,1--------60s
//10,5,1-------30s
//100,50,1-----12s------OK
//1000,500,1---14s
/////////////////////////////////////////////////////////////////////////////////////////
void  pinghuaCD::PX (int arraysize,point *ptr)
{
    int k=arraysize;	
    int px4,px3,px2,px1,px;
    double temp,temp1,temp2;
    int span[3];
    span[0]=100;
    span[1]=50;
    span[2]=1;
    for(px=0;px<3;px++)
	 {
	     px1=span[px];
	     for(px2=0;px2<px1;px2++)
		 {
             for(px3=px2;px3<k-px1;px3=px3+px1)
			 {
             	temp=ptr[px3+px1].x;
				temp1=ptr[px3+px1].y;
	            temp2=ptr[px3+px1].z;
                px4=px3;
	            while((px4>-1)&&(temp<ptr[px4].x))
				{
	             	ptr[px4+px1].x=ptr[px4].x;
					ptr[px4+px1].y=ptr[px4].y;
					ptr[px4+px1].z=ptr[px4].z;
	                px4=px4-px1;
				}
                ptr[px4+px1].x=temp;
                ptr[px4+px1].y=temp1;
                ptr[px4+px1].z=temp2;
			 }
		 }
	 }

}
///////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////求一点的邻域
//////////////////////////////////////////////////////////////////////////////////////////////////////////
pinghuaCD::point* pinghuaCD::NearK(point *ptr,point *neark,point *kpoint,int *r,int *n,int P,int arraysize,double a,double b,double c)
{
    int i;
	int countc;
        a=a/2;
        b=b/2;
        c=c/2;
	NearKpoint temp;
	//point *ptr1;
	point *ptr2;
	point *ptr3;
	NearKpoint *ptr4;
	static int C;
	C=0;
	//ptr1=new point[arraysize/1];
	/////////P点的立方域
	//////x方向选定--->
	for(i=0;(ptr[P-i].x)>=(ptr[P].x-a);i++)
	{
		if(P==0)
		{
           kpoint[C].x=ptr[P-i].x;
           kpoint[C].y=ptr[P-i].y;
           kpoint[C].z=ptr[P-i].z;
		   C++;
		}
		if((P-i)==0)
        break;
        kpoint[C].x=ptr[P-i].x;
        kpoint[C].y=ptr[P-i].y;
        kpoint[C].z=ptr[P-i].z;
		C++;
	}
    for(i=1;(ptr[P+i].x)<=(ptr[P].x+a);i++)
	{
		if((P+i)>(arraysize-1))
        break;
        kpoint[C].x=ptr[P+i].x;
        kpoint[C].y=ptr[P+i].y;
        kpoint[C].z=ptr[P+i].z;
		C++;
	}
	
	//////y方向选定--->
	countc=C;
    C=0;
	ptr2=new point[countc];
    for(i=0;i<countc;i++)
	{
        if((kpoint[i].y<=(ptr[P].y+b))&&(kpoint[i].y>=(ptr[P].y-b)))
		{
            ptr2[C].x=kpoint[i].x;
			ptr2[C].y=kpoint[i].y;
			ptr2[C].z=kpoint[i].z;
            C++;
		}
	}
	//////z方向选定--->
	countc=C;
    C=0;
	ptr3=new point[countc];
    for(i=0;i<countc;i++)
	{
        if((ptr2[i].z<=(ptr[P].z+c))&&(ptr2[i].z>=(ptr[P].z-c)))
		{
            ptr3[C].x=ptr2[i].x;
			ptr3[C].y=ptr2[i].y;
			ptr3[C].z=ptr2[i].z;
            C++;
		}
	}
	//////////////NearK邻域
    //////当立方域中的点数不足k时，判断值Y为0；---〉
	countc=C;
	(*n)=countc;
	C=0;
	   (*r)=0;
	   neark=new point[countc];
	   for(i=0;i<countc;i++)
	   {
		    neark[i].x=ptr3[i].x;
		    neark[i].y=ptr3[i].y;
		    neark[i].z=ptr3[i].z;
			//cout<<neark[i].x<<"  "<<neark[i].y<<"  "<<neark[i].z<<"R"<<endl;
	   }
   
//  delete ptr4;
	delete ptr3;
	delete ptr2;
//	delete ptr1;

	return neark;
}
//////////////////////////////////////////////////////////////
//////////////////////配置新的领域
//////////////////////////////////////////////////////////////
pinghuaCD::point* pinghuaCD::newNearK(point *ptr,point *neark,point *kpoint,int *r,int *n,int *nn,int P,int arraysize,double a,double b,double c,double cs)
{
	int i;
	int countc;
        a=a/2;
        b=b/2;
        c=c/2;
	NearKpoint temp;
	//point *ptr1;
	point *ptr2;
	point *ptr3;
	NearKpoint *ptr4;
	static int C;
	C=0;
	//ptr1=new point[arraysize/1];
	/////////P点的立方域
	//////x方向选定--->
	for(i=0;(ptr[P-i].x)>=(ptr[P].x-a);i++)
	{
		if(P==0)
		{
           kpoint[C].x=ptr[P-i].x;
           kpoint[C].y=ptr[P-i].y;
           kpoint[C].z=ptr[P-i].z;
		   C++;
		}
		if((P-i)==0)
        break;
        kpoint[C].x=ptr[P-i].x;
        kpoint[C].y=ptr[P-i].y;
        kpoint[C].z=ptr[P-i].z;
		C++;
	}
    for(i=1;(ptr[P+i].x)<=(ptr[P].x+a);i++)
	{
		if((P+i)>(arraysize-1))
        break;
        kpoint[C].x=ptr[P+i].x;
        kpoint[C].y=ptr[P+i].y;
        kpoint[C].z=ptr[P+i].z;
		C++;
	}
	
	//////y方向选定--->
	countc=C;
    C=0;
	ptr2=new point[countc];
    for(i=0;i<countc;i++)
	{
        if((kpoint[i].y<=(ptr[P].y+b))&&(kpoint[i].y>=(ptr[P].y-b)))
		{
            ptr2[C].x=kpoint[i].x;
			ptr2[C].y=kpoint[i].y;
			ptr2[C].z=kpoint[i].z;
            C++;
		}
	}
	//////z方向选定--->
	countc=C;
    C=0;
	ptr3=new point[countc];
    for(i=0;i<countc;i++)
	{
        if((ptr2[i].z<=(ptr[P].z+c))&&(ptr2[i].z>=(ptr[P].z-c)))
		{
            ptr3[C].x=ptr2[i].x;
			ptr3[C].y=ptr2[i].y;
			ptr3[C].z=ptr2[i].z;
            C++;
		}
	}
	//////////////NearK邻域
    //////当立方域中的点数不足k时，判断值Y为0；---〉
	int k=0;
	countc=C;
	(*n)=countc;
	C=0;
	if(countc<k)
	(*r)=0;
	ptr4=new NearKpoint [countc];
	if(countc>=k)
	{
		(*r)=1;
		for(i=0;i<countc;i++)
		{
            ptr4[C].x=ptr3[i].x;
            ptr4[C].y=ptr3[i].y;
            ptr4[C].z=ptr3[i].z;
			ptr4[C].s=sqrt((ptr3[i].x-ptr[P].x)*(ptr3[i].x-ptr[P].x)+(ptr3[i].y-ptr[P].y)*(ptr3

[i].y-ptr[P].y)+(ptr3[i].z-ptr[P].z)*(ptr3[i].z-

ptr[P].z));
	        C++;  	
		}
   ///////////按距离提取新领域----〉
     C=0;
	///////////输出邻域----〉
     for(i=0;i<(*n);i++)
		{
		  if(ptr4[i].s<=2*cs)
		  {
			C++;
		  }
		}
	 int newcount=C;
	 C=0;
	  neark=new point[newcount];
	  (*nn)=newcount;
	  for(i=0;i<(*n);i++)
		{
		  if(ptr4[i].s<=2*cs)
		  {
			neark[C].x=ptr4[i].x;
			neark[C].y=ptr4[i].y;
			neark[C].z=ptr4[i].z;
			C++;
		  }
		}
      
	}
    delete ptr4;
	delete ptr3;
	delete ptr2;
//	delete ptr1;

	return neark;
}

///////////////////////////////////////////////////////////////////////////////////
////////////////////gauss高斯平滑函数(迭代次数,高斯核,阈值控制百分比,速度控制参数)
///////////////////////////////////////////////////////////////////////////////////

void  gauss(double he,double l,double limit,double *av,double *max,int con,const char *filename,const char *outname)
{
	int k,kk;
	int i,j;
	int r,n,nn;
	double x;
	double w,w1,w2;
	double X,X1,X2;
	double Y,Y1,Y2;
	static int F1;
	int ii;
	double bili;
	(*max)=0;
	(*av)=0;
	n=nn=1;
	pinghuaCD C;
	pinghuaCD::c10point *C1;
	C.arraynumber=C.countnumber(filename);
	C.loaddata(filename,C.arraynumber);
	pinghuaCD::point  *kpoint;
    kpoint=new pinghuaCD::point[C.arraynumber];
	/////////进入主要程序
		C.PX(C.arraynumber,C.array);
	    cout<<"OK"<<endl;
		C1=new pinghuaCD::c10point[C.arraynumber];
		F1=0;
            for(i=0;i<C.arraynumber;i++)
			{
			    C1[F1].x=C.array[i].x;
			    C1[F1].y=C.array[i].y;
			    C1[F1].z=C.array[i].z;
			    w1=w2=0;
				X1=X2=0;
				Y1=Y2=0;
			    pinghuaCD::point  *neark;
			    //neark=C.newNearK(C.array,neark,kpoint,&r,&n,&nn,i,C.arraynumber,4*he,4*he,4*he,he);
			    neark=C.NearK(C.array,neark,kpoint,&r,&n,i,C.arraynumber,l,l,l);
			    if((nn<=1)&&(n<=1))
				{
				    C1[F1].c0=C.array[i].z;
				    //C1[F1].c1=0;
					C1[F1].c2=C.array[i].x;
				    //C1[F1].c3=0;
					C1[F1].c4=C.array[i].y;
				    //C1[F1].c5=0;
					C1[F1].c6=0;
					cout<<"worry"<<endl;
				}
			    if((nn>1)||(n>1))
				{
				//	cout<<"OK"<<endl;
			    	for(j=0;j<n;j++)
					{
					    x=sqrt((neark[j].x-C.array[i].x)*(neark[j].x-C.array[i].x)+(neark[j].y-C.array[i].y)*(neark[j].y-C.array[i].y)+(neark[j].z-C.array[i].z)*(neark[j].z-C.array[i].z));
						w2=w2+exp((-1*x*x)/(2*he*he));
					    
						w1=w1+neark[j].z*exp((-1*x*x)/(2*he*he));
						
						   X1=X1+neark[j].x*exp((-1*x*x)/(2*he*he));
						   X2=w2;
					
						   Y1=Y1+neark[j].y*exp((-1*x*x)/(2*he*he));
						   Y2=w2;
					
                        
					}
				
				        if(w2<=0)
					    w=C.array[i].z;
				        if(w2!=0)
				        w=w1/w2;
					    C1[F1].c0=w;
					
						if(X2<=0)
					    X=C.array[i].x;
				        if(X2!=0)
				        X=X1/X2;
						C1[F1].c2=X;
				        
						if(Y2<=0)
					    Y=C.array[i].y;
				        if(Y2!=0)
				        Y=Y1/Y2;
						C1[F1].c4=Y;
				        
				///////////////////////计算偏差
				    C1[F1].c6=sqrt((C1[F1].c0-C1[F1].z)*(C1[F1].c0-C1[F1].z)+(C1[F1].c2-C1[F1].x)*(C1[F1].c2-C1[F1].x)+(C1[F1].c4-C1[F1].y)*(C1[F1].c4-C1[F1].y));
				//////////////////////偏差控制
					if(C1[F1].c6>limit)
					{
						bili=limit/C1[F1].c6;
						C1[F1].c0=(C1[F1].c0-C1[F1].z)*bili+C1[F1].z;
						C1[F1].c2=(C1[F1].c2-C1[F1].x)*bili+C1[F1].x;
						C1[F1].c4=(C1[F1].c4-C1[F1].y)*bili+C1[F1].y;
						C1[F1].c6=limit;
					}
						
				}
			    delete neark;
			    F1++;
			    cout<<i<<"  "<<endl;
			}
		
	
		    ///////////////////////////////////////////////////////////////////////////////
			/////////////////输出
			///////////////////////////////////////////////////////////////////////////////
            for(i=0;i<C.arraynumber;i++)
			{
			
				C.array[i].z=C1[i].c0;				  
		
		        C.array[i].x=C1[i].c2;
			
		        C.array[i].y=C1[i].c4;
			}
			////////////////////////////////////////////////////////////////////////////
			////////////////////计算最大偏差和平均偏差（前90%）
			////////////////////////////////////////////////////////////////////////////
	        if(con==1)
			{
				/////////////////////偏差排序（希尔排序）
                  int k=C.arraynumber;	
                  int px4,px3,px2,px1,px;
                  double temp,temp1,temp2;
                  int span[3];
                  span[0]=100;
                  span[1]=50;
                  span[2]=1;
                  for(px=0;px<3;px++)
				  {
	                  px1=span[px];
	                  for(px2=0;px2<px1;px2++)
					  {
                      for(px3=px2;px3<k-px1;px3=px3+px1)
					  {
             	         temp=C1[px3+px1].c6;
				         //temp1=ptr[px3+px1].y;
	                     //temp2=ptr[px3+px1].z;
                         px4=px3;
	                  while((px4>-1)&&(temp<C1[px4].c6))
					  {
	             	     C1[px4+px1].c6=C1[px4].c6;
					     //ptr[px4+px1].y=ptr[px4].y;
					     //ptr[px4+px1].z=ptr[px4].z;
	                     px4=px4-px1;
					  }
                    C1[px4+px1].c6=temp;
                  //ptr[px4+px1].y=temp1;
                  //ptr[px4+px1].z=temp2;
					  }
					  }
				  }
			  ////////////////////////////////////计算平均偏差
				 kk=(C.arraynumber/10)*9;
			     for(i=0;i<kk;i++)
				 {
					 (*av)=(*av)+C1[i].c6;
				 }
				 (*av)=(*av)/kk;
			  ////////////////////////////////////返回最大值
				 (*max)=C1[C.arraynumber-1].c6;
			}
	
	delete C1;
    delete kpoint;
	C.creatasc(outname,C.arraynumber,C.array);
}