% Comments:
%   much easier than camera calibration
%   just used a similar technique as A*m = 0 with the new point correspondences
%   originally had the computation of H as going from P -> Q, but H can be used
%   directly when in the form of Q -> P

clear all;
A = imread('Q3a.jpg');
%A = A(300:600,200:600);
B = zeros(size(A));
[rows cols] = size(A);

% these are the points used:
%   I tried to find pipes or other narrow lines which were easy to follow, I
%   also tried to pick a large area to reduce the human error component
%   P is the original image, Q is the transformed location
P = [302 2 1; 127 551 1; 692 557 1; 655 2 1];
Q = [302 2 1; 302 551 1; 692 551 1; 692 2 1];

% form a matrix X in the same style as A:
%   [ Qx Qy 1 0  0  0 -QxPx -QyPx -Px ]
%   [ 0  0  0 Qx Qy 1 -QxPy -QyPy -Py ]
%  for each correspondence P -> Q
%  then do SVD
X = zeros( length(P)*2, 9 );
for i = 1:length(P)
  X(2*i-1,:) = [ Q(i,:) 0 0 0 -Q(i,:)*P(i,1) ];
  X(2*i,:)   = [ 0 0 0 Q(i,:) -Q(i,:)*P(i,2) ];
end
[U S V] = svd(X);
H = zeros(3);
H(1,:) = V(1:3,9)';
H(2,:) = V(4:6,9)';
H(3,:) = V(7:9,9)';

% this is the homography
H = H / H(3,3)

% for i = 1:length(P)
%   z = H*Q(i,:)';
%   z = z/z(3)
% end
% return;

% for each pixel in the warped image, find out where it came from
% interpolate -- and just watch out for running off the ends of the image
for r = 1:rows
  for c = 1:cols
    p = H*[r c 1]';
    p = p / p(3);
    or = floor(p(1));
    oc = floor(p(2));
    dr = p(1)-or;
    dc = p(2)-oc;

    if (or < 1) or = 1; elseif (or >= rows) or = rows-1; end
    if (oc < 1) oc = 1; elseif (oc >= cols) oc = cols-1; end

    B(r,c) =  (1-dr)*(1-dc)*A(or,oc) + (dr)*(1-dc)*A(or+1,oc);
    B(r,c) += (1-dr)*(dc)*A(or,oc+1) + (dr)*(dc)*A(or+1,oc+1);
  end
end

imagesc(A,1)
imagesc(B,1)