-
Notifications
You must be signed in to change notification settings - Fork 0
/
CircleFitByTaubin.m
96 lines (79 loc) · 2.56 KB
/
CircleFitByTaubin.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
function Par = CircleFitByTaubin(XY)
%--------------------------------------------------------------------------
%
% Circle fit by Taubin
% G. Taubin, "Estimation Of Planar Curves, Surfaces And Nonplanar
% Space Curves Defined By Implicit Equations, With
% Applications To Edge And Range Image Segmentation",
% IEEE Trans. PAMI, Vol. 13, pages 1115-1138, (1991)
%
% Input: XY(n,2) is the array of coordinates of n points x(i)=XY(i,1), y(i)=XY(i,2)
%
% Output: Par = [a b R] is the fitting circle:
% center (a,b) and radius R
%
% Note: this fit does not use built-in matrix functions (except "mean"),
% so it can be easily programmed in any programming language
%
%--------------------------------------------------------------------------
n = size(XY,1); % number of data points
centroid = mean(XY); % the centroid of the data set
% computing moments (note: all moments will be normed, i.e. divided by n)
Mxx = 0; Myy = 0; Mxy = 0; Mxz = 0; Myz = 0; Mzz = 0;
for i=1:n
Xi = XY(i,1) - centroid(1); % centering data
Yi = XY(i,2) - centroid(2); % centering data
Zi = Xi*Xi + Yi*Yi;
Mxy = Mxy + Xi*Yi;
Mxx = Mxx + Xi*Xi;
Myy = Myy + Yi*Yi;
Mxz = Mxz + Xi*Zi;
Myz = Myz + Yi*Zi;
Mzz = Mzz + Zi*Zi;
end
Mxx = Mxx/n;
Myy = Myy/n;
Mxy = Mxy/n;
Mxz = Mxz/n;
Myz = Myz/n;
Mzz = Mzz/n;
% computing the coefficients of the characteristic polynomial
Mz = Mxx + Myy;
Cov_xy = Mxx*Myy - Mxy*Mxy;
A3 = 4*Mz;
A2 = -3*Mz*Mz - Mzz;
A1 = Mzz*Mz + 4*Cov_xy*Mz - Mxz*Mxz - Myz*Myz - Mz*Mz*Mz;
A0 = Mxz*Mxz*Myy + Myz*Myz*Mxx - Mzz*Cov_xy - 2*Mxz*Myz*Mxy + Mz*Mz*Cov_xy;
A22 = A2 + A2;
A33 = A3 + A3 + A3;
xnew = 0;
ynew = 1e+20;
epsilon = 1e-12;
IterMax = 20;
% Newton's method starting at x=0
for iter=1:IterMax
yold = ynew;
ynew = A0 + xnew*(A1 + xnew*(A2 + xnew*A3));
if abs(ynew) > abs(yold)
disp('Newton-Taubin goes wrong direction: |ynew| > |yold|');
xnew = 0;
break;
end
Dy = A1 + xnew*(A22 + xnew*A33);
xold = xnew;
xnew = xold - ynew/Dy;
if (abs((xnew-xold)/xnew) < epsilon), break, end
if (iter >= IterMax)
disp('Newton-Taubin will not converge');
xnew = 0;
end
if (xnew<0.)
fprintf(1,'Newton-Taubin negative root: x=%f\n',xnew);
xnew = 0;
end
end
% computing the circle parameters
DET = xnew*xnew - xnew*Mz + Cov_xy;
Center = [Mxz*(Myy-xnew)-Myz*Mxy , Myz*(Mxx-xnew)-Mxz*Mxy]/DET/2;
Par = [Center+centroid , sqrt(Center*Center'+Mz)];
end % CircleFitByTaubin