------------------圆拟合-----------------
在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