% Finds the approximate bilinear projective model parameters (q) between
% two images, g and h, by solving eq. 19 in Mann and Picard.
%
% Model:   u = q1xy + q2x + q3y + q4
%          v = q5xy + q6x + q7y + q8
%
% q = [q_x'xy, q_x'x, q_x'y, q_x', q_y'xy, q_y'x, q_y'y, q_y']'

function [q] = est_q(g, h)

% Convert to doubles
g = double(g);
h = double(h);

% Get dimensions of image
[y,x] = size(g);

% Lose one in each dimension because of diff
y = y - 1;
x = x - 1;

% Find the spatial and temporal derivatives
% TODO: Could use differences between images as derivatives
%Ex = imfilter(double(g), fspecial('sobel')', 'replicate');
%Ey = imfilter(double(g), fspecial('sobel'), 'replicate');
%Et = double(h) - double(g);
Ex = (diff(g(1:y,:)')' + diff(h(1:y,:)')') / 2;
Ey = (diff(g(:,1:x)) + diff(h(:,1:x))) / 2; 
Et = h(1:y,1:x) - g(1:y,1:x);

% Replace NaN with 0
Ex = nanto(Ex,0);
Ey = nanto(Ey,0);
Et = nanto(Et,0);

% Build x and y index matrices
%gx = size(g,2);
%gy = size(g,1);
%X = repmat([0:gx-1],[gy 1]);
%Y = repmat([0:gy-1]',[1 gx]);
[y,x] = size(Ex);
[X,Y] = meshgrid(1:x,y:-1:1);

% Solve A*q = -B

% Build matrix A
A = zeros(8);
% Column 1
A(:,1) = [sum(sum((X.^2).*(Y.^2).*(Ex.^2)));
          sum(sum((X.^2).*(Y).*(Ex.^2)));
          sum(sum((X).*(Y.^2).*(Ex.^2)));
          sum(sum((X).*(Y).*(Ex.^2)));
          sum(sum((X.^2).*(Y.^2).*(Ex).*(Ey)));
          sum(sum((X.^2).*(Y).*(Ex).*(Ey)));
          sum(sum((X).*(Y.^2).*(Ex).*(Ey)));
          sum(sum((X).*(Y).*(Ex).*(Ey)))];
% Column 2
A(:,2) = [sum(sum((X.^2).*(Y).*(Ex.^2)));
          sum(sum((X.^2).*(Ex.^2)));
          sum(sum((X).*(Y).*(Ex.^2)));
          sum(sum((X).*(Ex.^2)));
          sum(sum((X.^2).*(Y).*(Ex).*(Ey)));
          sum(sum((X.^2).*(Ex).*(Ey)));
          sum(sum((X).*(Y).*(Ex).*(Ey)));
          sum(sum((X).*(Ex).*(Ey)))];
% Column 3
A(:,3) = [sum(sum((X).*(Y.^2).*(Ex.^2)));
          sum(sum((X).*(Y).*(Ex.^2)));
          sum(sum((Y.^2).*(Ex.^2)));
          sum(sum((Y).*(Ex.^2)));
          sum(sum((X).*(Y.^2).*(Ex).*(Ey)));
          sum(sum((X).*(Y).*(Ex).*(Ey)));
          sum(sum((Y.^2).*(Ex).*(Ey)));
          sum(sum((Y).*(Ex).*(Ey)))];
% Column 4
A(:,4) = [sum(sum((X).*(Y).*(Ex.^2)));
          sum(sum((X).*(Ex.^2)));
          sum(sum((Y).*(Ex.^2)));
          sum(sum((Ex.^2)));
          sum(sum((X).*(Y).*(Ex).*(Ey)));
          sum(sum((X).*(Ex).*(Ey)));
          sum(sum((Y).*(Ex).*(Ey)));
          sum(sum((Ex).*(Ey)))];
% Column 5
A(:,5) = [sum(sum((X.^2).*(Y.^2).*(Ex).*(Ey)));
          sum(sum((X.^2).*(Y).*(Ex).*(Ey)));
          sum(sum((X).*(Y.^2).*(Ex).*(Ey)));
          sum(sum((X).*(Y).*(Ex).*(Ey)));
          sum(sum((X.^2).*(Y.^2).*(Ey.^2)));
          sum(sum((X.^2).*(Y).*(Ey.^2)));
          sum(sum((X).*(Y.^2).*(Ey.^2)));
          sum(sum((X).*(Y).*(Ey.^2)))];
% Column 6
A(:,6) = [sum(sum((X.^2).*(Y).*(Ex).*(Ey)));
          sum(sum((X.^2).*(Ex).*(Ey)));
          sum(sum((X).*(Y).*(Ex).*(Ey)));
          sum(sum((X).*(Ex).*(Ey)));
          sum(sum((X.^2).*(Y).*(Ey.^2)));
          sum(sum((X.^2).*(Ey.^2)));
          sum(sum((X).*(Y).*(Ey.^2)));
          sum(sum((X).*(Ey.^2)))];
% Column 7
A(:,7) = [sum(sum((X).*(Y.^2).*(Ex).*(Ey)));
          sum(sum((X).*(Y).*(Ex).*(Ey)));
          sum(sum((Y.^2).*(Ex).*(Ey)));
          sum(sum((Y).*(Ex).*(Ey)));
          sum(sum((X).*(Y.^2).*(Ey.^2)));
          sum(sum((X).*(Y).*(Ey.^2)));
          sum(sum((Y.^2).*(Ey.^2)));
          sum(sum((Y).*(Ey.^2)))];
% Column 8
A(:,8) = [sum(sum((X).*(Y).*(Ex).*(Ey)));
          sum(sum((X).*(Ex).*(Ey)));
          sum(sum((Y).*(Ex).*(Ey)));
          sum(sum((Ex).*(Ey)));
          sum(sum((X).*(Y).*(Ey.^2)));
          sum(sum((X).*(Ey.^2)));
          sum(sum((Y).*(Ey.^2)));
          sum(sum((Ey.^2)))];
        
% Build vector B
B = [sum(sum((X).*(Y).*(Et).*(Ex)));
     sum(sum((X).*(Et).*(Ex)));
     sum(sum((Y).*(Et).*(Ex)));
     sum(sum((Et).*(Ex)));
     sum(sum((X).*(Y).*(Et).*(Ey)));
     sum(sum((X).*(Et).*(Ey)));
     sum(sum((Y).*(Et).*(Ey)));
     sum(sum((Et).*(Ey)))] * -1;
   
% Solve for q
q = A \ B;

% Compress into [0,1) dimensions
q = q + [0;1;0;0;0;0;1;0];
q = q .* [x;1;x/y;1/y;y;y/x;1;1/x];