function Par = LM(XY,ParIni,LambdaIni) %-------------------------------------------------------------------------- % % Geometric circle fit (minimizing orthogonal distances) % based on the standard Levenberg-Marquardt scheme % in the full (a,b,R) parameter space % This is perhaps the best geometric circle fit % % Input: XY(n,2) is the array of coordinates of n points x(i)=XY(i,1), y(i)=XY(i,2) % ParIni = [a b R] is the initial guess (supplied by user) % LambdaIni is initial value for the correction factor lambda % (this is optional; if it is missing, LM sets it to 1) % % Output: Par = [a b R] is the fitting circle: % center (a,b) and radius R % %-------------------------------------------------------------------------- if (nargin < 3), LambdaIni = 1; end; % if Lambda(initial) is not supplied, set it to one epsilon=0.000001; % tolerance (small threshold) IterMAX = 50; % maximal number of (main) iterations; usually 10-20 suffice lambda_sqrt = sqrt(LambdaIni); % sqrt(Lambda) is actually used by the code Par = ParIni; % starting with the given initial guess [J,g,F] = CurrentIteration(Par,XY); % compute objective function and its derivatives for iter=1:IterMAX % main loop, each run is one (main) iteration while (1) % secondary loop - adjusting Lambda (no limit on cycles) DelPar = [J; lambda_sqrt*eye(3)]\[g; zeros(3,1)]; % step candidate progress = norm(DelPar)/(norm(Par)+epsilon); if (progress < epsilon) break; end; % stopping rule ParTemp = Par - DelPar'; [JTemp,gTemp,FTemp] = CurrentIteration(ParTemp,XY); % objective function + derivatives if (FTemp < F && ParTemp(3)>0) % yes, improvement lambda_sqrt = lambda_sqrt/2; % reduce lambda, move to next iteration break; else % no improvement lambda_sqrt = lambda_sqrt*2; % increase lambda, recompute the step continue; end end % while (1), the end of the secondary loop % fprintf(1,' %d %.8f %.8f %.8f\n',iter,Par); if (progress < epsilon) break; end; % stopping rule Par = ParTemp; J = JTemp; g = gTemp; F = FTemp; % update the iteration end % the end of the main loop (over iterations) end % LM %================ function CurrentIteration ================ function [J,g,F] = CurrentIteration(Par,XY) % computes the objective function F and its derivatives at the current point Par Dx = XY(:,1) - Par(1); Dy = XY(:,2) - Par(2); D = sqrt(Dx.*Dx + Dy.*Dy); J = [-Dx./D, -Dy./D, -ones(size(XY,1),1)]; g = D - Par(3); F = norm(g)^2; end % CurrentIteration