c++ 最小二乘算法拟合球心 使用Vs2008

亲测可用 算法可以较为准确的拟合球三维点云数据的球心和半径

#include    
#include    
#include        
using namespace std;      
#define   ABS（x）   （x）>0?（x）:-（x）      
#define   SWAP（ab）   {temp=（a）;（a）=（b）;（b）=temp;}      

void solve（double **adouble *bdouble *xint n）      
{      
	int ijkik;      
	double miktemp;      
	for（k=0;k	{ 
		mik=-1;      
		for（i=k;i			if（ABS（a[i][k]）>mik）      
			{      
			  mik=ABS（a[i][k]）;      
			  ik=i;   
			}
			for（j=k;j				SWAP（a[ik][j]a[k][j]）;      
			SWAP（b[k]b[ik]）;      
			b[k]/=a[k][k];      
			for（i=n-1;i>=k;i--）      
				a[k][i]/=a[k][k];      
			for（i=k+1;i			{  
				b[i]-=a[i][k]*b[k];  
				for（j=n-1;j>=k;j--）  
					a[i][j]-=a[i][k]*a[k][j];   
			}
	}      
	for（i=n-1;i>=0;i--）      
	{      
		x[i]=b[i];      
		for（j=i+1;j			x[i]-=a[i][j]*x[j];      
	}      
}      

void linear（double **xdouble *ydouble *betaint nint p）// p=4 
{      
	double **a*b;
	int ijk;
	a=new double*[p];
	for（i=0;i		a[i]=new double[p];
	for（i=0;i		for（j=0;j		{
			a[i][j]=0;
			for（k=0;k				a[i][j]+=x[k][i]*x[k][j];      
		}      
		b=new double[p];      
		for（i=0;i		{      
			b[i]=0;      
			for（j=0;j				b[i]+=x[j][i]*y[j];      
		}      
		solve（abbetap）;      
		for（i=0;i			delete   a[i];      
		delete   ab;      
}      

void make（int xint yint zint rint n）   
{   
	float arg=0.5;   
	float err=2;   
	float abefg;   
	ofstream file（“log.txt“）;   
	file<	while（n--）   
	{   
		a=（3.141/arg）*float（rand（））/RAND_MAX;   
		b=（3.141/arg）*float（rand（））/RAND_MAX;   
		e=x+r*sin（a）*cos（b）*（1+（（float（rand（））-（RAND_MAX>>1））/RAND_MAX）/err）;   
		f=y+r*sin（a）*sin（b）*（1+（（float（rand（））-（RAND_MAX>>1））/RAND_MAX）/err）;   
		g=z+r*cos（a）*（1+（（float（rand（））-（RAND_MAX>>1））/RAND_MAX）/err）;       
		file<	}   
}   

void main（）      
{      
	//make（00010010000）;   
	ifstream file（“Ball_num_3.txt“）;

	while（!file.eof（））
	{
	int inp=4;
	double x0y0z0r;
	double **x*ybeta[4];   
	file>>n;
	x=new double*[n];
	y=new double[n]; 
	for（i=0;i	{ 
		x[i]=new double[4];
		file>>x[i][0]>>x[i][1]>>x[i][2];   
		x[i][3]=1;      
		y[i]=-x[i][0]*x[i][0]-x[i][1]*x[i][1]-x[i][2]*x[i][2];      
	}
	linear（xybetanp）;// x是数据 y是半径 beta存圆心和半径数据 p=4.
	x0=-beta[0]/2;      
	y0=-beta[1]/2;      
	z0=-beta[2]/2;
	r=sqrt（x0*x0+y0*y0+z0*z0-beta[3]）;
	printf（“%f\t%f\t%f\t%f\t\n“x0y0z0r）;
	delete[]y;
	while（n--）delete[]x[n];
	delete[]x;}
	file.close（）;
}  

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2016-07-30 21:29  ball_center_1\
     目录           0  2016-08-01 09:10  ball_center_1\ball_center\
     目录           0  2016-08-01 09:10  ball_center_1\ball_center\ball_center\
     文件     1707008  2016-08-01 09:04  ball_center_1\ball_center\ball_center.ncb
     文件         899  2016-07-30 21:29  ball_center_1\ball_center\ball_center.sln
     文件       10240  2016-07-31 23:44  ball_center_1\ball_center\ball_center.suo
     文件        3666  2016-07-30 21:44  ball_center_1\ball_center\ball_center\ball_center.vcproj
     文件        1411  2016-07-31 23:44  ball_center_1\ball_center\ball_center\ball_center.vcproj.minm-PC.minm.user
     文件       11462  2016-07-31 14:33  ball_center_1\ball_center\ball_center\Ball_num_1.txt
     文件       26462  2016-07-31 14:34  ball_center_1\ball_center\ball_center\Ball_num_2.txt
     文件       26723  2016-07-31 14:34  ball_center_1\ball_center\ball_center\Ball_num_3.txt
     文件       16173  2016-07-31 14:34  ball_center_1\ball_center\ball_center\Ball_num_4.txt
     文件       15880  2016-07-31 14:34  ball_center_1\ball_center\ball_center\Ball_num_5.txt
     文件       15534  2016-07-31 10:37  ball_center_1\ball_center\ball_center\Ball_num_6.txt
     文件       14113  2016-07-31 10:38  ball_center_1\ball_center\ball_center\Ball_num_7.txt
     文件       16499  2016-07-31 10:39  ball_center_1\ball_center\ball_center\Ball_num_8.txt
     文件       14073  2016-07-31 10:40  ball_center_1\ball_center\ball_center\Ball_num_9.txt
     目录           0  2016-08-01 09:10  ball_center_1\ball_center\ball_center\Debug\
     文件         621  2016-07-30 22:03  ball_center_1\ball_center\ball_center\Debug\ball_center.exe.intermediate.manifest
     文件        8166  2016-07-30 22:03  ball_center_1\ball_center\ball_center\Debug\BuildLog.htm
     文件       55026  2016-07-30 22:03  ball_center_1\ball_center\ball_center\Debug\main.obj
     文件          67  2016-07-30 22:03  ball_center_1\ball_center\ball_center\Debug\mt.dep
     文件      183296  2016-07-30 22:03  ball_center_1\ball_center\ball_center\Debug\vc90.idb
     文件      225280  2016-07-30 22:03  ball_center_1\ball_center\ball_center\Debug\vc90.pdb
     文件        2735  2016-07-31 21:13  ball_center_1\ball_center\ball_center\main.cpp
     目录           0  2016-08-01 09:10  ball_center_1\ball_center\Debug\
     文件       43520  2016-07-30 22:03  ball_center_1\ball_center\Debug\ball_center.exe
     文件      419896  2016-07-30 22:03  ball_center_1\ball_center\Debug\ball_center.ilk
     文件      551936  2016-07-30 22:03  ball_center_1\ball_center\Debug\ball_center.pdb