Code covered by the BSD License

fitellipse.m

Richard Brown (view profile)

28 May 2007 (Updated )

Fit ellipses to 2D points using linear or nonlinear least squares

fitellipse(x, varargin)
```function [z, a, b, alpha] = fitellipse(x, varargin)
%FITELLIPSE   least squares fit of ellipse to 2D data
%
%   [Z, A, B, ALPHA] = FITELLIPSE(X)
%       Fit an ellipse to the 2D points in the 2xN array X. The ellipse is
%       returned in parametric form such that the equation of the ellipse
%       parameterised by 0 <= theta < 2*pi is:
%           X = Z + Q(ALPHA) * [A * cos(theta); B * sin(theta)]
%       where Q(ALPHA) is the rotation matrix
%           Q(ALPHA) = [cos(ALPHA), -sin(ALPHA);
%                       sin(ALPHA), cos(ALPHA)]
%
%       Fitting is performed by nonlinear least squares, optimising the
%       squared sum of orthogonal distances from the points to the fitted
%       ellipse. The initial guess is calculated by a linear least squares
%       routine, by default using the Bookstein constraint (see below)
%
%   [...]            = FITELLIPSE(X, 'linear')
%       Fit an ellipse using linear least squares. The conic to be fitted
%       is of the form
%           x'Ax + b'x + c = 0
%       and the algebraic error is minimised by least squares with the
%       Bookstein constraint (lambda_1^2 + lambda_2^2 = 1, where
%       lambda_i are the eigenvalues of A)
%
%   [...]            = FITELLIPSE(..., 'Property', 'value', ...)
%       Specify property/value pairs to change problem parameters
%          Property                  Values
%          =================================
%          'constraint'              {|'bookstein'|, 'trace'}
%                                    For the linear fit, the following
%                                    x'Ax + b'x + c = 0. Different
%                                    constraints on the parameters yield
%                                    different fits. Both 'bookstein' and
%                                    'trace' are Euclidean-invariant
%                                    constraints on the eigenvalues of A,
%                                    meaning the fit will be invariant
%                                    under Euclidean transformations
%                                    'bookstein': lambda1^2 + lambda2^2 = 1
%                                    'trace'    : lambda1 + lambda2     = 1
%
%           Nonlinear Fit Property   Values
%           ===============================
%           'maxits'                 positive integer, default 200
%                                    Maximum number of iterations for the
%                                    Gauss Newton step
%
%           'tol'                    positive real, default 1e-5
%                                    Relative step size tolerance
%   Example:
%       % A set of points
%       x = [1 2 5 7 9 6 3 8;
%            7 6 8 7 5 7 2 4];
%
%       % Fit an ellipse using the Bookstein constraint
%       [zb, ab, bb, alphab] = fitellipse(x, 'linear');
%
%       % Find the least squares geometric estimate
%       [zg, ag, bg, alphag] = fitellipse(x);
%
%       % Plot the results
%       plot(x(1,:), x(2,:), 'ro')
%       hold on
%       % plotellipse(zb, ab, bb, alphab, 'b--')
%       % plotellipse(zg, ag, bg, alphag, 'k')
%

% Copyright Richard Brown, this code can be freely used and modified so
% long as this line is retained
error(nargchk(1, 5, nargin, 'struct'))

% Default parameters
params.fNonlinear = true;
params.constraint = 'bookstein';
params.maxits     = 200;
params.tol        = 1e-5;

% Parse inputs
[x, params] = parseinputs(x, params, varargin{:});

% Constraints are Euclidean-invariant, so improve conditioning by removing
% centroid
centroid = mean(x, 2);
x        = x - repmat(centroid, 1, size(x, 2));

% Obtain a linear estimate
switch params.constraint
% Bookstein constraint : lambda_1^2 + lambda_2^2 = 1
case 'bookstein'
[z, a, b, alpha] = fitbookstein(x);

% 'trace' constraint, lambda1 + lambda2 = trace(A) = 1
case 'trace'
[z, a, b, alpha] = fitggk(x);
end % switch

% Minimise geometric error using nonlinear least squares if required
if params.fNonlinear
% Initial conditions
z0     = z;
a0     = a;
b0     = b;
alpha0 = alpha;

% Apply the fit
[z, a, b, alpha, fConverged] = ...
fitnonlinear(x, z0, a0, b0, alpha0, params);

% Return linear estimate if GN doesn't converge
if ~fConverged
warning('fitellipse:FailureToConverge', ...'
'Gauss-Newton did not converge, returning linear estimate');
z = z0;
a = a0;
b = b0;
alpha = alpha0;
end
end

% Add the centroid back on
z = z + centroid;

end % fitellipse

% ----END MAIN FUNCTION-----------%

function [z, a, b, alpha] = fitbookstein(x)
%FITBOOKSTEIN   Linear ellipse fit using bookstein constraint
%   lambda_1^2 + lambda_2^2 = 1, where lambda_i are the eigenvalues of A

% Convenience variables
m  = size(x, 2);
x1 = x(1, :)';
x2 = x(2, :)';

% Define the coefficient matrix B, such that we solve the system
% B *[v; w] = 0, with the constraint norm(w) == 1
B = [x1, x2, ones(m, 1), x1.^2, sqrt(2) * x1 .* x2, x2.^2];

% To enforce the constraint, we need to take the QR decomposition
[Q, R] = qr(B);

% Decompose R into blocks
R11 = R(1:3, 1:3);
R12 = R(1:3, 4:6);
R22 = R(4:6, 4:6);

% Solve R22 * w = 0 subject to norm(w) == 1
[U, S, V] = svd(R22);
w = V(:, 3);

% Solve for the remaining variables
v = -R11 \ R12 * w;

% Fill in the quadratic form
A        = zeros(2);
A(1)     = w(1);
A([2 3]) = 1 / sqrt(2) * w(2);
A(4)     = w(3);
bv       = v(1:2);
c        = v(3);

% Find the parameters
[z, a, b, alpha] = conic2parametric(A, bv, c);

end % fitellipse

function [z, a, b, alpha] = fitggk(x)
% Linear least squares with the Euclidean-invariant constraint Trace(A) = 1

% Convenience variables
m  = size(x, 2);
x1 = x(1, :)';
x2 = x(2, :)';

% Coefficient matrix
B = [2 * x1 .* x2, x2.^2 - x1.^2, x1, x2, ones(m, 1)];

v = B \ -x1.^2;

% For clarity, fill in the quadratic form variables
A        = zeros(2);
A(1,1)   = 1 - v(2);
A([2 3]) = v(1);
A(2,2)   = v(2);
bv       = v(3:4);
c        = v(5);

% find parameters
[z, a, b, alpha] = conic2parametric(A, bv, c);

end

function [z, a, b, alpha, fConverged] = fitnonlinear(x, z0, a0, b0, alpha0, params)
% Gauss-Newton least squares ellipse fit minimising geometric distance

% Get initial rotation matrix
Q0 = [cos(alpha0), -sin(alpha0); sin(alpha0) cos(alpha0)];
m = size(x, 2);

% Get initial phase estimates
phi0 = angle( [1 i] * Q0' * (x - repmat(z0, 1, m)) )';
u = [phi0; alpha0; a0; b0; z0];

% Iterate using Gauss Newton
fConverged = false;
for nIts = 1:params.maxits
% Find the function and Jacobian
[f, J] = sys(u);

% Solve for the step and update u
h = -J \ f;
u = u + h;

% Check for convergence
delta = norm(h, inf) / norm(u, inf);
if delta < params.tol
fConverged = true;
break
end
end

alpha = u(end-4);
a     = u(end-3);
b     = u(end-2);
z     = u(end-1:end);

function [f, J] = sys(u)
% SYS : Define the system of nonlinear equations and Jacobian. Nested
% function accesses X (but changeth it not)
% from the FITELLIPSE workspace

% Tolerance for whether it is a circle
circTol = 1e-5;

% Unpack parameters from u
phi   = u(1:end-5);
alpha = u(end-4);
a     = u(end-3);
b     = u(end-2);
z     = u(end-1:end);

% If it is a circle, the Jacobian will be singular, and the
% Gauss-Newton step won't work.
%TODO: This can be fixed by switching to a Levenberg-Marquardt
%solver
if abs(a - b) / (a + b) < circTol
warning('fitellipse:CircleFound', ...
'Ellipse is near-circular - nonlinear fit may not succeed')
end

% Convenience trig variables
c = cos(phi);
s = sin(phi);
ca = cos(alpha);
sa = sin(alpha);

% Rotation matrices
Q    = [ca, -sa; sa, ca];
Qdot = [-sa, -ca; ca, -sa];

% Preallocate function and Jacobian variables
f = zeros(2 * m, 1);
J = zeros(2 * m, m + 5);
for i = 1:m
rows = (2*i-1):(2*i);
% Equation system - vector difference between point on ellipse
% and data point
f((2*i-1):(2*i)) = x(:, i) - z - Q * [a * cos(phi(i)); b * sin(phi(i))];

% Jacobian
J(rows, i) = -Q * [-a * s(i); b * c(i)];
J(rows, (end-4:end)) = ...
[-Qdot*[a*c(i); b*s(i)], -Q*[c(i); 0], -Q*[0; s(i)], [-1 0; 0 -1]];
end
end
end % fitnonlinear

function [z, a, b, alpha] = conic2parametric(A, bv, c)
% Diagonalise A - find Q, D such at A = Q' * D * Q
[Q, D] = eig(A);
Q = Q';

% If the determinant < 0, it's not an ellipse
if prod(diag(D)) <= 0
error('fitellipse:NotEllipse', 'Linear fit did not produce an ellipse');
end

% We have b_h' = 2 * t' * A + b'
t = -0.5 * (A \ bv);

c_h = t' * A * t + bv' * t + c;

z = t;
a = sqrt(-c_h / D(1,1));
b = sqrt(-c_h / D(2,2));
alpha = atan2(Q(1,2), Q(1,1));
end % conic2parametric

function [x, params] = parseinputs(x, params, varargin)
% PARSEINPUTS put x in the correct form, and parse user parameters

% CHECK x
% Make sure x is 2xN where N > 3
if size(x, 2) == 2
x = x';
end
if size(x, 1) ~= 2
error('fitellipse:InvalidDimension', ...
'Input matrix must be two dimensional')
end
if size(x, 2) < 6
error('fitellipse:InsufficientPoints', ...
'At least 6 points required to compute fit')
end

% Determine whether we are solving for geometric (nonlinear) or algebraic
% (linear) distance
if ~isempty(varargin) && strncmpi(varargin{1}, 'linear', length(varargin{1}))
params.fNonlinear = false;
varargin(1)       = [];
else
params.fNonlinear = true;
end

% Parse property/value pairs
if rem(length(varargin), 2) ~= 0
error('fitellipse:InvalidInputArguments', ...
'Additional arguments must take the form of Property/Value pairs')
end

% Cell array of valid property names
properties = {'constraint', 'maxits', 'tol'};

while length(varargin) ~= 0
% Pop pair off varargin
property      = varargin{1};
value         = varargin{2};
varargin(1:2) = [];

% If the property has been supplied in a shortened form, lengthen it
iProperty = find(strncmpi(property, properties, length(property)));
if isempty(iProperty)
error('fitellipse:UnknownProperty', 'Unknown Property');
elseif length(iProperty) > 1
error('fitellipse:AmbiguousProperty', ...
'Supplied shortened property name is ambiguous');
end

% Expand property to its full name
property = properties{iProperty};

% Check for irrelevant property
if ~params.fNonlinear && ismember(property, {'maxits', 'tol'})
warning('fitellipse:IrrelevantProperty', ...
'Supplied property has no effect on linear estimate, ignoring');
continue
end

% Check supplied property value
switch property
case 'maxits'
if ~isnumeric(value) || value <= 0
error('fitcircle:InvalidMaxits', ...
'maxits must be an integer greater than 0')
end
params.maxits = value;
case 'tol'
if ~isnumeric(value) || value <= 0
error('fitcircle:InvalidTol', ...
'tol must be a positive real number')
end
params.tol = value;
case 'constraint'
switch lower(value)
case 'bookstein'
params.constraint = 'bookstein';
case 'trace'
params.constraint = 'trace';
otherwise
error('fitellipse:InvalidConstraint', ...
'Invalid constraint specified')
end
end % switch property
end % while

end % parseinputs```