%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% CS223b HW# 1b: 
% Rohit Singh & Mitul Saha:  {rohitsi, mitul}@stanford.edu
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function lk()
% Lucas Kanade Tracker
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% function  [] =lk()
%
% HEADER FILE: This code requires an additional function file addTrackingROI.m
% Other files required are: ReadQTMovie.m,dgauss.m,gauss.m and MakeQTMovie.m 
%
% INPUT: This code reads images from the current directory with name in the format: 
% 'lbw%d.png'.
% Number of images read is initially set to 19. One can change these parameters in the  
% code by following the comments.
%
% REGION TO BE TRACKED: When the code is run it will ask the user to select an initial 
% window.
%
% IMPORTANT PARAMETERS: This code allows upto 'maxIterations'(set to 15; one can change 
% this number in the code) iterations.
% But if the change in [u v] over successive iterations is less than 'eps'(set to 1%) at 
% any time, it terminates before that. One can vary these parameters to achieve different
% SPEED and accuracy, but with a trade off.
%
% OUTPUT: Finally a movie 'lk.mov' is generated.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%




MakeQTMovie('start','lk.mov'); 
MakeQTMovie('quality',0.75); 
MakeQTMovie('size',[320 240]); 
MakeQTMovie('framerate',30); 

F = double(imread('1bw1.png'));

% let the user select the region of interest

imagesc(F); colormap(gray); axis image;
roi = double(roipoly);

% lets find the edges of the ROI so we can draw a nice tracking box
% at the final stage. our own canny function doesn't produce an image the same size as the
% input image. so use Matlab's edge detector
    
roiedges = double(edge(roi,'canny'));

% No. of frames scanned:

No_of_frames=19;

for i=2:No_of_frames
    
    sprintf('Iterating between frame %d and frame %d',i-1, i)
    
    % reading images of the following filename format:
    
    fname=sprintf('1bw%d.png',i);    
    G= double(imread(fname));

    Forig = F;
    Gorig = G;
  
    close all;

    sig=1.5;
    Gaus  = gauss(sig);
    dGaus = dgauss(sig);

    % not on whole image:
    
    inds=find(roi);

    count=0;
    maxIterations = 15; % maximum number of iterations
    eps = 0.01; % accuracty desired in terms of pixel-width
    
    u = 0; v = 0;  % F + [u v] == G
    
    % stopping criterion for the Newton-Raphson like iteration: 
    % either the frame shift is less than 'eps' of pixel width
    % or maxIterations have been done

    du=1;dv=1; %initialize to dummy values
    while ((abs(du) > eps | abs(dv) > eps) & count < maxIterations)
        
        count=count+1;
        
        % smoothen and take derivative
        Fx = conv2(F, dGaus'*Gaus, 'same');
        Fy = conv2(F, Gaus'*dGaus, 'same');
  
        C=[sum(Fx(inds).^2)         sum(Fx(inds).*Fy(inds)); 
           sum(Fx(inds).*Fy(inds))  sum(Fy(inds).^2)];
  
        Ft = G - F;
  
        D=[sum(Ft(inds).*Fx(inds)) ; sum(Ft(inds).*Fy(inds))];
        
        % checking the rank of C:
        
        if(rank(C) < 2)    break;   end
            
        uv = inv(C)*D;
        du = uv(1); dv = uv(2);
        
        u = u+du;
        v = v+dv;
  
        [rows, cols] = size(F);
        r = 1:rows;
        c = 1:cols;
 
        % warp F
        [X1, Y1] = meshgrid(c+dv, r + du);
  
        F_new= interp2(F,X1,Y1,'cubic');
    
        % set the remaining pixels to black.
        extrapixels = find(isnan(F_new));
        F_new(extrapixels) = 0;
  
        % err = norm( F_new(inds) - G(inds) );
        F = F_new;
    end;
    
    sprintf('No. of iterations: %d',count)
    [u v];
    
    % lets find the edges of the ROI so we can draw a nice tracking box
    % at the final stage. our own canny function doesn't produce an image the same size as the
    % input image. so use Matlab's edge detector

    Forig1 = addTrackingROI(roiedges,Forig);
    
    
    
    % new ROI is shifted...since we could have fractional values.
    % we make a compromise by rounding u and v....
    % make the new ROI
  
    offset = round(u) + round(v)*rows;
    ur = round(u); vr = round(v);
 
    % Finding and removing the parts of the ROI outside the image boudaries
    [r,c] =find(roi>0);
    roiinds = find(roi>0);
    badinds = find( r-ur<1 | r-ur>rows | c-vr<1 | c-vr>cols);
    roiinds(badinds) = [];
    
    % if the window vanishes then we quit:
    
    if(isempty(roiinds))    
        i=No_of_frames+1;
        count=maxIterations+1;
    end
    
        
    
    roiinds = roiinds - offset;

    % Finding and removing the parts of the ROI outside the image boudaries
    
    % l = roiinds;
    % badinds = find ( mod(l+,rows) < 1 | mod(l,rows) > 
    % badinds=find(roiinds < 1 | roiinds > rows*cols);
    % roiinds(badinds)=[];    
  
    roi = roi*0;
    roi(roiinds)=1;
    
    % rather than calling edge detector again on new roi, we just manipulate the old roiedges
    
    [r, c] = find(roiedges >0);
    roiedgesinds = find(roiedges>0);
    
    badinds = find(r-ur<1 | r-ur>rows | c-vr<1 | c-vr>cols);
    roiedgesinds(badinds) = [];
    
    roiedges = roiedges*0;
    roiedgesinds=roiedgesinds-offset;
    
    % Finding and removing the parts of the ROI outside the image boudaries
    
    % badinds=find(roiedgesinds < 1 | roiedgesinds > rows*cols);
    % roiedgesinds(badinds)=[];
    
    roiedges(roiedgesinds)=1; 
    
    
    
    G1     = addTrackingROI(roiedges,G);
   
    if(i==2)
        
    figure,imagesc(Forig1);axis image;
    str = sprintf('Frame# %d',i-1);
    title(str);
    MakeQTMovie('addframe'); 
    
end;
    
    figure,imagesc(G1);axis image;
    str = sprintf('Frame# %d',i);
    title(str);
    MakeQTMovie('addframe'); 
    
    % Comment this out if you dont want to close the windows
    close all;
    
    F=Gorig;

end;

MakeQTMovie('finish'); 

