% Relates the approximate parameters (q) to the exact
% parameters (p).
%
% 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 [p] = q_to_p(q, g)

% Find the dimensions of g
gx = size(g,2);
gy = size(g,1);

% Create matrix of original coordinates (x, y)
%OC = [ 0 0; 0 gy-1; gx-1 0; gx-1 gy-1 ];
OC = [0 0; 0 1; 1 0; 1 1];

% Once again, we have a linear equation to solve (NC = A*pvec)

% Build vector NC (new coordinates)
NC = zeros(8,1);
for m = 1:4
  x = OC(m,1);
  y = OC(m,2);
  % x' = q_x'xy*x*y + q_x'x*x + q_x'y*y + q_x'
  NC(2*m-1) = q(1)*x*y + q(2)*x + q(3)*y + q(4);
  % y' = q_y'xy*x*y + q_y'x*x + q_y'y*y + q_y'
  NC(2*m)   = q(5)*x*y + q(6)*x + q(7)*y + q(8);
end

% Build matrix A
A = zeros(8);
for m = 1:4
  x = OC(m,1);
  y = OC(m,2);
  xp = NC(2*m-1);
  yp = NC(2*m);
  A(2*m-1,:) = [x, y, 1, 0, 0, 0, -x*xp, -y*xp];
  A(2*m,:)   = [0, 0, 0, x, y, 1, -x*yp, -y*yp];
end

% Solve for pvec
pvec = A \ NC;

% Rearrange into the standard transformation matrix
% pvec = [a_x'x, a_x'y, b_x', a_y'x, a_y'y, b_y', c_x, c_y]';
p = [ pvec(1), pvec(2), pvec(3);
      pvec(4), pvec(5), pvec(6);
      pvec(7), pvec(8), 1        ];