fit.m

改进等高线拟合代码

function fit(x,y,ftype,number)

% Fit Handles different fit models in GUI.

% All the software included in this package is presented as is.
% It may be distributed freely. The author can, however, not be
% held responsible for any problems whatever.
% 
% Designed by Johan Baeten, Walter Verdonck
% Last updated: 22-03-2000
% [e-mail]


% Calculate and plot a fit trough the given points [x,y]
% ftype = 1 : 1st order fit (straigth lines)
%      = 2 : Single full degree 2nd order curve
%      = 3 : Single 3rd order curve
%      = 4 : 3rd order (cubic) spline
%      = 5 : Matlab's spline (also cubic)
%      = 6 : Smoothing cubic spline
%      = 7 Parametric Hermite spline (also cubic)
%      = 8 : Natural cubic spline
%      = 9 : Single Parametric 3rd order curve
%      = 10 : Arclength - Parametric Hermite spline (also cubic)
%      = 11 : Arclength - Natural cubic spline
%      = 12 : Single Arclength - Parametric 3rd order curve
% number is an optional number indicating the color and style


global NR_OF_FITS;
global SUBMENUHANDLES;

nargs = nargin;

if nargs== 3, 
   number = NR_OF_FITS;
elseif ~(nargs==4),
   seterror( ' Bug: Wrong number in FIT arguments')
   return
end

number = abs(number);
number_style = abs(number);
while number > 3 ,
    number = number-3;
end
while number_style > 4 ,
    number_style = number_style-4;
end

colors =['r';'b';'m'];
styles =['- ';': ';'-.';'--'];

mainfig = gcf;


%%%%%%%%%%%%%%%%%%%%%
% select ftype start
%%%%%%%%%%%%%%%%%%%%%

if ftype == 1,
  hold on,
  plot(x+0.5,y+0.5,[colors(number),styles(number_style,:)]);
  return    % skip plt ot and of file
  
%%%%%%%%%%%%%%%%%
elseif  ftype == 2,
  % Second order TLS Fit
  for i = 1: length(x),
    A(i,1) = x(i)*x(i);
    A(i,2) = x(i)*y(i);
    A(i,3) = y(i)*y(i);
    A(i,4) = x(i);
    A(i,5) = y(i);
    A(i,6) = 1;
  end
  [u,s,v]=svd(A);
  par = v(:,6);
 
  x1=[min(x):(max(x)-min(x))/100:max(x)];
  discr = sqrt((par(2)*x1 + par(5)).^2 - 4*par(3)*(par(1)* x1.^2 + par(4)*x1 +par(6)));
  if isreal(discr),
    if abs(par(3))<eps,
      if abs(par(2))>eps | abs(par(5))>eps,
        tel=1;
        for i=1:length(x1);
          if abs(par(2)*x1(i)+par(5))>eps,
            x2(tel) = x1(i);
            y2(tel) = -(par(1)*x1(i).^2+par(4)*x1(i)+par(6))./(par(2)*x1(i)+par(5));
            tel=tel+1;
          end
        end
        hold on, plot(x2+0.5,y2+0.5,[colors(number),styles(number_style,:)]);
      else
        seterror(' In the fitted function axx+bxy+cyy+dx+ey+f=0 are b,c and e equal zero!!!)');
      end
    else
      y1=(-par(2)*x1-par(5)+discr)/(2*par(3));
      y2=(-par(2)*x1-par(5)-discr)/(2*par(3));
      hold on, plot(x1+0.5,y1+0.5,[colors(number),styles(number_style,:)],...
                    x1+0.5,y2+0.5,[colors(number),styles(number_style,:)]);
    end

  else
    y1=[min(y):(max(y)-min(y))/100:max(y)];
    discr = sqrt((par(2)*y1 + par(4)).^2 - 4*par(1)*(par(3)* y1.^2 + par(5)*y1 + par(6)));
    if isreal(discr),
      if abs(par(1))<eps,
        if abs(par(2))>eps | abs(par(4))>eps,
          tel=1;
          for i=1:length(y1);
            if abs(par(2)*y1(i)+par(4))>eps,
              y2(tel) = y1(i);
              x2(tel) = -(par(3)*y1(i).^2+par(5)*y1(i)+par(6))./(par(2)*y1(i)+par(4));
              tel=tel+1;
            end
          end
          hold on, plot(x2+0.5,y2+0.5,[colors(number),styles(number_style,:)]);
        else
          seterror(' In the fitted function axx+bxy+cyy+dx+ey+f=0 are a,b and d equal zero!!!)');
        end
      else
       x1=(-par(2)*y1-par(4)+discr)/(2*par(1));
       x2=(-par(2)*y1-par(4)-discr)/(2*par(1));
       hold on, plot(x1+0.5,y1+0.5,[colors(number),styles(number_style,:)],...
                     x2+0.5,y1+0.5,[colors(number),styles(number_style,:)]);
      end
    else
      seterror(' Twice complex discriminant. Can''t draw fit.')
      return
    end
  end
  if strcmp(get(SUBMENUHANDLES(1),'checked'),'on'),
      seterror(' Curvature calculation not available.')  
  end
  return % skip plot at end of file
   

%%%%%%%%%%%%%%%%%
elseif  ftype == 3,     % Single 3rd order polynome

  if  std(x)<std(y),   % Adapted by WV
     p = polyfit(y,x,3);
     a=p(1);
     b=p(2);
     c=p(3);
     d=p(4);

     if strcmp(get(SUBMENUHANDLES(15),'checked'),'on'), % extrapolate
        [ylim] = get(gca,'ylim')-0.5;
        if y(1)<y(end),
           yi=[ylim(1):(ylim(2)-ylim(1))/100:ylim(2)];      
        else
           yi=[ylim(2):(ylim(1)-ylim(2))/100:ylim(1)];      
        end
     else  
        if y(1)<y(end),
	        yi=[min(y):(max(y)-min(y))/100:max(y)];
	     else
	        yi=[max(y):(min(y)-max(y))/100:min(y)];
	     end
     end
     xi=a*yi.^3+b*yi.^2+c*yi+d; 
     x1= c+yi.*(2*b+yi.*3*a);   % first derivative
     x2= 2*b+yi.*3*2*a;         % 2nd derivative
     y1 = 0;
     
     
%     disp(' 3e orde polynoom')
%     disp('xr_pix = ')      % stuurwaarden
%     disp(num2str(polyval(p, 64)))
%     disp('alfa_pix = ')
%     disp(num2str(3*a*64.^2+2*b*64+c));
%     disp('kappa_pix = ')
%     disp(num2str(-2*b/sqrt((1+c^2)^3)));

  else 
     p = polyfit(x,y,3);
     a=p(1);
     b=p(2);
     c=p(3);
     d=p(4);
     if strcmp(get(SUBMENUHANDLES(15),'checked'),'on'), % extrapolate     
	     [xlim] = get(gca,'xlim')-0.5;
        if x(1)<x(end),	     
	    	  xi=[xlim(1):(xlim(2)-xlim(1))/100:xlim(2)];
	     else
	     	  xi=[xlim(2):(xlim(1)-xlim(2))/100:xlim(1)];
	     end
     else
        if x(1)<x(end),
          xi = [min(x):(max(x)-min(x))/100:max(x)];
        else
          xi = [max(x):(min(x)-max(x))/100:min(x)];
        end
     end
     yi=a*xi.^3+b*xi.^2+c*xi+d;
     y1= c+xi.*(2*b+xi.*3*a); 
     y2= 2*b+xi.*3*2*a;
   
  end
  

%%%%%%%%%%%%%%%%%
elseif ftype == 4,         % 3rd order (cubic) spline
   n = length(x);
   if n<3, 
     seterror(' At least 3 points needed for a cubic spline fit !');
     return
   end
   % for the cubic spline fit the x- or the y-values must be monotonic
   xx=x;
   yy=y;
   if ((sort(x)-x) == zeros(1,n)),
      ccase = 1; % x ascending
   elseif ((sort(y)-y) == zeros(1,n)),
      ccase = 2; % y ascending
      xx=y;      % switch
      yy=x;      
   elseif ((sort(max(x)-x)-max(x)+x) == zeros(1,n)),
      ccase = 3;  % x descending
      xx=max(x)-x; % make ascending      
   elseif ((sort(max(y)-y)-max(y)+y) == zeros(1,n)),
      ccase = 4;  % y descending
      xx=max(y)-y; % switch and make ascending
      yy=x;
   else
      seterror(' !! Either x- or y-values must be monotonic to get a good fit!  Maybe ''Sort'' can help.');
      ccase = 1;
   end

	% start calculations
   a=yy;
   [b]=grad7(xx,yy,1e-7);
   [c,d]=hecub(xx,yy,b);
   tel = 1;
   aantal = 20; % # of caluclated points per interval
   for k = 1:n-1,
     	xv = linspace(xx(k),xx(k+1),aantal);
	   xi(tel:tel+aantal-1)= xv;
      dx = xv -xx(k);
      yi(tel:tel+aantal-1)= a(k)+dx.*(b(k)+dx.*(c(k)+d(k).*dx)); 
     	y1(tel:tel+aantal-1)= b(k)+dx.*(2*c(k)+dx.*3*d(k)); 
	   y2(tel:tel+aantal-1)= 2*c(k)+dx.*3*2*d(k);
      tel = tel + aantal-1;
   end   
      

   switch ccase,
     case 1,
       % ok
	  case 2, % swictch x and y
       	 temp = xi; % switch back
       	 xi = yi;
       	 yi = temp; 
     case 3, % x descending -> make x ascending and correct afterwards
      	 xi = max(x)-xi;
     case 4, % y descending -> make y ascending, switch and correct afterwards
          temp = max(y)-xi; % correct and switch back
          xi = yi;
          yi = temp; 
     otherwise
        	seterror('Illegal case in fit.m. This should never happen.');
        	return
   end

%       disp('Cubic spline')
%       disp('xr_pix = ')
%       ind = find(yi<63.5);
%       xr=interp1([yi(ind(1)) yi(ind(1)-1)], [xi(ind(1)) xi(ind(1)-1)], 63.5);
%       k = floor(ind(1)./aantal)+1
%       disp(num2str(xr));
%       disp('alfa_pix = ')
%       3*d(k)*63.5^2+2*c(k)*63.5+b(k)
%       disp(num2str(3*d(k)*63.5^2+2*c(k)*63.5+b(k)));
%       disp('kappa_pix = ')
%       disp(num2str(-2*c(k)/sqrt((1+b(k)^2)^3)));
 
   
%%%%%%%%%%%%%%%%   
elseif ftype == 5,             % Matlab's spline (also cubic)
   n=length(x);
   if ((sort(x)-x) == zeros(1,n)),
     xi=min(x):((max(x)-min(x))/50):max(x);
     yi=spline(x,y,xi);
   else
%     if ((sort(y)-y) == zeros(1,n)), 
        % dummy
%     else
%       seterror(' You get better results if either x- or y-values are monotonic!  Maybe ''Sort'' can help.');
%     end
     yi=min(y):((max(y)-min(y))/50):max(y);
     xi=spline(y,x,yi);
   end


%%%%%%%%%%%%%%%%   
elseif ftype == 6  % smoothing spline   
   p=ones(size(x));
   n = length(x);
   if n<5, 
     seterror(' At least 5 points needed for a smoothing cubic spline fit !')
     return
   end
   % for the smoothing cubic spline fit the x- or the y-values must be monotonic
   xx=x;
   yy=y;   
   if ((sort(x)-x) == zeros(1,n)),
      ccase = 1; % x ascending, OK
   elseif ((sort(y)-y) == zeros(1,n)),
      ccase = 2; % y ascending
      xx=y;      % switch
      yy=x;
   elseif ((sort(max(x)-x)-max(x)+x) == zeros(1,n)),
      ccase = 3;   % x descending
      xx=max(x)-x; % make ascending
   elseif ((sort(max(y)-y)-max(y)+y) == zeros(1,n)),
      ccase = 4;   % y descending
      xx=max(y)-y; % switch and make ascending
      yy=x;
   else
	   seterror(' !! Either x- or y-values must be monotonic to get a good fit!  Maybe ''Sort'' can help.');
      ccase = 1;   
   end

	% start calculations
   [a,b,c,d,y2,c_flag]=cubsum1(n,xx,yy,p);
   if c_flag ~=0,
      seterror(' Error: Singular system');
      return
   end
   tel = 1;
   aantal = 20; % # of caluclated points per interval
   for k = 1:n-1,
      xv = linspace(xx(k),xx(k+1),aantal);
      xi(tel:tel+aantal-1)= xv;
      dx = xv -xx(k);
      yi(tel:tel+aantal-1)= a(k)+dx.*(b(k)+dx.*(c(k)+d(k).*dx)); 
      y1(tel:tel+aantal-1)= b(k)+dx.*(2*c(k)+dx.*3*d(k)); 
      y2(tel:tel+aantal-1)= 2*c(k)+dx.*3*2*d(k);
      tel = tel + aantal-1;
   end
     
   switch ccase
     case 1,
       %ok
     case 2,  
		 temp = xi; % switch back
     	 xi = yi;
     	 yi = temp;        
     case 3,
     	 xi = max(x)-xi;     
     case 4,
	    temp = max(y)-xi; % correct and switch back
       xi = yi;
       yi = temp;      
     otherwise,
     	 seterror('Illegal case in fit.m. This should never happen.');
     	 return     
   end
  

%%%%%%%%%%%%%%%%%
elseif ftype == 7,     % Parametric Hermite spline (also cubic)
   n = length(x);
   if n<3, 
     seterror(' At least 3 points needed for a cubic spline fit !');
     return
   end   
   u =1:n;   
   % first fit van x as a function of u
   ax=x;
   [bx]=grad7(u,x,1e-7);
   [cx,dx]=hecub(u,x,bx);
   % then fit y as a function of u
   ay=y;
   [by]=grad7(u,y,1e-7);
   [cy,dy]=hecub(u,y,by);

   [xi, yi, kappa] = boogkrom(ax, bx, cx, dx, ay, by, cy, dy, u);

%       disp('Parametric Hermite spline')
%       disp('xr_pix = ')
%       ind = find(yi<63.5);
%       xr=interp1([yi(ind(1)) yi(ind(1)-1)], [xi(ind(1)) xi(ind(1)-1)], 63.5);
%       disp(num2str(xr));



%%%%%%%%%%%%%%%%
elseif ftype == 8,    % Parametric Natural Spline
   n = length(x);
   if n<3, 
     seterror(' At least 3 points needed for a cubic spline fit !');
     return
   end
   u =1:n;
   % first fit van x as a function of u
   ax=x;
   [bx]=natcub(u,x);
   [cx,dx]=hecub(u,x,bx);
   % then fit y as a function of u
   ay=y;
   [by]=natcub(u,y);
   [cy,dy]=hecub(u,y,by);
   [xi, yi, kappa] = boogkrom(ax, bx, cx, dx, ay, by, cy, dy, u);

  
%%%%%%%%%%%%%%%%  
elseif ftype == 9    % Parametric 3rd order polynome
   n = length(x);
   u = 1:n;
   u = u - 1;
   % fit for x
   px = polyfit(u,x,3);
   % fit for y
   py = polyfit(u,y,3);
   [xi, yi, kappa] = boogkrom(px(4), px(3), px(2), px(1), py(4), py(3), py(2), py(1), [1 n]);
   
%%%%%%%%%%%%%%%%   
elseif ftype == 10,       % Arclength Parametric Hermite spline (also cubic)
   n = length(x);
   if n<3, 
	  seterror(' At least 3 points needed for a cubic spline fit !')
     return   
   end

   u = cumsum(sqrt((x(2:n)-x(1:n-1)).^2+(y(2:n)-y(1:n-1)).^2));
   u = [0 u];
   % first fit van x as a function of u
   ax=x;
   [bx]=grad7(u,x,1e-7);
   [cx,dx]=hecub(u,x,bx);
   % then fit van y as a function of u
   ay=y;
   [by]=grad7(u,y,1e-7);
   [cy,dy]=hecub(u,y,by);
   [xi, yi, kappa] = boogkrom(ax, bx, cx, dx, ay, by, cy, dy, u);

 %      disp('Booglengte - Parametric Hermite spline')
 %      disp('xr_pix = ')
 %      ind = find(yi<63.5);
 %      xr=interp1([yi(ind(1)) yi(ind(1)-1)], [xi(ind(1)) xi(ind(1)-1)], 63.5);
 %      disp(num2str(xr));
       
  
%%%%%%%%%%%%%%%%%  
elseif ftype == 11,     % Arclength Parametric Natural spline (also cubic)
   n = length(x);
   if n>=3, 
       u = cumsum(sqrt((x(2:n)-x(1:n-1)).^2+(y(2:n)-y(1:n-1)).^2));
       u = [0 u];
       % first fit van x as a function of u
       ax=x;
       [bx]=natcub(u,x);
       [cx,dx]=hecub(u,x,bx);
       % then fit van y as a function of u
       ay=y;
       [by]=natcub(u,y);
       [cy,dy]=hecub(u,y,by);
       
       [xi, yi, kappa] = boogkrom(ax, bx, cx, dx, ay, by, cy, dy, u);

%       disp('Booglengte - Parametric Natural spline')
%       disp('xr_pix = ')
%       ind = find(yi<63.5);
%       xr=interp1([yi(ind(1)) yi(ind(1)-1)], [xi(ind(1)) xi(ind(1)-1)], 63.5);
%       disp(num2str(xr));
   else
     seterror(' At least 3 points needed for a cubic spline fit !')
     return
   end
   

%%%%%%%%%%%%%%%%%
elseif ftype == 12           % Arclength - Parametric 3rd order polynome
   
   n = length(x);
   u = cumsum(sqrt((x(2:n)-x(1:n-1)).^2+(y(2:n)-y(1:n-1)).^2));
   u = [0 u];
   px = polyfit(u,x,3);
   py = polyfit(u,y,3);
   [xi, yi, kappa] = boogkrom(px(4), px(3), px(2), px(1), py(4), py(3), py(2), py(1),[0 u(n)]);   
  
%%%%   
else
  seterror(' BUG: Invalid Option in FIT.');
end


% OK handle plotting for ftype 3 to 12

figure(mainfig),
hold on,
plot(xi+0.5,yi+0.5,[colors(number),styles(number_style,:)]);


% Plot curvature 

if strcmp(get(SUBMENUHANDLES(1),'checked'),'on'),
	switch ftype

   case {3, 4 , 6},
     if y1 == 0,
       noemer=sqrt((1+x1.^2).^3);
       kappa=x2./noemer;
     else
       noemer=sqrt((1+y1.^2).^3);
       kappa=y2./noemer;
     end
     for i=1:length(xi)-1
        boog(i)=sqrt((xi(i+1)-xi(i))^2+(yi(i+1)-yi(i))^2);
     end
     booglengte = cumsum(boog);
     booglengte = [0 booglengte];
     curvplot(booglengte, kappa, [colors(number),styles(number_style,:)]);

   case 5,
      seterror(' Curvature calculation not implemented. Try using cubic splines.')  

   case {7 , 8 , 9 , 10 , 11, 12},
		for i=1:length(xi)-1
   		boog(i)=sqrt((xi(i+1)-xi(i))^2+(yi(i+1)-yi(i))^2);
		end
		booglengte=cumsum(boog);
		booglengte = [0 booglengte];
	   curvplot(booglengte, kappa, [colors(number),styles(number_style,:)]);

	otherwise
    	seterror('Illegal case in fit.m. This should never happen.');
     	return
   end
   
end