%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This programs detects and tracks feature points in a given set of registered  %
% images.  The points are identified using the Harris corner criteria, and are  %
% tracked via SSD measures.  Change detection is used to determine areas where  %
% movement has occured, and thus limit the selection of feature points.         %
%                                                                               %
% Authors: William Roberts, Leslie Watkins                                      %
% Date: June/July 2007                                                          %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


clear all

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Global variables
global winx winy saturation ...
    wintx winty spacing boundary boundary_t ...
    Nmax thresh ThreshQ hist_eq_flag...
    N_max_feat method obj_mask mask_name motion...
    img_filename number_list image_type build_mask;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Video/ Image Parameters
% All images must be in the format img_filename#.image_type where # denotes
% the image numbers referenced by number_list.
filename = 'img';
img_filename = ['..\Object Tracking\Images\' filename];
                % the portion of the image filname that all images in the
                % sequence have in common
image_type = 'jpeg';    %= 'bmp'; %= 'jpeg';
                % only applicable if input_type = 'image'
number_list = 2:50;
duration = length(number_list);
                % number of frames to be tracked
input_type = 'image';   %= 'qcif'; %= 'avi';
mask_name = 'Angelfire_road_seg\rmask.mat';%'Angelfire_road_seg\singlecarmask2.mat';%
                % binary mask that aids in feature selection
                % Save mask as variable 'y'.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
build_mask = 0;
                % Set to 1 to include the building mask 
hist_eq_flag = 0;
                % Set to 1 to equalize the histogram of the image.
interlaced = 0;
                % Set to 1 if images are interlaced (show alternating lines
                % of pixels in consecuitive fames).
bPlot = 1;
                % Set to 1 to get graphical output.
pause_at_plots = 0;
                % Set to 1 to pause after showing each frame.
reselect_feat_flag = 1;
                % Set to 1 to search for additional feature points
                % after every 5 frames.
obj_mask = 1;
                % Set to 1 to apply binary mask 'mask_name' during each
                % search for feature points.
method = 0;
                % Set to 1 to take saturation into consideration (used in
                % selection and tracking).
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Tracking/ Selection Parameters
winx = 1; winy = 1;
                % selection window size, the gradient of which is used to
                % determine whether or not a point is a feature point
                % (size = 2*winx+1, 2*winy+1)
saturation = 5;
                % saturation in Q for selection (win=1)
wintx = 5; winty = 5;
                % tracking window size
spacing = 8;
                % minimum spacing between 2 features (in pixels).
boundary = 10;
                % size of frame around the screen in which pixels are
                % rejected during selection
boundary_t = 1;
                % size of frame around the screen in which pixels are
                % rejected during tracking
thresh = 0.001;
                % threshold of selection
ThreshQ = 0.6;
                % threshold of tracking
Nmax = 500;
                % maximum selected features in image
N_max_feat = 500;
                % maximum space reserved for feature storage
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
next = 1;
                % used for iteration
good = zeros(N_max_feat,duration);
featx = zeros(N_max_feat,duration);
featy = zeros(N_max_feat,duration);
featq = zeros(N_max_feat,duration);
motion = zeros(2, N_max_feat);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Reads First Image
switch input_type
    case 'image'
        first = [img_filename num2str(number_list(1)) '.' image_type];
        tt = ReadImage(first,image_type,1200, 700);
                % tt is a matrix of intensity values of the image

    case 'avi'
        filename=[filename '.avi'];
        fmov = aviread(filename,number_list);
                % reads the AVI file
        tdata = fmov(1,1).cdata;
                % indexed
        tmap = fmov(1,1).colormap;
        tt3 = ind2rgb(tdata, tmap);
        tt = tt3(:,:,1);
        tt = double(tt);
        [nx,ny] = size(tt);

    case 'qcif'
        filename=[filename '.qcif'];
        frames=149;
        col=176;
        row=144;
        UV_ratio=2;
        UV_col=col/UV_ratio;
        UV_row=row/UV_ratio;
        Y=zeros(row,col,frames);
                % open file
        Y_size=col*row;
        U_size=UV_col*UV_row;
        V_size=UV_col*UV_row;
        buffer_size=Y_size+U_size+V_size;
        [fid,message]=fopen(filename,'r');
        for pp = 1:number_list(1)
             [raw_YUV,count]=fread(fid,buffer_size,'uint8');
             temp_Y_frame=reshape(uint8(raw_YUV(1:Y_size)),[col row])';
             temp_U_frame=reshape(uint8(raw_YUV(Y_size+1:Y_size+U_size)),[UV_col UV_row])';
             temp_V_frame=reshape(uint8(raw_YUV(Y_size+U_size+1:Y_size+U_size+V_size)),[UV_col UV_row])';
        end
        tt = temp_Y_frame;
        tt = double(tt);
        [nx,ny] = size(tt);
end

if interlaced
  Ipi = tt(1:2:nx,1:2:ny,1);
                % throw away even lines
else
  Ipi = tt;
end

clear tt;
    % tt is now saved as Ipi
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[nrow, ncol] = size(Ipi(:,:,1));
SampleSize(:,1) = [nrow;ncol];
                % Note that SampleSize is only computed once here
                % therefore all the images must have the same size;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
xtt = SelectFeature(Ipi);
                % 2 X # feature points matrix that give the x,y
                % coordinates of each feature point, sorted based on
                % quality of point

Nini = size(xtt,2); 			
                % initial # feature points
                
% makes the length of xtt and goodfeat = N_max_feat, even if there are
% more or less feature points than N_max_feat
if Nini < N_max_feat,
  xtt = [xtt,zeros(2,N_max_feat-Nini)];
  goodfeat = [ones(Nini,1);zeros(N_max_feat-Nini,1)];
else
  xtt = xtt(:,1:N_max_feat);
  goodfeat = ones(N_max_feat,1);
end;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

fprintf(1, 'On initial image: %d features\n', size(find(goodfeat),1));

Qtt = ComputeQuality(Ipi,xtt,goodfeat,wintx,winty);

aviobj = avifile('test_1.avi', 'colormap', colormap(gray(256)), 'fps', 15, 'compression', 'none');
                % writes avi file so the data can be reviewed later

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% plots first image
if bPlot,
  h = imshow(Ipi(:,:,1), [0,255]); colormap(gray(256));
  axis('equal'); axis([1 ncol 1 nrow]);
  hold on;
  xf = xtt(:,find(goodfeat));
  if size(xf,1) > 0,
      plot(xf(2,:),xf(1,:), 'r+');
  end;
  hold off; drawnow;

  F = getframe;
  aviobj = addframe(aviobj,F);
                % writes avi file so data can be reviewed later
end;

if pause_at_plots
    pause;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
feature_nums = zeros(1,duration);
feature_nums(1) = size(xtt,2);
                % # feature points in each image
prv_feat_num = 0;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
                
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
next = 1;
                % used for iteration
good = zeros(N_max_feat,duration);
                % keeps a record of goodfeat for each frame
featx = zeros(N_max_feat,duration);
                % keeps a record of the x-coordinate of each feature point
                % for each frame
featy = zeros(N_max_feat,duration);
                % keeps a record of the y-coordiante of each feature point
                % for each frame
featq = zeros(N_max_feat,duration);
                % keeps a record of the quality of each feature point for
                % each frame
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Main Tracking Loop
for nbri = 2:duration
    Ii = Ipi;
                % Ii saves previous image
    Qt = Qtt;
                %Qt saves the previous qualities
    xt = xtt;
                %xt saves the previous features
    next = next + 1;
                % read the next image

    switch input_type
        case 'image'
            next_name = [img_filename num2str(number_list(next)) '.' image_type];
            [tt,x] = ReadImage(next_name, image_type);
            
        case 'avi'
            tdata = fmov(1,next).cdata;
            tmap = fmov(1,next).colormap;
            tt3 = ind2rgb(tdata, tmap);
            tt = tt3(:,:,1);
            tt = double(tt);
            
        case 'qcif'
            [raw_YUV,count]=fread(fid,buffer_size,'uint8');
            temp_Y_frame=reshape(uint8(raw_YUV(1:Y_size)),[col row])';
            temp_U_frame=reshape(uint8(raw_YUV(Y_size+1:Y_size+U_size)),[UV_col UV_row])';
            temp_V_frame=reshape(uint8(raw_YUV(Y_size+U_size+1:Y_size+U_size+V_size)),[UV_col UV_row])';
            tt = temp_Y_frame;
            tt = double(tt);
    end
    
    if interlaced
        Ipi = tt(1:2:nx,1:2:ny,1);
    else
        Ipi = tt;
    end;
    
    clear tt
    
    [xttnew,Qtt,goodfeat] = track(Ii,Ipi,SampleSize,xtt,Qt,goodfeat, nbri, prv_feat_num);
                % tracks feature points
    prv_feat_num = length(find(goodfeat>0));
    
    % reselects additional feature points up to N_max_feat every fifth frame
    if reselect_feat_flag
        if mod(nbri, 5)==0
            index = find(goodfeat>0); 
            motion = motion(:, index');
            temp = zeros(2,(Nmax-size(index, 2)));

            motion = [motion temp];
            indices = nbri:(nbri+2);
            [xttnew, goodfeat, Qtt] = SelectFeature2(Ipi(:,:,1), xttnew, Qtt, goodfeat, indices);
        end
    end
    feature_nums(nbri)=size(find(goodfeat),1);
    good(:,nbri) = goodfeat;
    featx(:,nbri) = xttnew(1,:)';
    featy(:,nbri) = xttnew(2,:)';
    featq(:,nbri) = Qtt;
    xtt = xttnew;
                % updates existence, location, and quality of feature points
    
     fprintf(1, 'After track #%d: %d features\n',nbri-1, size(find(goodfeat),1));
     
    % plots images
    if bPlot
        figure(1);
        h = imshow(Ipi(:,:,1),[0,255]); colormap(gray(256));
        axis('equal'); axis([1 ncol 1 nrow]);
        hold on;
        xf = xtt(:,find(goodfeat));
        if size(xf,1) > 0
            plot(xf(2,:),xf(1,:), 'r+');
            ind_tracked = find(good(:,nbri-1) & good(:,nbri));
        end
        hold off; drawnow;
        
        if pause_at_plots
            pause;
        end

    F = getframe;
    aviobj = addframe(aviobj,F);
    
    end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

aviobj = close(aviobj);
                % closes avi after it is created
                
save('feature_nums.mat', 'feature_nums');
                % makes a record of the number of feature points detected
                % after each track
return;
                % remove this if you want to save variables good, featx,
                % featy, and featq
save result good featx featy featq;

     
     