% Finds the approximate pseudo projective model parameters (q) between
% two images, g and h.  Adapted from video orbits code.
% Model:   u = a + bx + cy + gx^2 + hxy
%          v = d + ex + fy + gxy  + hy^2

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((Y.^2.*Ey+Y.*X.*Ex).^2));
          sum(sum((Y.^2.*Ey+Y.*X.*Ex).*Y.*Ey));
          sum(sum((Y.^2.*Ey+Y.*X.*Ex).*X.*Ey));
          sum(sum((Y.^2.*Ey+Y.*X.*Ex).*Ey));
          sum(sum((Y.^2.*Ey+Y.*X.*Ex).*(Y.*X.*Ey+X.^2.*Ex)));
          sum(sum((Y.^2.*Ey+Y.*X.*Ex).*Y.*Ex));
          sum(sum((Y.^2.*Ey+Y.*X.*Ex).*X.*Ex));
          sum(sum((Y.^2.*Ey+Y.*X.*Ex).*Ex))];
% Column 2
A(:,2) = [sum(sum(Y.*Ey.*(Y.^2.*Ey+Y.*X.*Ex)));
          sum(sum(Y.^2.*Ey.^2));
          sum(sum(Y.*X.*Ey.^2));
          sum(sum(Y.*Ey.^2));
          sum(sum(Y.*Ey.*(Y.*X.*Ey+X.^2.*Ex)));
          sum(sum(Y.^2.*Ey.*Ex));
          sum(sum(Y.*X.*Ey.*Ex));
          sum(sum(Y.*Ey.*Ex))];
% Column 3
A(:,3) = [sum(sum(X.*Ey.*(Y.^2.*Ey+Y.*X.*Ex)));
          sum(sum(X.*Y.*Ey.^2));
          sum(sum(X.^2.*Ey.^2));
          sum(sum(X.*Ey.^2));
          sum(sum(X.*Ey.*(Y.*X.*Ey+X.^2.*Ex)));
          sum(sum(X.*Y.*Ey.*Ex));
          sum(sum(X.^2.*Ey.*Ex));
          sum(sum(X.*Ey.*Ex))];
% Column 4
A(:,4) = [sum(sum(Ey.*(Y.^2.*Ey+Y.*X.*Ex)));
          sum(sum(Y.*Ey.^2));
          sum(sum(X.*Ey.^2));
          sum(sum(Ey.^2));
          sum(sum(Ey.*(Y.*X.*Ey+X.^2.*Ex)));
          sum(sum(Y.*Ey.*Ex));
          sum(sum(X.*Ey.*Ex));
          sum(sum(Ey.*Ex))];
% Column 5
A(:,5) = [sum(sum((Y.*X.*Ey+X.^2.*Ex).*(Y.^2.*Ey+Y.*X.*Ex)));
          sum(sum((Y.*X.*Ey+X.^2.*Ex).*Y.*Ey));
          sum(sum((Y.*X.*Ey+X.^2.*Ex).*X.*Ey));
          sum(sum((Y.*X.*Ey+X.^2.*Ex).*Ey));
          sum(sum((Y.*X.*Ey+X.^2.*Ex).*(Y.*X.*Ey+X.^2.*Ex)));
          sum(sum((Y.*X.*Ey+X.^2.*Ex).*Y.*Ex));
          sum(sum((Y.*X.*Ey+X.^2.*Ex).*X.*Ex));
          sum(sum((Y.*X.*Ey+X.^2.*Ex).*Ex))];
% Column 6
A(:,6) = [sum(sum(Y.*Ex.*(Y.^2.*Ey+Y.*X.*Ex)));
          sum(sum(Y.^2.*Ey.*Ex));
          sum(sum(Y.*X.*Ey.*Ex));
          sum(sum(Y.*Ey.*Ex));
          sum(sum(Y.*Ex.*(Y.*X.*Ey+X.^2.*Ex)));
          sum(sum(Y.^2.*Ex.^2));
          sum(sum(Y.*X.*Ex.^2));
          sum(sum(Y.*Ex.^2))];
% Column 7
A(:,7) = [sum(sum(X.*Ex.*(Y.^2.*Ey+Y.*X.*Ex)));
          sum(sum(X.*Y.*Ey.*Ex));
          sum(sum(X.^2.*Ey.*Ex));
          sum(sum(X.*Ey.*Ex));
          sum(sum(X.*Ex.*(Y.*X.*Ey+X.^2.*Ex)));
          sum(sum(X.*Y.*Ex.^2));
          sum(sum(X.^2.*Ex.^2));
          sum(sum(X.*Ex.^2))];
% Column 8
A(:,8) = [sum(sum(Ex.*(Y.^2.*Ey+Y.*X.*Ex)));
          sum(sum(Y.*Ey.*Ex));
          sum(sum(X.*Ey.*Ex));
          sum(sum(Ey.*Ex));
          sum(sum(Ex.*(Y.*X.*Ey+X.^2.*Ex)));
          sum(sum(Y.*Ex.^2));
          sum(sum(X.*Ex.^2));
          sum(sum(Ex.^2))];
        
% Build vector B
B = [sum(sum(Et.*(Y.^2.*Ey+Y.*X.*Ex)));  % g
     sum(sum(Et.*Y.*Ey));                % b
     sum(sum(Et.*X.*Ey));                % c
     sum(sum(Et.*Ey));                   % a
     sum(sum(Et.*(Y.*X.*Ey+X.^2.*Ex)));  % h
     sum(sum(Et.*Y.*Ex));                % e
     sum(sum(Et.*X.*Ex));                % f
     sum(sum(Et.*Ex))] * -1;             % d
   
% Solve for q
 q = A \ B;

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

% Rearrange into correct order
q = q([4 2 3 8 6 7 1 5]);