scripttk.m

Recovering 3-D structure from motion in noisy 2-D images is a problem addressed by many vision syste

% scriptTK.m
close all;
clear all;

for z = 1:1
    switch z
    case 1,               
        imgSeq = 'hotel.seq';  
        seqNum = 1:1:10;
        imgType = 'png';
    case 2,
        imgSeq = 'ball.seq';
        seqNum = 1:1:10;
        imgType = 'png';
    case 3,
        imgSeq = 'hand.seq';
        seqNum = 1:1:10;
        imgType = 'png';
    end
    % resolution required for the track (in pixel)
    resolution = 0.03; % to 3/100ths of a pixel
    % selection window sizes (size = (2*winx+1)(2*wY+1))
    wX = 1; 
    wY = 1;
    % tracking window sizes
    wintx = 4; 
    winty = 4;
    % wintx = 8; 
    % winty = 8;
    % minimum spacing between two features (in pixels)
    spacing = 5;
    % pixels around the screen (selection)
    boundary = 5;
    % threshold for the selection
    thresh = 0.1;
    % upper level of the pyramid
    levelMax = 2;
    % threshold for rejection of a point
    rejection = 0.1;
    % Minimum feature storage
    N = 500;
    %-----------------------------------------------------------------
    fprintf(1,'\n');
    disp('Starting tracker...');
    fprintf(1,'\n');
    duration = length(seqNum);
    dFormat = sprintf('%%0%dd', 3);
    % create space for data collection
    tracked = zeros(N, duration);
    trackX = zeros(N, duration);
    trackY = zeros(N, duration);
    % process the first image
    index = sprintf(dFormat, seqNum(1));
    first = sprintf('%s%d.%s', imgSeq, seqNum(1), imgType);
    
    imgT = double(imread(first));
    %imgT = double(readpgm(first));
    if (size(imgT,3) ~= 1)
        imgT = rgb2gray(imgT);
    end
    
    imgI(:,:,1) = imgT;
    [nrow,ncol] = size(imgI(:,:,1));
    SampleSize(:,1) = [nrow; ncol];
    fprintf(1,'\n');
    disp(['Features selected on initial image "' first '"...']);
    trackC = featureSelection(imgI, wX, wY, thresh);
    nTrack = length(trackC) % # trackable features found
    if (nTrack < N)
        trackC = [trackC, zeros(2,N-nTrack)];
        featureA = [ones(nTrack,1); zeros(N - nTrack,1)];
    else
        trackC = trackC(:,1:N);
        featureA = ones(N,1);
    end
    % save the first image for use in dissimilarity calculations
    img1 = imgI(:,:,1);
    corners1 = trackC;
    % Build pyramid representation
    disp(['Original sample size is ' num2str(size(imgI,1)) ...
            'x' num2str(size(imgI,2))])
    for k=1:levelMax,
        % compute downsampled images
        imgT = downSamp(imgI(1:SampleSize(1,k),1:SampleSize(2,k),k));
        SampleSize(:,k+1) = [size(imgT,1);size(imgT,2)];
        disp(['Downsample #' num2str(k) ' to ' num2str(SampleSize(1,k+1))...
                'x' num2str(SampleSize(2,k+1))])
        imgI(1:SampleSize(1,k+1),1:SampleSize(2,k+1),k+1) = imgT;
    end;
    fprintf(1,'First image has %d features\n', length(find(featureA)));
    tracked(:,1) = featureA;
    trackX(:,1) = trackC(1,:)';
    trackY(:,1) = trackC(2,:)';
    h1 = figure(1);
    hold off, clf;
    image(imgI(:,:,1))
    colormap(gray(256))
    hold on
    coord = trackC(:,find(featureA));
    if (size(coord,1) > 0)
        plot(coord(2,:), coord(1,:), 'y.', coord(2,:), coord(1,:), 'b+');
    end
    hold off
    drawnow
    
    for i = 2:duration   
        seq = seqNum(i);    
        for j = 1:levelMax+1;
            imgBase(:,:,j) = imgI(:,:,j);
        end
        next = sprintf('%s%d.%s', imgSeq, seq, imgType);
        fprintf(1,'\nTracking image "%s"...\n', next);
        
        imgT = double(imread(next));
        %imgT = double(readpgm(next));  
        if (size(imgT,3) ~= 1)
            imgT = rgb2gray(imgT);
        end
        
        imgI(:,:,1) = imgT;    
        for k=1:levelMax,
            % compute downsampled images
            imgT = downSamp(imgI(1:SampleSize(1,k),1:SampleSize(2,k),k));
            SampleSize(:,k+1) = [size(imgT,1);size(imgT,2)];
            imgI(1:SampleSize(1,k+1),1:SampleSize(2,k+1),k+1) = imgT;
        end;    
        [cornersT, featureA] = pyramidLK(imgBase, imgI, SampleSize, trackC,...
            featureA,  N, levelMax, wintx, winty,...
            resolution, boundary, rejection);
        
        tracked(:,i) = featureA;
        trackX(:,i) = cornersT(1,:)';
        trackY(:,i) = cornersT(2,:)';
        trackC = cornersT;    
        fprintf(1,'Track #%d: %d features\n', (i-1), size(find(featureA),1));
        figure(h1)
        image(imgI(:,:,1))
        colormap(gray(256));  
        hold on
        coord = trackC(:,find(featureA));
        if (size(coord,1) > 0)
            plot(coord(2,:), coord(1,:), 'y.');
            % find the points that exist in the previous 2 frames
            ptsTracked = find(tracked(:,i-1) & tracked(:,i));
            if (length(ptsTracked) > 0)
                plot([trackY(ptsTracked, i-1),trackY(ptsTracked, i)]', ...
                    [trackX(ptsTracked, i-1),trackX(ptsTracked, i)]', 'g+');
            end
        end
        hold off
        drawnow    
    end
    
    W1 = [trackY(ptsTracked,:)';  trackX(ptsTracked,:)'];
    W = W1;
    h2 = figure(2);
    image(imgI(:,:,1))
    colormap(gray(256));  
    hold on
    
    plot( W(1:(size(W,1)/2),:), W( ((size(W,1)/2)+1):size(W,1),:), 'g+')
    hold off
    save stk.mat  W
    [R, T, S] = tomasiKanadeFactor(W);
    h3 = figure(3);
    plot3(S(1,:), S(2,:), S(3,:), '.')
    if (z == 2)
        h4 = figure(4);
        fill3(S(1,:), S(2,:), S(3,:),'r')
    end 
end % end z loop