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 returned0001 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;