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 % Created: 2017-08-11
0037 % Copyright 2017
0038 
0039 parser = inputParser;
0040 addRequired(parser, 'points', @(x) validateattributes(x, {'numeric'},...
0041     {'ncols',3,'real','finite','nonnan'}));
0042 addOptional(parser,'visualization',false,@islogical);
0043 parse(parser,points,varargin{:});
0044 points=parser.Results.points;
0045 
0046 % Mean of all points
0047 meanPoint = mean(points,1);
0048 % Center points around origin
0049 centeredPoints = bsxfun(@minus, points, meanPoint);
0050 
0051 % Transformation to x-y plane
0052 [~,~,R]=svd(centeredPoints);
0053 tfmPoints = transformPoint3d(centeredPoints, R');
0054 
0055 % Fit ellipse
0056 parEllipse = ellipsefit_direct(tfmPoints(:,1),tfmPoints(:,2));
0057 % Convert to explicit form
0058 [X, Y, A, B, phi2D] = ellipse_im2ex(parEllipse);
0059 
0060 % Transform to fitted 2d ellipse
0061 TFM2D = createRotationOz(phi2D);
0062 TFM2D(1:3,4)=[X; Y; 0];
0063 
0064 % Transformation back to 3d space
0065 TFM3D = [inv(R'), meanPoint'; 0 0 0 1]*TFM2D;
0066 
0067 % Extract translation
0068 center = TFM3D(1:3,4)';
0069 
0070 % Extract rotation
0071 TFM3D_ROT=TFM3D(1:3,1:3);
0072 % Convert to euler angles
0073 [PHI, THETA, PSI] = rotation3dToEulerAngles(TFM3D_ROT,'ZYZ');
0074 
0075 % Test if psi is correct
0076 TFM3D_test = eulerAnglesToRotation3d(PHI, THETA, PSI,'ZYZ');
0077 if ~all(all(ismembertol(TFM3D_test(1:3,1:3), TFM3D_ROT)))
0078     PSI=-1*PSI;
0079 end
0080 
0081 % matGeom format
0082 fittedEllipse3d=[center A B THETA PHI PSI];
0083 
0084 %% Visualization
0085 if parser.Results.visualization
0086     
0087     figure('Color','w'); axis equal tight; hold on; view(3)
0088     xlabel('x'); ylabel('y'); zlabel('z');
0089     
0090     % Input points
0091     scatter3(points(:,1),points(:,2),points(:,3), 'r', 'filled')
0092     
0093     % Centered points
0094     scatter3(centeredPoints(:,1),centeredPoints(:,2),centeredPoints(:,3), 'g', 'filled')
0095     
0096     % SVD points
0097     scatter3(tfmPoints(:,1),tfmPoints(:,2),tfmPoints(:,3), 'b', 'filled')
0098     % planeProps.FaceAlpha=0.25;
0099     % planeProps.FaceColor='b';
0100     % drawPlane3d([0 0 0 1 0 0 0 1 0],planeProps)
0101     
0102     % Fitted ellipse
0103     drawEllipse3d(fittedEllipse3d)
0104 end
0105 
0106 %% Nested functions
0107     function p = ellipsefit_direct(x,y)
0108         % Direct least squares fitting of ellipses.
0109         %
0110         % Input arguments:
0111         % x,y;
0112         %    x and y coodinates of 2D points
0113         %
0114         % Output arguments:
0115         % p:
0116         %    a 6-parameter vector of the algebraic ellipse fit with
0117         %    p(1)*x^2 + p(2)*x*y + p(3)*y^2 + p(4)*x + p(5)*y + p(6) = 0
0118         %
0119         % References:
0120         % Andrew W. Fitzgibbon, Maurizio Pilu and Robert B. Fisher, "Direct Least
0121         %    Squares Fitting of Ellipses", IEEE Trans. PAMI 21, 1999, pp476-480.
0122         
0123         % Copyright 2011 Levente Hunyadi
0124         
0125         narginchk(2,2);
0126         validateattributes(x, {'numeric'}, {'real','nonempty','vector'});
0127         validateattributes(y, {'numeric'}, {'real','nonempty','vector'});
0128         x = x(:);
0129         y = y(:);
0130         
0131         % normalize data
0132         mx = mean(x);
0133         my = mean(y);
0134         sx = (max(x)-min(x))/2;
0135         sy = (max(y)-min(y))/2;
0136         smax = max(sx,sy);
0137         sx = smax;
0138         sy = smax;
0139         x = (x-mx)/sx;
0140         y = (y-my)/sy;
0141         
0142         % build design matrix
0143         D = [ x.^2  x.*y  y.^2  x  y  ones(size(x)) ];
0144         
0145         % build scatter matrix
0146         S = D'*D;
0147         
0148         % build 6x6 constraint matrix
0149         C = zeros(6,6);
0150         C(1,3) = -2;
0151         C(2,2) = 1;
0152         C(3,1) = -2;
0153         
0154         if 1
0155             p = ellipsefit_robust(S,-C);
0156         elseif 0
0157             % solve eigensystem
0158             [gevec, geval] = eig(S,C);
0159             geval = diag(geval);
0160             
0161             % extract eigenvector corresponding to unique negative (nonpositive) eigenvalue
0162             p = gevec(:,geval < 0 & ~isinf(geval));
0163             r = geval(geval < 0 & ~isinf(geval));
0164         elseif 0
0165             % formulation as convex optimization problem
0166             gamma = 0; %#ok<*UNRCH>
0167             cvx_begin sdp
0168             variable('gamma');
0169             variable('lambda');
0170             
0171             maximize(gamma);
0172             lambda >= 0; %#ok<*VUNUS>
0173             %[ S + lambda*C,       zeros(size(S,1),1) ...
0174             %; zeros(1,size(S,2)), lambda - gamma ...
0175             %] >= 0;
0176             S + lambda*C >= 0;
0177             lambda - gamma >= 0;
0178             cvx_end
0179             
0180             % recover primal optimal values from dual
0181             [evec, eval] = eig(S + lambda*C);
0182             eval = diag(eval);
0183             [~,ix] = min(abs(eval));
0184             p = evec(:,ix);
0185         end
0186         
0187         % unnormalize
0188         p(:) = ...
0189             [ p(1)*sy*sy ...
0190             ; p(2)*sx*sy ...
0191             ; p(3)*sx*sx ...
0192             ; -2*p(1)*sy*sy*mx - p(2)*sx*sy*my + p(4)*sx*sy*sy ...
0193             ; -p(2)*sx*sy*mx - 2*p(3)*sx*sx*my + p(5)*sx*sx*sy ...
0194             ; 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 ...
0195             ];
0196         
0197         p = p ./ norm(p);
0198     end
0199 
0200     function p = ellipsefit_robust(R, Q)
0201         % Constrained ellipse fit by solving a modified eigenvalue problem.
0202         % The method is numerically stable.
0203         %
0204         % Input arguments:
0205         % R:
0206         %    positive semi-definite data covariance matrix
0207         % Q:
0208         %    constraint matrix in parameters x^2, xy, y^2, x, y and 1.
0209         %
0210         % Output arguments:
0211         % p:
0212         %    estimated parameters (taking constraints into account)
0213         
0214         % References:
0215         % Radim Halir and Jan Flusser, "Numerically stable direct least squares fitting of
0216         %    ellipses", 1998
0217         
0218         % Copyright 2012 Levente Hunyadi
0219         
0220         validateattributes(R, {'numeric'}, {'real','2d','size',[6,6]});
0221         validateattributes(Q, {'numeric'}, {'real','2d','size',[6,6]});
0222         
0223         % check that constraint matrix has all zeros except in upper left block
0224         assert( nnz(Q(4:6,:)) == 0 );
0225         assert( nnz(Q(:,4:6)) == 0 );
0226         
0227         S1 = R(1:3,1:3);     % quadratic part of the scatter matrix
0228         S2 = R(1:3,4:6);     % combined part of the scatter matrix
0229         S3 = R(4:6,4:6);     % linear part of the scatter matrix
0230         T = -(S3 \ S2');     % for getting a2 from a1
0231         M = S1 + S2 * T;     % reduced scatter matrix
0232         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
0233         [evec,~] = eig(M);   % solve eigensystem
0234         
0235         % evaluate a'*C*a, e.g. cond = 4 * evec(1,:).*evec(3,:) - evec(2,:).^2 for an ellipse
0236         cond = zeros(1,size(evec,2));
0237         for k = 1 : numel(cond)
0238             cond(k) = evec(:,k)'*Q(1:3,1:3)*evec(:,k);
0239         end
0240         
0241         % eigenvector for minimum positive eigenvalue
0242         evec = evec(:,cond > 0);
0243         cond = cond(cond > 0);
0244         [~,ix] = min(cond);
0245         p1 = evec(:,ix);  % eigenvector for minimum positive eigenvalue
0246         
0247         % ellipse coefficients
0248         p = [p1 ; T * p1];
0249     end
0250 
0251     function varargout = ellipse_im2ex(varargin)
0252         % Cast ellipse defined with implicit parameter vector to explicit form.
0253         %
0254         % See also: ellipse_ex2im
0255         
0256         % Copyright 2011 Levente Hunyadi
0257         
0258         if nargin > 1
0259             narginchk(6,6);
0260             for k = 1 : 6
0261                 validateattributes(varargin{k}, {'numeric'}, {'real','scalar'});
0262             end
0263             [c1,c2,a,b,phi] = ellipse_explicit(varargin{:});
0264         else
0265             narginchk(1,1);
0266             p = varargin{1};
0267             validateattributes(p, {'numeric'}, {'real','vector'});
0268             p = p(:);
0269             validateattributes(p, {'numeric'}, {'size',[6 1]});
0270             [c1,c2,a,b,phi] = ellipse_explicit(p(1), 0.5*p(2), p(3), 0.5*p(4), 0.5*p(5), p(6));
0271         end
0272         if nargout > 1
0273             varargout = num2cell([c1,c2,a,b,phi]);
0274         else
0275             varargout{1} = [c1,c2,a,b,phi];
0276         end
0277         
0278         function [c1,c2,semia,semib,phi] = ellipse_explicit(a,b,c,d,f,g)
0279             % Cast ellipse defined with explicit parameter vector to implicit form.
0280             
0281             % helper quantities
0282             N = 2*(a*f^2+c*d^2+g*b^2-2*b*d*f-a*c*g);
0283             D = b^2-a*c;
0284             S = realsqrt((a-c)^2+4*b^2);
0285             
0286             % semi-axes
0287             ap = realsqrt( N/(D*(S-(a+c))) );
0288             bp = realsqrt( N/(D*(-S-(a+c))) );
0289             semia = max(ap,bp);
0290             semib = min(ap,bp);
0291             
0292             % center
0293             c1 = (c*d-b*f)/D;
0294             c2 = (a*f-b*d)/D;
0295             
0296             % angle of tilt
0297             if b ~= 0
0298                 if abs(a) < abs(c)
0299                     phi = 0.5*acot((a-c)/(2*b));
0300                 else
0301                     phi = 0.5*pi+0.5*acot((a-c)/(2*b));
0302                 end
0303             else
0304                 if abs(a) < abs(c)
0305                     phi = 0;
0306                 else  % a > c
0307                     phi = 0.5*pi;
0308                 end
0309             end
0310         end
0311     end
0312 end
fitEllipse3d

PURPOSE

SYNOPSIS

DESCRIPTION

CROSS-REFERENCE INFORMATION

SUBFUNCTIONS

SOURCE CODE
