# fit_ellipse

## PURPOSE

## SYNOPSIS

function ellipse_t = fit_ellipse( x,y,plot_opt )

## DESCRIPTION

     fit_ellipse - finds the best fit to an ellipse for the given set of points.

     Format:   ellipse_t = fit_ellipse( x,y,axis_handle )

     Input:    x,y         - a set of points in 2 column vectors. AT LEAST 5 points are needed !
               axis_handle - optional. a handle to an axis, at which the estimated ellipse
                             will be drawn along with it's axes

     Output:   ellipse_t - structure that defines the best fit to an ellipse
                           a           - sub axis (radius) of the X axis of the non-tilt ellipse
                           b           - sub axis (radius) of the Y axis of the non-tilt ellipse
                           phi         - orientation in radians of the ellipse (tilt)
                           X0          - center at the X axis of the non-tilt ellipse
                           Y0          - center at the Y axis of the non-tilt ellipse
                           X0_in       - center at the X axis of the tilted ellipse
                           Y0_in       - center at the Y axis of the tilted ellipse
                           long_axis   - size of the long axis of the ellipse
                           short_axis  - size of the short axis of the ellipse
                           status      - status of detection of an ellipse

     Note:     if an ellipse was not detected (but a parabola or hyperbola), then
               an empty structure is returned

## CROSS-REFERENCE INFORMATION

This function calls:

This function is called by:

- FTrack FTRACK

## SOURCE CODE

    0001 function ellipse_t = fit_ellipse( x,y,plot_opt )
    0002 %
    0003 % fit_ellipse - finds the best fit to an ellipse for the given set of points.
    0004 %
    0005 % Format:   ellipse_t = fit_ellipse( x,y,axis_handle )
    0006 %
    0007 % Input:    x,y         - a set of points in 2 column vectors. AT LEAST 5 points are needed !
    0008 %           axis_handle - optional. a handle to an axis, at which the estimated ellipse
    0009 %                         will be drawn along with it's axes
    0010 %
    0011 % Output:   ellipse_t - structure that defines the best fit to an ellipse
    0012 %                       a           - sub axis (radius) of the X axis of the non-tilt ellipse
    0013 %                       b           - sub axis (radius) of the Y axis of the non-tilt ellipse
    0014 %                       phi         - orientation in radians of the ellipse (tilt)
    0015 %                       X0          - center at the X axis of the non-tilt ellipse
    0016 %                       Y0          - center at the Y axis of the non-tilt ellipse
    0017 %                       X0_in       - center at the X axis of the tilted ellipse
    0018 %                       Y0_in       - center at the Y axis of the tilted ellipse
    0019 %                       long_axis   - size of the long axis of the ellipse
    0020 %                       short_axis  - size of the short axis of the ellipse
    0021 %                       status      - status of detection of an ellipse
    0022 %
    0023 % Note:     if an ellipse was not detected (but a parabola or hyperbola), then
    0024 %           an empty structure is returned
    0025
    0026 % =====================================================================================
    0027 %                  Ellipse Fit using Least Squares criterion
    0028 % =====================================================================================
    0029 % We will try to fit the best ellipse to the given measurements. the mathematical
    0030 % representation of use will be the CONIC Equation of the Ellipse which is:
    0031 %
    0032 %    Ellipse = a*x^2 + b*x*y + c*y^2 + d*x + e*y + f = 0
    0033 %
    0034 % The fit-estimation method of use is the Least Squares method (without any weights)
    0035 % The estimator is extracted from the following equations:
    0036 %
    0037 %    g(x,y;A) := a*x^2 + b*x*y + c*y^2 + d*x + e*y = f
    0038 %
    0039 %    where:
    0040 %       A   - is the vector of parameters to be estimated (a,b,c,d,e)
    0041 %       x,y - is a single measurement
    0042 %
    0043 % We will define the cost function to be:
    0044 %
    0045 %   Cost(A) := (g_c(x_c,y_c;A)-f_c)'*(g_c(x_c,y_c;A)-f_c)
    0046 %            = (X*A+f_c)'*(X*A+f_c)
    0047 %            = A'*X'*X*A + 2*f_c'*X*A + N*f^2
    0048 %
    0049 %   where:
    0050 %       g_c(x_c,y_c;A) - vector function of ALL the measurements
    0051 %                        each element of g_c() is g(x,y;A)
    0052 %       X              - a matrix of the form: [x_c.^2, x_c.*y_c, y_c.^2, x_c, y_c ]
    0053 %       f_c            - is actually defined as ones(length(f),1)*f
    0054 %
    0055 % Derivation of the Cost function with respect to the vector of parameters "A" yields:
    0056 %
    0057 %   A'*X'*X = -f_c'*X = -f*ones(1,length(f_c))*X = -f*sum(X)
    0058 %
    0059 % Which yields the estimator:
    0060 %
    0061 %       ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
    0062 %       |  A_least_squares = -f*sum(X)/(X'*X) ->(normalize by -f) = sum(X)/(X'*X)  |
    0063 %       ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
    0064 %
    0065 % (We will normalize the variables by (-f) since "f" is unknown and can be accounted for later on)
    0066 %
    0067 % NOW, all that is left to do is to extract the parameters from the Conic Equation.
    0068 % We will deal the vector A into the variables: (A,B,C,D,E) and assume F = -1;
    0069 %
    0070 %    Recall the conic representation of an ellipse:
    0071 %
    0072 %       A*x^2 + B*x*y + C*y^2 + D*x + E*y + F = 0
    0073 %
    0074 % We will check if the ellipse has a tilt (=orientation). The orientation is present
    0075 % if the coefficient of the term "x*y" is not zero. If so, we first need to remove the
    0076 % tilt of the ellipse.
    0077 %
    0078 % If the parameter "B" is not equal to zero, then we have an orientation (tilt) to the ellipse.
    0079 % we will remove the tilt of the ellipse so as to remain with a conic representation of an
    0080 % ellipse without a tilt, for which the math is more simple:
    0081 %
    0082 % Non tilt conic rep.:  A`*x^2 + C`*y^2 + D`*x + E`*y + F` = 0
    0083 %
    0084 % We will remove the orientation using the following substitution:
    0085 %
    0086 %   Replace x with cx+sy and y with -sx+cy such that the conic representation is:
    0087 %
    0088 %   A(cx+sy)^2 + B(cx+sy)(-sx+cy) + C(-sx+cy)^2 + D(cx+sy) + E(-sx+cy) + F = 0
    0089 %
    0090 %   where:      c = cos(phi)    ,   s = sin(phi)
    0091 %
    0092 %   and simplify...
    0093 %
    0094 %       x^2(A*c^2 - Bcs + Cs^2) + xy(2A*cs +(c^2-s^2)B -2Ccs) + ...
    0095 %           y^2(As^2 + Bcs + Cc^2) + x(Dc-Es) + y(Ds+Ec) + F = 0
    0096 %
    0097 %   The orientation is easily found by the condition of (B_new=0) which results in:
    0098 %
    0099 %   2A*cs +(c^2-s^2)B -2Ccs = 0  ==> phi = 1/2 * atan( b/(c-a) )
    0100 %
    0101 %   Now the constants   c=cos(phi)  and  s=sin(phi)  can be found, and from them
    0102 %   all the other constants A`,C`,D`,E` can be found.
    0103 %
    0104 %   A` = A*c^2 - B*c*s + C*s^2                  D` = D*c-E*s
    0105 %   B` = 2*A*c*s +(c^2-s^2)*B -2*C*c*s = 0      E` = D*s+E*c
    0106 %   C` = A*s^2 + B*c*s + C*c^2
    0107 %
    0108 % Next, we want the representation of the non-tilted ellipse to be as:
    0109 %
    0110 %       Ellipse = ( (X-X0)/a )^2 + ( (Y-Y0)/b )^2 = 1
    0111 %
    0112 %       where:  (X0,Y0) is the center of the ellipse
    0113 %               a,b     are the ellipse "radiuses" (or sub-axis)
    0114 %
    0115 % Using a square completion method we will define:
    0116 %
    0117 %       F`` = -F` + (D`^2)/(4*A`) + (E`^2)/(4*C`)
    0118 %
    0119 %       Such that:    a`*(X-X0)^2 = A`(X^2 + X*D`/A` + (D`/(2*A`))^2 )
    0120 %                     c`*(Y-Y0)^2 = C`(Y^2 + Y*E`/C` + (E`/(2*C`))^2 )
    0121 %
    0122 %       which yields the transformations:
    0123 %
    0124 %           X0  =   -D`/(2*A`)
    0125 %           Y0  =   -E`/(2*C`)
    0126 %           a   =   sqrt( abs( F``/A` ) )
    0127 %           b   =   sqrt( abs( F``/C` ) )
    0128 %
    0129 % And finally we can define the remaining parameters:
    0130 %
    0131 %   long_axis   = 2 * max( a,b )
    0132 %   short_axis  = 2 * min( a,b )
    0133 %   Orientation = phi
    0134 %
    0135 %
    0136
    0137 % initialize
    0138 orientation_tolerance = 1e-3;
    0139
    0140 % empty warning stack
    0141 warning( '' );
    0142
    0143 % prepare vectors, must be column vectors
    0144 x = x(:);
    0145 y = y(:);
    0146
    0147 % remove bias of the ellipse - to make matrix inversion more accurate. (will be added later on).
    0148 mean_x = mean(x);
    0149 mean_y = mean(y);
    0150 x = x-mean_x;
    0151 y = y-mean_y;
    0152
    0153 % the estimation for the conic equation of the ellipse
    0154 X = [x.^2, x.*y, y.^2, x, y ];
    0155 a = sum(X)/(X'*X);
    0156
    0157 % check for warnings
    0158 if ~isempty( lastwarn )
    0159     disp( 'stopped because of a warning regarding matrix inversion' );
    0160     ellipse_t = [];
    0161     return
    0162 end
    0163
    0164 % extract parameters from the conic equation
    0165 [a,b,c,d,e] = deal( a(1),a(2),a(3),a(4),a(5) );
    0166
    0167 % remove the orientation from the ellipse
    0168 if ( min(abs(b/a),abs(b/c)) > orientation_tolerance )
    0169
    0170     orientation_rad = 1/2 * atan( b/(c-a) );
    0171     cos_phi = cos( orientation_rad );
    0172     sin_phi = sin( orientation_rad );
    0173     [a,b,c,d,e] = deal(...
    0174         a*cos_phi^2 - b*cos_phi*sin_phi + c*sin_phi^2,...
    0175         0,...
    0176         a*sin_phi^2 + b*cos_phi*sin_phi + c*cos_phi^2,...
    0177         d*cos_phi - e*sin_phi,...
    0178         d*sin_phi + e*cos_phi );
    0179     [mean_x,mean_y] = deal( ...
    0180         cos_phi*mean_x - sin_phi*mean_y,...
    0181         sin_phi*mean_x + cos_phi*mean_y );
    0182 else
    0183     orientation_rad = 0;
    0184     cos_phi = cos( orientation_rad );
    0185     sin_phi = sin( orientation_rad );
    0186 end
    0187
    0188 % check if conic equation represents an ellipse
    0189 test = a*c;
    0190 switch (1)
    0191 case (test>0),  status = '';
    0192 case (test==0), status = 'Parabola found';  warning( 'fit_ellipse: Did not locate an ellipse' );
    0193 case (test<0),  status = 'Hyperbola found'; warning( 'fit_ellipse: Did not locate an ellipse' );
    0194 end
    0195
    0196 % if we found an ellipse return it's data
    0197 if (test>0)
    0198
    0199     % make sure coefficients are positive as required
    0200     if (a<0), [a,c,d,e] = deal( -a,-c,-d,-e ); end
    0201
    0202     % final ellipse parameters
    0203     X0          = mean_x - d/2/a;
    0204     Y0          = mean_y - e/2/c;
    0205     F           = 1 + (d^2)/(4*a) + (e^2)/(4*c);
    0206     [a,b]       = deal( sqrt( F/a ),sqrt( F/c ) );
    0207     long_axis   = 2*max(a,b);
    0208     short_axis  = 2*min(a,b);
    0209
    0210     % rotate the axes backwards to find the center point of the original TILTED ellipse
    0211     R           = [ cos_phi sin_phi; -sin_phi cos_phi ];
    0212     P_in        = R * [X0;Y0];
    0213     X0_in       = P_in(1);
    0214     Y0_in       = P_in(2);
    0215
    0216     % pack ellipse into a structure
    0217     ellipse_t = struct( ...
    0218         'a',a,...
    0219         'b',b,...
    0220         'phi',orientation_rad,...
    0221         'X0',X0,...
    0222         'Y0',Y0,...
    0223         'X0_in',X0_in,...
    0224         'Y0_in',Y0_in,...
    0225         'long_axis',long_axis,...
    0226         'short_axis',short_axis,...
    0227         'status','' );
    0228 else
    0229     % report an empty structure
    0230     ellipse_t = struct( ...
    0231         'a',[],...
    0232         'b',[],...
    0233         'phi',[],...
    0234         'X0',[],...
    0235         'Y0',[],...
    0236         'X0_in',[],...
    0237         'Y0_in',[],...
    0238         'long_axis',[],...
    0239         'short_axis',[],...
    0240         'status',status );
    0241 end
    0242
    0243 % check if we need to plot an ellipse with it's axes.
    0244 %if (nargin>2) & ~isempty( axis_handle ) & (test>0)
    0245
    0246     % rotation matrix to rotate the axes with respect to an angle phi
    0247     R = [ cos_phi sin_phi; -sin_phi cos_phi ];
    0248
    0249     % the axes
    0250     ver_line        = [ [X0 X0]; Y0+b*[-1 1] ];
    0251     horz_line       = [ X0+a*[-1 1]; [Y0 Y0] ];
    0252     new_ver_line    = R*ver_line;
    0253     new_horz_line   = R*horz_line;
    0254
    0255     % the ellipse
    0256     theta_r         = linspace(0,2*pi);
    0257     ellipse_x_r     = X0 + a*cos( theta_r );
    0258     ellipse_y_r     = Y0 + b*sin( theta_r );
    0259     xaligned_ellipse = [ellipse_x_r;ellipse_y_r];
    0260     rotated_ellipse = R * [ellipse_x_r;ellipse_y_r];
    0261
    0262     if (strcmp(plot_opt,'y'))
    0263         % draw
    0264         hold on
    0265         plot( rotated_ellipse(1,:),rotated_ellipse(2,:),'r' );
    0266         drawnow
    0267     end
    0268
    0269     ellipse_t.xaligned_ellipse = xaligned_ellipse;
    0270     ellipse_t.rotated_ellipse = rotated_ellipse;
    0271     ellipse_t.ellipse_x_r = ellipse_x_r;
    0272     ellipse_t.ellipse_y_r = ellipse_y_r;
    0273     ellipse_t.R = R;