function Par = TaubinNTN(XY) %-------------------------------------------------------------------------- % % Algebraic circle fit by Taubin, based on Newton's method % 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 is a version optimized for speed, not for stability % it does not use built-in matrix functions, % so it can be easily programmed in any 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 % TaubinNTN