% Computes the projective transformation p between two images
% g and h using the method of Mann and Picard.  nlvl specifies
% the number of pyramid levels to use and niter specifies the
% number of estimation iterations for each level.

function [p] = proj_reg(g, h, nlvl, niter, p, stoplvl)

% Give me double!
g = double(g);
h = double(h);

% Check that dimensions are the same
if (size(g) ~= size(h))
  error('Images have different dimensions!');
end

if (nargin < 6)
  stoplvl = 2; % Skip last level
end

% Create Gaussian pyramids for g and h
gpyr = make_gpyr(g, nlvl);
hpyr = make_gpyr(h, nlvl);

% Initialize p to the identity matrix
if (nargin < 5)
  p = eye(3);
end

max_lvl = floor(min(log2(size(g,1)/16),log2(size(g,2)/16)));
if (nlvl > max_lvl)
  nlvl = max_lvl;
end

% Starting from the lowest res level, apply the current p
% to h from that level, iterate to refine p, then move up
% to the next level.
for lvl = nlvl:-1:stoplvl
  % Pull out current g for readability
  curg = gpyr{lvl};
  % Iterate to refine p
  for m = 1:niter
    % Apply the current p to update curh
    % imtransform enjoys setting its own conventions...
    curh = imtransform(hpyr{lvl}, maketform('projective', p.'), ...
             'bicubic', 'size', size(hpyr{lvl}), ...
             'udata', [0 1], 'vdata', [0 1], ...
             'xdata', [0 1], 'ydata', [0 1], ...
             'fillvalues', -1);
    % Need NaN, but imtransform doesn't like that.  Replace -1 with NaN.
    curh(curh==-1) = NaN;
    % Show changes
    %figure, subplot 121, imshow(curg,[]), subplot 122, imshow(curh,[]);
    % Use approximate projective model to find q
    q = est_q_pseudo(curg, curh);
    % Relate q to p
    delta_p = q_to_p_pseudo(q, curg);
    % delta_p maps g to h, invert to map h to g
    delta_p = inv(delta_p);
    % Renormalize
    delta_p = delta_p / delta_p(3,3);
    % Compose delta_p with p
    p = delta_p * p;
    % Renormalize
    p = p / p(3,3);
  end
end