% est_transrotr.m                                    Steve Mann, 1992
%
% recursive 2-D image group parameter est; adapted for Peleg: Steve Mann, 1993
%
% transrot (not transrot2 because the 2 is obvious; no such thing as 1D rot)
%
% estimates parameters of the rigid body motion group
%
% Use: [a,b] = est_transrotr(E1,E2);                 % complex trans; rot mag=1
% or : [rot,trans_m,trans_n] = est_transrotr(E1,E2)  % 3 real output arguments
% or : est_transrotr(E1,E2);                         % nicely formatted screen
%                                                    % output in radians + deg
%
%      est_transrotr(E1,E2,3);                       % does 3 itterations
%                                                    % default is 2 itterations
%
%      est_transrotr(E1,E2,3,a0,b0);                 % use starting resample a,b
%
% sign convention: real down, imag to right
%
% properly handles situation when one or both images contain some NaN points

function [p1, p2, p3] = multi_parameter_function(E1,E2,K,a0,b0);

if nargin==1
  disp('est_transrotr: you must supply 2 images')
  return
end%if

if nargin <3
  disp('est_transrotr: 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_transrot: number of itterations must be positive ...exiting...')
  return
end%if

if nargin == 4
  disp('est_transrotr: still have not decided what to do with 4 input ')
  disp('               parameters.  please specify a0 and b0 or neither')
  return
end%if

% 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
%else
%  disp1=sprintf('est_transrotr: you got %g figures already:re-using fig %g',...
%                fig_n, fig_n);
%  disp(disp1)
%  clg
%  figure(fig_n);
%end%if
clg
subplot(221)
tvs(E1)
title('Reference frame: E1')
drawnow
subplot(222)
tvs(E2)
title('Comparison frame: E2(k=1)')
drawnow

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

o=pi/180;
if nargin <= 3 
  disp('est_transrotr: (estimating initial parameters from the two images)')
  [b0,a0] = est_transrot(E1,E2);
  disp1=sprintf('beg 1:init angle(a)=%+7.4f= %+9.4fo; ',...
                angle(a0),angle(a0)/o');
  disp2=sprintf('init b=%+9.3f + %+9.3f*i',real(b0),imag(b0));
  disp([disp1 disp2])
else% more than 3 input arguments; therefore a0 and b0 have been specified
  if K==1
    disp('est_transrotr: you specified a0 and b0, 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:init angle(a)=%+7.4f= %+9.4fo; ',...
                angle(a0),angle(a0)/o');
  disp2=sprintf('init b=%+9.3f + %+9.3f*i',real(b0),imag(b0));
  disp([disp1 disp2])
end%if

a_comp=a0;  % initialize composite group parameters
b_comp=b0;

for k = 2:K  % don't need to desample first time (K=1)
%  E2d = detransrot(a_comp,E2,b_comp); %leaving out interp_order... prints default
%                           % good for following flow of algorithm
  E2d = detransrot(a_comp,E2,b_comp,1,-1,NaN);%E2desampled; bilinear; no antialias
  % DISPLAY display
  subplot(223)
  tvs(E2d)
  title1=sprintf('Desampled E2 at itteration%g',k);
  xlabel('should converge to E1')
  title(title1)
  drawnow
  subplot(224)
  tvs(abs(E2d-E1))
  title(sprintf('scaled |E1-E2(%g)|',k));
  drawnow

  [b1,a1] = est_transrot(E1,E2d); %estimate residual parameters after desampling
  %disp1=sprintf('est_transrotr: itteration%g: residual angle(a)=%g =%go;',...
  %              k,angle(a1),angle(a1)/o');
  %disp2=sprintf('             : residual b=%g + %g*i',real(b1),imag(b1));
  %disp(disp1)
  %disp(disp2)
  a_comp = a_comp*a1;
  b_comp = b_comp*a1+b1;
  disp1=sprintf('itt%2.0f:comp angle(a)=%+7.4f= %+9.4fo; ',...
                k,angle(a_comp),angle(a_comp)/o');
  disp2=sprintf('comp b=%+9.3f + %+9.3f*i',...
                real(b_comp),imag(b_comp));
  disp([disp1 disp2])
  a0 = a1; % new initial parameters for next itteration
  b0 = b1;
end%for

trans_m = real(b_comp);
trans_n = imag(b_comp);
rot     = angle(a_comp);

if nargout == 3
  p1 = rot;
  p2 = trans_m;
  p3 = trans_n;
end%if

if nargout == 2
  p1 = exp(i*rot);             % the ``a'' in ax+b; contains zoom=1 and rot
  p2 = trans_m + i*trans_n;    % the ``b'' in the ax+b group
end%if

if nargout == 1
  disp('only 1 output argument was given; still need to decide what to do here')
  disp('i have not decided what to do in the case of 1 output argument')
  disp('please use either 3,2, or 0 output arguments')
  return
end%if

if nargout == 0
  % disp('0 output arguments were given so I am displaying result to screen:')
  o = pi/180;
  disp(sprintf('rot=%g =%go;  trans_m=%g pels;  trans_n=%g pels',...
               rot, rot/o, trans_m, trans_n))
end%if