-
Notifications
You must be signed in to change notification settings - Fork 2
/
LM.m
70 lines (54 loc) · 2.8 KB
/
LM.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
% from: https://people.cas.uab.edu/~mosya/cl/MATLABcircle.html
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