%% Script of using homography/projection matrix for lab1 sequence
%
% This example shows how to map ground plane points in the world
% coordinates to pixel locations in each camera view.
%
% Author: Zheng Wu 
%         wuzheng1127@gmail.com
%         01/03/2014
%
%% Homography matrices for green(1st), orange(2nd) and red(3rd) camera views 
% The homography matrix maps ground plane points to image plane
% H_green = [0.8825, -0.7189, 207.5195; ...
%            0.0834, -0.0775, 246.9873; ...
%            0.0011, 0.0006, 0.6053];
H_orange = [0.1673, 0.7345, -140.1910; ...
            -0.0482, -0.0274, -159.3901; ...
            -0.0009, 0.0004, -0.5744]; 
% H_red = [0.2525, 1.0342, 123.2801; ...
%          -0.0452, -0.0351, 203.1472; ...
%          -0.0011, 0.0007, 0.4993];        
        
%% Convert to the image coordinates from world coordinates (ground plane)
% Define grid points on the ground plane. The unit is cm.
[X, Y] = meshgrid(-300:30:250,-180:30:220);
X = X(:)'; 
Y = Y(:)';
% Map to image coordinates
pts_orange = H_orange * ([X; Y; ones(size(X))]);
img_orange_X = pts_orange(1,:)./pts_orange(3,:);
img_orange_Y = pts_orange(2,:)./pts_orange(3,:);

%% Additional approximate projection matrices
% The projection matrix maps world points to image plane.
% You may consider applying it for localizing person's head (e.g, 1.8m above
% ground plane)
% P_green = [ -0.002786035695535, 0.002193727449735, 0.000098803810803, -0.643736400162840; ...
%             -0.000305778602280, 0.000156353189121, 0.003591413377346, -0.765228283068671; ...
%             -0.000003664210115, -0.000002156667213, 0.000001351972802, -0.001876561515848];
P_orange = [0.000820225351151, 0.003504147932828, 0.000422641202096, -0.660896201090138; ...
            -0.000189826452802, -0.000062657321570, 0.003691380622275, -0.750454594348728; ...
            -0.000004240132334, 0.000002315531878, 0.000001310485458, -0.002704899551688];
% P_red = [0.001043152007303, 0.004384657893969, -0.000719593652994, 0.517897852022788; ...
%          -0.000205879840610, -0.000141334218114, -0.004592420139080, 0.855415352164487; ...
%          -0.000004760367991, 0.000002975847716, -0.000001870323991, 0.002099101209336];
     
% Map to image coordinates: ground plane and plane at 180cm height
pts_orange1 = P_orange*[X;Y;zeros(1,numel(X));ones(1,numel(X))];
X1 = pts_orange1(1,:)./pts_orange1(3,:); 
Y1 = pts_orange1(2,:)./pts_orange1(3,:);

pts_orange2 = P_orange*[X;Y;180*ones(1,numel(X));ones(1,numel(X))];
X2 = pts_orange2(1,:)./pts_orange2(3,:); 
Y2 = pts_orange2(2,:)./pts_orange2(3,:);

% frame = imread('../orange/nuc/frame_00001.png');
% imagesc(imadjust(double(frame)/10000));
% axis image
% hold on
% 
% ind = find(X1>0&X1<512&Y1>0&Y1<512);
% plot(X1(ind), Y1(ind), 'xb');
% plot(X2(ind), Y2(ind), 'xr');
% for n = 1:numel(ind)
%     plot([X1(ind(n)),X2(ind(n))], [Y1(ind(n)),Y2(ind(n))], 'g-');
% end