圆拟合与点云数据球拟合算法

------------------圆拟合-----------------

在http://www.cnblogs.com/dotLive/archive/2007/04/06/524633.html和http://blog.csdn.net/liyuanbhu/article/details/50889951中都对最小二乘法圆拟合的原理进行了介绍，下面直接用一下，方便对代码理解：

下面给出使用上述方法的圆拟合代码：code1(C)

struct Point{
double x;
double y;

bool FitCicle(const double* X, const double* Y, const int Len, Point* Center, double* Radius)
{

	if (Len < 3) return false;

	double* xx = (double*)malloc(sizeof(double)*Len);
	if (!xx) return false;
	memcpy(xx,X, sizeof(double)*Len);
	double* yy = (double*)malloc(sizeof(double)*Len);
	if (!yy){
		free(xx);
		return false;
	}
	memcpy(yy, Y, sizeof(double)*Len);
	double X1,X2, X3,Y1,Y2,Y3,X1Y1,X1Y2,X2Y1;
	X1=X2=X3=Y1=Y2=Y3=X1Y1=X1Y2=X2Y1=0;

	int i = 0;
	bool Equalx = false, Equaly = false;
	for (i = 0; i < Len; i++)
	{
		X1 = X1 + xx[i];
		Y1 = Y1 + yy[i];
		if (yy[i] != Y1 / (i + 1)){
			Equaly = true;
		}
		if (xx[i] != X1 / (i + 1)){
			Equalx = true;
		}
		X2 = X2 + xx[i] * xx[i];
		Y2 = Y2 + yy[i] * yy[i];
		X3 = X3 + xx[i] * xx[i] * xx[i];
		Y3 = Y3 + yy[i] * yy[i] * yy[i];
		X1Y1 = X1Y1 + xx[i] * yy[i];
		X1Y2 = X1Y2 + xx[i] * yy[i] * yy[i];
		X2Y1 = X2Y1 + xx[i] * xx[i] * yy[i];
	}
        if(!Equalx||!Equaly){
            free(xx);
            free(yy);
            return false;
        }
       double C, D, E, G, H, N;
	double a, b, c;
	N = iLen;
	C = N*X2 - X1*X1;
	D = N*X1Y1 - X1*Y1;
	E = N*X3 + N*X1Y2 - (X2 + Y2)*X1;
	G = N*Y2 - Y1*Y1;
	H = N*X2Y1 + N*Y3 - (X2 + Y2)*Y1;
	a = (H*D - E*G) / (C*G - D*D + 1e-10);
	b = (H*C - E*D) / (D*D - G*C + 1e-10);
	c = -(a*X1 + b*Y1 + X2 + Y2) / (N + 1e-10);

	Center->dX = a / (-2);
	Center->dY = b / (-2);
	double dTemp = a*a + b*b - 4 * c;
	if (dTemp < 0){
		free(xx);
		free(yy);
		return false;
	}
	*Radius = sqrt(dTemp) / 2;

	free(xx);
	free(yy);
	return true;
}

------------------球拟合-----------------

同样的，使用最小二乘法进行球拟合，（x-x0）^2+(y-y0)^2+(z-z0)^2=R^2,下面给出的matlab代码，代码中X.txt等三个文件是我自己生成的球数据，分别存放X,Y,Z坐标:

%%球拟合，求出球心位置，及球的直径
clc;clear all;close all;
x=dlmread('X.txt'); %球的x坐标
y=dlmread('Y.txt');  %球的y坐标
z=dlmread('Z.txt');  %球的z坐标
data=unique([x(:),y(:),z(:)],'rows');
f=@(p,data)(data(:,1)-p(1)).^2+(data(:,2)-p(2)).^2+(data(:,3)-p(3)).^2-p(4)^2;
p=nlinfit(data,zeros(size(data,1),1),f,[0 0 0 1]');%拟合的参数??
hold on
plot3(data(:,1),data(:,2),data(:,3),'c*')
[X,Y,Z]=meshgrid(linspace(-14,14));
V=(X-p(1)).^2+(Y-p(2)).^2+(Z-p(3)).^2-p(4)^2;
isosurface(X,Y,Z,V,0);
alpha .5;
camlight;axis equal;grid on;view(3);
title(sprintf('(x- %f)^2+(y- %f)^2+(z- %f)^2=%f^2',p(1),p(2),p (3),p(4)))

参考：

http://blog.csdn.net/liyuanbhu/article/details/50890587

http://buaagc.blog.163.com/blog/static/7278839420095115218810

http://blog.sina.com.cn/s/blog_703f59920100n4zm.html