%% Script of using homography matrices for atrium 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 matrix for orange(1st) and red(2nd) camera views 
% The homography matrix maps ground plane points to image plane
H_orange = [-11.1419, -0.7598, -38.1841; ...
            0.0583, 3.9483, -227.1610; ...
            -0.0028, -0.0107, -0.5082];
H_red = [-9.2021, -5.3431, 9.2074; ...
        -0.1586, 3.7189,  -229.6984; ...
        0.0042,  -0.0106, -0.6047];
        
%% Convert to the image coordinates from world coordinates (ground plane)
% Define grid points on the ground plane. One unit is approximately 20.5cm
[X, Y] = meshgrid(0:28,-7:21);
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,:);

pts_red = H_red * ([X; Y; ones(size(X))]);
img_red_X = pts_red(1,:)./pts_red(3,:);
img_red_Y = pts_red(2,:)./pts_red(3,:);

%% Additional homography matrix at approximately 1.8m height
% The homography matrix maps plane points at 1.8m to image plane.
% You may consider applying it for localizing person's head
H_orange2 = [11.6427, 0.7568, 25.7323; ...
             -0.1563, -4.6163, 157.9280; ...
             0.0026, 0.0109, 0.4654];
H_red2 = [-10.0848, -5.4957, 37.4517; ...
          -0.5226, 4.4235, -154.8395; ...
          0.0035, -0.0107, -0.5526];
      
% Map to image coordinates
pts_orange2 = H_orange2 * ([X; Y; ones(size(X))]);
img_orange_X2 = pts_orange2(1,:)./pts_orange2(3,:);
img_orange_Y2 = pts_orange2(2,:)./pts_orange2(3,:);

pts_red2 = H_red2 * ([X; Y; ones(size(X))]);
img_red_X2 = pts_red2(1,:)./pts_red2(3,:);
img_red_Y2 = pts_red2(2,:)./pts_red2(3,:);
