% est_affiner.m                                    Steve Mann, 1992-1994
%
% recursively estimates parameters of the affine motion group
%
% Use: p = est_affiner(E1,E2);                     %
% or : est_affiner(E1,E2);                         % nicely formatted screen
%
%      est_affiner(E1,E2,3);                       % does 3 itterations
%                                                   % default is 2 itterations
%
%      est_affiner(E1,E2,3,p0);                    % use starting p0
%
% Example: p=est_affiner(A,B,3);
%          p=est_affiner(A,B,3,p);                 % pick up where left off
%          p=est_affiner(A,B,3,p);                 % recursively, can repeat
%
% sign convention: x or m down, y or n to right
%
% properly handles situation when one or both images contain some NaN points
%
% [p, mse_hist]=est_affiner(A,B,3);  % gives mean square error (mse) plot data
%
% calls est_affine.m   (recursively)

this affine estimater does not give correct answer

function [p_out,mse_hist] = multi_parameter_function(E1,E2,K,p0);

if nargin==0
  error('est_affiner: you must supply some input images')
end%if

if nargin==1
  error('est_affiner: you must supply 2 (two) input images')
end%if

if nargin==2
  disp('est_affiner: number of itterations not specified; defaulting to 2')
  disp('              (est once, then interpolate once, and est residuals)')
  K=2;
end%if

if K<=0
  disp('est_affiner: number of itterations must be positive ...exiting...')
  return
end%if

[M, N]  = size(E1);
[M2,N2] = size(E2);
if (M2~=M) | (N2~=N)
  error(...
    sprintf('est_affiner: images must be same size; %gby%g and %gby%g differ',...
            M,N,M2,N2)...
       )
end%if

% amplitude used for display of error image on same fixed range as E1
amplitudeE1=max2nan(E1)-min2nan(E1);

% DISPLAY display figures in successively newly created windows
fig_n = gcf;
%if fig_n < 5  % limit maximum number of windows
figure(fig_n+1); % open new window
set(gcf,'Name',sprintf('  given %g iterations,...',K))
clg
subplot(221)
tvs(E1)
title('Reference frame: E1')
drawnow
subplot(222)
tvs(E2)
title('Comparison frame: E2(k=1)')
drawnow

disp1='est_affiner: ';
disp2='formatted state-variables at each itteration displayed below:';
disp([disp1 disp2])

if nargin <= 3 
  disp('est_affiner: estimating initial parameters from the two images:')
  p0 = est_affine(E1,E2); p0=p0([2 3 1 5 6 4]); p0=[p0;0;0];
  p1=p1.*[1;N/M;1/M; M/N;1;1/N; 0;0]; 
  disp(sprintf('est 1: %+10.6f %+10.6f %+10.6f    %+10.6f %+10.6f %+10.6f',...
               p0(1),p0(2),p0(3),p0(4),p0(5),p0(6)))
else% more than 3 input arguments; therefore p0 has been specified
  if min(size(p0)) ~= 1
    error('est_affiner: starting guess must be a vector (1 by 6 or 6 by 1)')
  end%if
  if length(p0) ~= 6
    error('est_affiner: starting guess must be a vector of length 6')
  end%if
  if K==1
    disp('est_affiner: you specified p0, and K=1, so this function')
    disp('             will do nothing other than display the images and')
    disp('             some diagnostic text')
  end%if
  disp1=sprintf('set 1:%+10.6f %+10.6f %+10.6f    %+10.6f %+10.6f %+10.6f',...
                p0(1),p0(2),p0(3),p0(4),p0(5),p0(6));
  disp([disp1])
end%if

p_comp=p0;  % initialize composite group parameters
keyboard
mse_hist = [];  % mean squared error history (for mse plot)
for k = 2:K  % don't need to dechirp first time (K=1)
  E2d = pchirp2(p_comp,E2,1,-1,NaN); % set chirp rates to zero
       %E2deaffined; bilinear interp; no antialias
  % DISPLAY display
  subplot(223)
  tvs(E2d)
  title1=sprintf('Deaffined E2 at itteration%g',k);
  xlabel('should converge to E1')
  title(title1)
  drawnow
  subplot(224)
  abserr=abs(E2d-E1)/amplitudeE1;   % will be on interval [0,1]
  tv(abserr*(length(colormap)-1))
  title(sprintf('|E1-E2(%g)|/a',k));
  xlabel('where a = |max2nan(E1)-min2nan(E1)|')
  sqrerr=(E2d-E1);
  mse=mean2nan(sqrerr.*sqrerr);
  mse_hist = [mse_hist mse];
  ylabel(sprintf('mse=%g',mse))
  drawnow

  p1 = est_affine(E1,E2d) %estimate residual parameters after desampling
  p1 = p1([2 3 1 5 6 4])  % wrong order convention
  p1 = [p1(:);0;0];
  p1=p1.*[1;N/M;1/M; M/N;1;1/N; 0;0]; 
  p_comp = pcompose(p1,p_comp)
	 % not Abelian, so not equal to p_comp \circ p1
	 % use pcompose with chirp rates set to zero
  disp1=sprintf('itt%2.0f: %+10.6f %+10.6f %+10.6f    %+10.6f %+10.6f %+10.6f',...
                k,p_comp(1),p_comp(2),p_comp(3),p_comp(4),p_comp(5),p_comp(6));
  disp([disp1])
  p0 = p1; % new initial parameters for next itteration
  set(gcf,'Name',sprintf('given %g iterations (done it %g),...',K,k))
end%for

if (nargout ~= 2) & (nargout ~= 1) & (nargout ~= 0)
  error(sprintf('est_affiner: nargout must be 0, 1, or 2, you had %g',nargout))
end%if

if nargout ~= 0
  p_out = p_comp;  % return composite result
end%if

if mse<500
  set(gcf,'Name',sprintf('  given %g iterations, found in SAME ``VIDEO ORBIT''''',K))
else%if
  set(gcf,'Name',sprintf('  given %g iterations, found in DIFFERENT ``VIDEO ORBIT''''',K))
end%if