Description of fitEllipse3d

0001 function [fittedEllipse3d, TFM3D] = fitEllipse3d(points, varargin)
0002 %FITELLIPSE3D Fit an ellipse to a set of points.
0003 %
0004 %   FITTEDELLIPSE3D = fitEllipse3d(POINTS) returns the 3D ellipse fitted to
0005 %   a set of 3D points.
0006 %
0007 %   Example
0008 %     % Create 2D ellipse
0009 %     n=randi([10,100]);
0010 %     a=randi([30,50]); b=randi([5,25]);
0011 %     [x, y] = ellipseToPolygon([0 0 a b 0 ], n);
0012 %     % 3D and add some noise
0013 %     points = [x, y, zeros(n,1)];
0014 %     points=points+(-1+2*rand(n,3));
0015 %     % Create a random transformation
0016 %     center=-100+200*rand(1,3);
0017 %     phi=randi([-180,180]); theta=randi([-180,180]); psi=randi([-180,180]);
0018 %     TFM=eulerAnglesToRotation3d(phi, theta, psi, 'ZYZ'); TFM(1:3,4)=center';
0019 %     points = transformPoint3d(points, TFM);
0020 %     % Fit ellipse
0021 %     [fE, fTFM] = fitEllipse3d(points, 'vis', true);
0022 %     % Plot reconstructed ellipse
0023 %     [fx, fy] = ellipseToPolygon([0 0 fE(4), fE(5) 0 ], n);
0024 %     fpoints = transformPoint3d([fx, fy, zeros(n,1)], fTFM);
0025 %     drawEllipse3d(fE,'k')
0026 %
0027 %   See also
0028 %     drawEllipse3d, ellipseToPolygon
0029 %
0030 %   Source
0031 %     Nested functions are part of the quadfit toolbox by Levente Hunyadi
0032 %     https://mathworks.com/matlabcentral/fileexchange/45356
0033 
0034 % ------
0035 % Author: oqilipo
0036 % E-mail: N/A
0037 % Created: 2017-08-11
0038 % Copyright 2017-2024
0039 
0040 parser = inputParser;
0041 addRequired(parser, 'points', @(x) validateattributes(x, {'numeric'},...
0042     {'ncols',3,'real','finite','nonnan'}));
0043 addOptional(parser,'visualization',false,@islogical);
0044 parse(parser,points,varargin{:});
0045 points=parser.Results.points;
0046 
0047 % Mean of all points
0048 meanPoint = mean(points,1);
0049 % Center points around origin
0050 centeredPoints = bsxfun(@minus, points, meanPoint);
0051 
0052 % Transformation to x-y plane
0053 [~,~,R]=svd(centeredPoints);
0054 tfmPoints = transformPoint3d(centeredPoints, R');
0055 
0056 % Fit ellipse
0057 parEllipse = ellipsefit_direct(tfmPoints(:,1),tfmPoints(:,2));
0058 % Convert to explicit form
0059 [X, Y, A, B, phi2D] = ellipse_im2ex(parEllipse);
0060 
0061 % Transform to fitted 2d ellipse
0062 TFM2D = createRotationOz(phi2D);
0063 TFM2D(1:3,4)=[X; Y; 0];
0064 
0065 % Transformation back to 3d space
0066 TFM3D = [inv(R'), meanPoint'; 0 0 0 1]*TFM2D;
0067 
0068 % Extract translation
0069 center = TFM3D(1:3,4)';
0070 
0071 % Extract rotation
0072 TFM3D_ROT=TFM3D(1:3,1:3);
0073 % Convert to euler angles
0074 [PHI, THETA, PSI] = rotation3dToEulerAngles(TFM3D_ROT,'ZYZ');
0075 
0076 % Test if psi is correct
0077 TFM3D_test = eulerAnglesToRotation3d(PHI, THETA, PSI,'ZYZ');
0078 if ~all(all(ismembertol(TFM3D_test(1:3,1:3), TFM3D_ROT)))
0079     PSI=-1*PSI;
0080 end
0081 
0082 % matGeom format
0083 fittedEllipse3d = [center A B THETA PHI PSI];
0084 
0085 %% Visualization
0086 if parser.Results.visualization
0087     
0088     figure('Color','w'); axis equal tight; hold on; view(3)
0089     xlabel('x'); ylabel('y'); zlabel('z');
0090     
0091     % Input points
0092     scatter3(points(:,1),points(:,2),points(:,3), 'r', 'filled')
0093     
0094     % Centered points
0095     scatter3(centeredPoints(:,1),centeredPoints(:,2),centeredPoints(:,3), 'g', 'filled')
0096     
0097     % SVD points
0098     scatter3(tfmPoints(:,1),tfmPoints(:,2),tfmPoints(:,3), 'b', 'filled')
0099     % planeProps.FaceAlpha=0.25;
0100     % planeProps.FaceColor='b';
0101     % drawPlane3d([0 0 0 1 0 0 0 1 0],planeProps)
0102     
0103     % Fitted ellipse
0104     drawEllipse3d(fittedEllipse3d)
0105 end
0106 
0107 %% Nested functions
0108     function p = ellipsefit_direct(x,y)
0109         % Direct least squares fitting of ellipses.
0110         %
0111         % Input arguments:
0112         % x,y;
0113         %    x and y coodinates of 2D points
0114         %
0115         % Output arguments:
0116         % p:
0117         %    a 6-parameter vector of the algebraic ellipse fit with
0118         %    p(1)*x^2 + p(2)*x*y + p(3)*y^2 + p(4)*x + p(5)*y + p(6) = 0
0119         %
0120         % References:
0121         % Andrew W. Fitzgibbon, Maurizio Pilu and Robert B. Fisher, "Direct Least
0122         %    Squares Fitting of Ellipses", IEEE Trans. PAMI 21, 1999, pp476-480.
0123         
0124         % Copyright 2011 Levente Hunyadi
0125         
0126         narginchk(2,2);
0127         validateattributes(x, {'numeric'}, {'real','nonempty','vector'});
0128         validateattributes(y, {'numeric'}, {'real','nonempty','vector'});
0129         x = x(:);
0130         y = y(:);
0131         
0132         % normalize data
0133         mx = mean(x);
0134         my = mean(y);
0135         sx = (max(x)-min(x))/2;
0136         sy = (max(y)-min(y))/2;
0137         smax = max(sx,sy);
0138         sx = smax;
0139         sy = smax;
0140         x = (x-mx)/sx;
0141         y = (y-my)/sy;
0142         
0143         % build design matrix
0144         D = [ x.^2  x.*y  y.^2  x  y  ones(size(x)) ];
0145         
0146         % build scatter matrix
0147         S = D'*D;
0148         
0149         % build 6x6 constraint matrix
0150         C = zeros(6,6);
0151         C(1,3) = -2;
0152         C(2,2) = 1;
0153         C(3,1) = -2;
0154         
0155         p = ellipsefit_robust(S,-C);
0156         
0157         % unnormalize
0158         p(:) = ...
0159             [ p(1)*sy*sy ...
0160             ; p(2)*sx*sy ...
0161             ; p(3)*sx*sx ...
0162             ; -2*p(1)*sy*sy*mx - p(2)*sx*sy*my + p(4)*sx*sy*sy ...
0163             ; -p(2)*sx*sy*mx - 2*p(3)*sx*sx*my + p(5)*sx*sx*sy ...
0164             ; p(1)*sy*sy*mx*mx + p(2)*sx*sy*mx*my + p(3)*sx*sx*my*my - p(4)*sx*sy*sy*mx - p(5)*sx*sx*sy*my + p(6)*sx*sx*sy*sy ...
0165             ];
0166         
0167         p = p ./ norm(p);
0168     end
0169 
0170     function p = ellipsefit_robust(R, Q)
0171         % Constrained ellipse fit by solving a modified eigenvalue problem.
0172         % The method is numerically stable.
0173         %
0174         % Input arguments:
0175         % R:
0176         %    positive semi-definite data covariance matrix
0177         % Q:
0178         %    constraint matrix in parameters x^2, xy, y^2, x, y and 1.
0179         %
0180         % Output arguments:
0181         % p:
0182         %    estimated parameters (taking constraints into account)
0183         
0184         % References:
0185         % Radim Halir and Jan Flusser, "Numerically stable direct least squares fitting of
0186         %    ellipses", 1998
0187         
0188         % Copyright 2012 Levente Hunyadi
0189         
0190         validateattributes(R, {'numeric'}, {'real','2d','size',[6,6]});
0191         validateattributes(Q, {'numeric'}, {'real','2d','size',[6,6]});
0192         
0193         % check that constraint matrix has all zeros except in upper left block
0194         assert( nnz(Q(4:6,:)) == 0 );
0195         assert( nnz(Q(:,4:6)) == 0 );
0196         
0197         S1 = R(1:3,1:3);     % quadratic part of the scatter matrix
0198         S2 = R(1:3,4:6);     % combined part of the scatter matrix
0199         S3 = R(4:6,4:6);     % linear part of the scatter matrix
0200         T = -(S3 \ S2');     % for getting a2 from a1
0201         M = S1 + S2 * T;     % reduced scatter matrix
0202         M = Q(1:3,1:3) \ M;  % premultiply by inv(C1), e.g. M = [M(3,:)./2 ; -M(2,:) ; M(1,:)./2] for an ellipse
0203         [evec,~] = eig(M);   % solve eigensystem
0204         
0205         % evaluate a'*C*a, e.g. cond = 4 * evec(1,:).*evec(3,:) - evec(2,:).^2 for an ellipse
0206         cond = zeros(1,size(evec,2));
0207         for k = 1 : numel(cond)
0208             cond(k) = evec(:,k)'*Q(1:3,1:3)*evec(:,k);
0209         end
0210         
0211         % eigenvector for minimum positive eigenvalue
0212         evec = evec(:,cond > 0);
0213         cond = cond(cond > 0);
0214         [~,ix] = min(cond);
0215         p1 = evec(:,ix);  % eigenvector for minimum positive eigenvalue
0216         
0217         % ellipse coefficients
0218         p = [p1 ; T * p1];
0219     end
0220 
0221     function varargout = ellipse_im2ex(varargin)
0222         % Cast ellipse defined with implicit parameter vector to explicit form.
0223         %
0224         
0225         % Copyright 2011 Levente Hunyadi
0226         
0227         if nargin > 1
0228             narginchk(6,6);
0229             for k = 1 : 6
0230                 validateattributes(varargin{k}, {'numeric'}, {'real','scalar'});
0231             end
0232             elli = ellipse_explicit(varargin{:});
0233         else
0234             narginchk(1,1);
0235             p = varargin{1};
0236             validateattributes(p, {'numeric'}, {'real','vector'});
0237             p = p(:);
0238             validateattributes(p, {'numeric'}, {'size',[6 1]});
0239             elli = ellipse_explicit(p(1), 0.5*p(2), p(3), 0.5*p(4), 0.5*p(5), p(6));
0240         end
0241         if nargout > 1
0242             varargout = num2cell(elli);
0243         else
0244             varargout{1} = elli;
0245         end
0246         
0247         function elli = ellipse_explicit(a,b,c,d,f,g)
0248             % Cast ellipse defined with explicit parameter vector to implicit form.
0249             
0250             % helper quantities
0251             N = 2*(a*f^2+c*d^2+g*b^2-2*b*d*f-a*c*g);
0252             D = b^2-a*c;
0253             S = realsqrt((a-c)^2+4*b^2);
0254             
0255             % semi-axes
0256             ap = realsqrt( N/(D*(S-(a+c))) );
0257             bp = realsqrt( N/(D*(-S-(a+c))) );
0258             semia = max(ap,bp);
0259             semib = min(ap,bp);
0260             
0261             % center
0262             c1 = (c*d-b*f)/D;
0263             c2 = (a*f-b*d)/D;
0264             
0265             % angle of tilt
0266             if b ~= 0
0267                 if abs(a) < abs(c)
0268                     phi = 0.5 * acot((a-c)/(2*b));
0269                 else
0270                     phi = 0.5 * pi + 0.5 * acot((a-c)/(2*b));
0271                 end
0272             else
0273                 if abs(a) < abs(c)
0274                     phi = 0;
0275                 else  % a > c
0276                     phi = 0.5*pi;
0277                 end
0278             end
0279 
0280             elli = [c1,c2,semia,semib,phi];
0281         end
0282     end
0283 end
fitEllipse3d

PURPOSE

SYNOPSIS

DESCRIPTION

CROSS-REFERENCE INFORMATION

SUBFUNCTIONS

SOURCE CODE
