%%% FILE: checkerAliasHandout.m
%%% DATE: Sept. 25, 2003
%%% Author: ADJ

%%% This file generates a point sampled image of an infinite
%%% checkerboard, illustrating oversampling and aliasing.
%%% The required modifications involve replacing the point sampling
%%% by averaging over multiple point samples which are randomly
%%% sampled from a normal distribution (thereby modelling the blur due
%%% to the camera's optics).

clear all;
close all;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%% Constants %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

checkSize = 30.0;  % Length of one side of checker
skyGray = 0.3*255; % Gray level for the sky.

%%% Intrinsic camera parameters
nIm = 128;    % Size of image in pixels
f = nIm/2;    % Focal length for +-45 degrees side to side field of view.
s1 = 2*f/nIm; % Size of pixel in x-direction
s2 = s1;      % Size of pixel in y-direction.
o1 = (nIm + 1)/2; % Piercing point of optical axis is in
o2 = o1;          % the middle of the image.
%% Matrix for Camera Centered Coords to Pixel Coords.
Mint = [1/s1 0 o1/f; 0 1/s2 o2/f; 0 0 1/f];

%% Compute a fixed nIm x nIm grid of pixel coordinates.
[p1,p2]=meshgrid(1:nIm,1:nIm);
coords=[p1(:)'; p2(:)'];

%%% Extrinsic camera parameters
d0 = [0; 0; 3*f];  % height of pinhole above checker board.
%%% Camera rotation matrix R
theta = 2/3 * pi;
ct = cos(theta);
st = sin(theta);
R1 = [1 0 0; 0 ct -st; 0 st ct];
theta = 0.05*pi;
ct = cos(theta);
st = sin(theta);
R2 = [ct 0 -st; 0 1 0; st 0 ct];
theta = pi/5;
ct = cos(theta);
st = sin(theta);
R3 = [ct -st 0; st ct 0; 0 0 1];
R = R1*R2*R3;

%% Translational direction of camera (parallel to camera's x-axis)
t = R'*[1; 0; 0];
xRange = (-1:0.1:1)*f;  % Set of displacements to acquire images at.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%% Parameter %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%% OPTICAL BLUR STANDARD DEVIATION
sigma = 2/pi;  % sigma for point spread function (DOES NOTHING YET)
% But after implementing the stochastic sampling, try 
% sigma = 0.2, 2/pi, or 2.

      
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%% Generate Image Sequence %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

mv = moviein(length(xRange)); %% Initialize a Matlab movie.

%% Loop over camera displacements and generate individual image frames
for k = 1:length(xRange)

  % Compute position of camera's nodal point in world frame.
  d = d0 + xRange(k)*t;

  % Compute the 3x4 external camera calibration matrix.
  Mext = [R -R*d];

  % Compute the homography matrix F.
  F = Mint*Mext;
  F = F(:,[1 2 4]);

  % Invert the homography matrix.
  iF = pinv(F);


  % Push the image coordinates through the inverse of the homography matrix, iF.
  Hw = iF * [coords; ones(1,size(coords,2))];
  % Now Hw = Hw(3) * (X_1, X_2, 1) where (X_1, X_2) are the intesection
  % points of the rays through the pixel locations. 
  % If Hw(3, :) <= 0 then the surface point is behind the camera.
  
  % Build an image using the coordinates Hw.
  im = zeros(1,size(Hw,2));
  
  % Normalize by 1/Hw(3,:)
  if any(Hw(3,:) == 0)
    Hw(3,Hw(3,:)==0) = -eps; % To avoid a divide by zero
  end
  Hw(1,:) = Hw(1,:)./Hw(3,:);
  Hw(2,:) = Hw(2,:)./Hw(3,:);
  %% Hw(1,:) and Hw(2,:) now equal X_1 and X_2.  Points behind the
  %% camera still need to be removed (using Hw(3,:) < 0).

  % Decide whether plane point (X_1, X_2) = (Hw_1, Hw_2) is white or black.
  a1 = floor(Hw(1,:)/checkSize);
  a2 = floor(Hw(2,:)/checkSize);
  a3 = rem(a1+a2,2);
  im(find(a3==0)) = 0;
  im(find(a3~=0)) = 255;

  % Set points on the sky to the sky gray-level.
  im(Hw(3,:) < 0) = skyGray;
  
  % Accumulate images
  im = reshape(im, [nIm, nIm]);

  %% Show final result for this frame.
  figure(1); clf; image(im);
  colormap(gray(256));
  resizeImageFig(figure(1), size(im), 2);
  
  %% Grab movie frame
  mv(:,k) = getframe;
  
end % camera displacement loop


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%% Display Image Sequence %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Show movie cycling forward and backward 10 times, at a rate up to 30Hz.
movie(mv, -10, 30);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%% END: checkerAliasHandout.m  %%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%