%% PLS-tracking
%% Multiple templates
%% Copyright (C)Qing Wang 2012
%% All rights reserved.

clear all;
close all;
clear;
clc;
iptsetpref('ImshowBorder', 'tight'); 

prefix = 'Sequences\car11\';
D = dir(prefix);
fNum = size(D,1)-4; 
%
prefix_result = 'result\';
% start frame
fstart = 1;  
tmpSZ = 32; 
% first frame
fname = sprintf('%s%04d.jpeg',prefix,fstart);  
%
im = imread(fname);
f_frame = sprintf('%d',1);
fprintf('tracking frame: # %s \n', f_frame); 

figure(1);
imshow(im);
frame = rgb2gray(im);
mapping=getmapping(8,'u2'); 
frame =  double(frame)/256;


hwin=gcf;
Current_title = sprintf('?%d?',fstart);
set(hwin, 'Name', Current_title);  
Current_title = sprintf('# %d',fstart);
text(5, 18, Current_title, 'Color','y', 'FontWeight','bold', 'FontSize',12);

%% locate the target object manually
%get 4 vertices of object
% fprintf('Please click 4 corners of the object to track:');
% [xbox,ybox] = ginput(4);  %user inputs corners of bounding box
% %get object bounding box
% xbox = round(xbox);   %get integer value
% ybox = round(ybox);
% left = min(xbox);    
% right = max(xbox); 
% top = min(ybox);     
% bottom = max(ybox);
% objBox = [left,right,top,bottom];      
% objCenterx = round((left+right)/2);
% objCentery = round((top+bottom)/2);
% width = right-left;
% height = bottom-top;
% %the intialized parameters
% p = [objCenterx, objCentery, width, height, 0];  %x, y, w, h, rotation, angle

%% locate the target object 
% location x, location y, width, height, rotation
p =  [89 140 30 25 0]; %IVT_car11


%%
% negative samples
alpha=0:pi/2000:2*pi;%[0,2*pi]
num = size(alpha,2);
R=sqrt(p(3)^2+p(4)^2)/2+sqrt(tmpSZ^2+tmpSZ^2)/2;
x=R*cos(alpha) + p(1)*ones(1,num);
y=R*sin(alpha) + p(2)*ones(1,num);
R2 = 1.5*R;
x2=R2*cos(alpha) + p(1)*ones(1,num);
y2=R2*sin(alpha) + p(2)*ones(1,num);


%% 
param0 = [p(1), p(2), p(3)/tmpSZ, p(5), p(4)/p(3), 0];
aTest = param0;
param0 = affparam2mat(param0);

bTest = affparam2geom(param0);

%% parameter setting
opt = struct('numsample', 600, 'condenssig',1, 'posNum',1, 'negNum',30, ...
    'tmplsize', [32,32], 'batchsize',15,'projNum',10, 'num_tmpl',5, 'r',1.5, 'posSig',[2,2,.01,.02,.002,.001]);

%% predicting standard covariance matrix (diagonal)
% center_x, center_y, width, height/width
opt.affsig = [5,5,.01,0, 0, 0];  %IVT_car11

step = 1; 
[posSample, posPosition] = SelectPos(frame, param0, opt);

for i = 1:opt.posNum
    figure(1);
    drawbox(opt.tmplsize, posPosition(:,i), 'Color','g', 'LineWidth',2.5);
end
%
mri = zeros(opt.tmplsize(1),opt.tmplsize(2),1,opt.posNum);
mri(:,:,1,:)=posSample(:,:,:);
figure(2);
montage(mri);

%% 
[negSample, position] = SelectNeg(frame, param0, opt);


%% 
figure(1);
drawbox(opt.tmplsize, param0, 'Color','r', 'LineWidth',2.5);

wname = sprintf('%s%04d.jpeg', prefix_result, fstart); %??????
imwrite(frame2im(getframe(gcf)),wname);

for i = 1:opt.negNum
    figure(1);
    drawbox(opt.tmplsize, position(:,i), 'Color','b', 'LineWidth',2.5);
end


%% 
tmp = PLS_sub(posSample, negSample, opt);
tmpl0 = tmp;
tmpl.mean = zeros(opt.tmplsize(1),opt.tmplsize(2),opt.num_tmpl);
tmpl.mean_pos = zeros(opt.tmplsize(1),opt.tmplsize(2),opt.num_tmpl);
tmpl.template = zeros(opt.projNum,opt.num_tmpl);
tmpl.W = zeros(opt.tmplsize(1)*opt.tmplsize(2), opt.projNum,opt.num_tmpl);
tmpl.sigma = zeros(opt.tmplsize(1)*opt.tmplsize(2),opt.num_tmpl);
tmpl.mean(:,:,1) = tmp.mean;
tmpl.mean_pos(:,:,1) = posSample(:,:,opt.posNum);
tmpl.template(:,1) = tmp.template;
tmpl.W(:,:,1) = tmp.W;
tmpl.sigma(:,1) = tmp.sigma;


param = [];
param.est =  param0'; 
paramRes = []; 
paramRes = [paramRes param.est];


%%
posSampleTotal = zeros(opt.tmplsize(1), opt.tmplsize(2),  opt.batchsize);
posSampleTotal(:,:,1) = posSample(:,:,opt.posNum);
mean = posSample(:,:,opt.posNum);
mean0 = mean;
wimgs = [];
f = 0.9; 


%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
                        %The main tracking algorithm
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for frameIndex = fstart+1:step:fNum
    frameIndex1 = mod(frameIndex,opt.batchsize);  
    % read the next frame
    fname = sprintf('%s%04d.jpeg',prefix,frameIndex);  

    wname = sprintf('%s%04d.jpeg', prefix_result, frameIndex); 
    im = imread(fname);
    figure(1);
    imshow(im);
    f_frame = sprintf('%d',frameIndex);
    fprintf('tracking frame: # %s \n', f_frame); 
    Current_title = sprintf('# %d',frameIndex);
    set(hwin, 'Name', Current_title);
    text(5, 18, Current_title, 'Color','y', 'FontWeight','bold', 'FontSize',12);    

    frame = rgb2gray(im); 
    frame =  double(frame)/256;
    
    param = est_condens_PLS_Multi(frame, tmpl, param, opt);% first stage
    tmp = estwarp_condens_PLS(frame, tmpl0, param, opt);   % second stage
    param.est = tmp.est;
    param.wimg = tmp.wimg;
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% update %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   
 
    tmp = tmpl.template;
    tmp(:,sum(abs(tmpl.template),1)==0) = [];
    num_tmpl = size(tmp,2);
    min_tmplate = tmpl.template(:,param.mini_t_idx);
    if param.mini_t_dis < sum(min_tmplate.^2)/5          
        
        tmpl.mean_pos(:,:,param.mini_t_idx) = tmpl.mean_pos(:,:,param.mini_t_idx)*f + (1-f)*param.wimg;
        [negSample,position] = SelectNeg(frame, param.est, opt);
        tmp_t = PLS_sub(tmpl.mean_pos(:,:,param.mini_t_idx), negSample, opt);
        tmpl.mean(:,:,param.mini_t_idx) = tmp_t.mean;
        tmpl.template(:,param.mini_t_idx) = tmp_t.template;
        tmpl.W(:,:,param.mini_t_idx) = tmp_t.W;
        tmpl.sigma(:,param.mini_t_idx) = tmp_t.sigma;
                    
    else  %add a new subspace and remove the subspace with largest reconstruction error
        if num_tmpl < opt.num_tmpl % if the number of subspace is less than the setting
            tmpl.mean_pos(:,:,num_tmpl+1) = param.wimg;
            [negSample,position] = SelectNeg(frame, param.est, opt);
            tmp_t = PLS_sub(param.wimg, negSample, opt);
            tmpl.mean(:,:,num_tmpl+1) = tmp_t.mean;
            tmpl.template(:,num_tmpl+1) = tmp_t.template;
            tmpl.W(:,:,num_tmpl+1) = tmp_t.W;
            tmpl.sigma(:,num_tmpl+1) = tmp_t.sigma;
            
        else            
            if param.max_t_idx ~= 1  
                tmpl.mean_pos(:,:,param.max_t_idx) = param.wimg;
                [negSample,position] = SelectNeg(frame, param.est, opt);
                tmp_t = PLS_sub(param.wimg, negSample, opt);
                tmpl.mean(:,:,param.max_t_idx) = tmp_t.mean;
                tmpl.template(:,param.max_t_idx) = tmp_t.template;
                tmpl.W(:,:,param.max_t_idx) = tmp_t.W;
                tmpl.sigma(:,param.max_t_idx) = tmp_t.sigma;
            end
        end       
    end

  
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% show the result %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
    paramRes = [paramRes param.est];
    % draw result    
    figure(1);
    drawbox(opt.tmplsize, param.est, 'Color','r', 'LineWidth',2.5);
    imwrite(frame2im(getframe(gcf)),wname);

end
paraSaveresult = sprintf('%s%s.mat',prefix_result,'result'); 
save(paraSaveresult, 'paramRes','opt');





