%Longin Jan Latecki, latecki@temple.edu, October 2003
%this program computes optical flow in a color video using red and green color components

clear all
close all

%you can either load an input:
% load camera1_data_3D_norm.mat %the loaded matrix called 'data' is 4D
%it is in the folder   F:\CoOp\Pokie\TrajDim
% and has size = 36          48        2482           3


%or generate it as follows:
%defines a simple matrix with a moving block

noframes=20;
yimagesize=20;
% data=rand([5, yimagesize, noframes, 3]);
data=zeros([5, yimagesize, noframes, 3]);
data(:,[2 4 6 8 10 12 14 16 18 20],:,:)=1; %generates vertical stripes in the background
% data([2 4],:,:,2)=1;


for i=1:noframes
    data(3,i,i,1)=21;
    data(3,i,i,2)=11;
end


    
[s1 s2 s3 s4]=size(data); 

R=data(:,:,:,1);
G=data(:,:,:,2);
B=data(:,:,:,3);
count=1;
for x=2:s1-1
    for y=2:s2-1
        for t=2:s3
            
            A=[R(x+1,y,t)-R(x,y,t) R(x,y+1,t)-R(x,y,t) 
               G(x+1,y,t)-G(x,y,t) G(x,y+1,t)-G(x,y,t)];

            b=[R(x,y,t)-R(x,y,t-1) G(x,y,t)-G(x,y,t-1)]';
            b=-b;
            %Av=b implies v=A\b
            if rank(A) > 1
                mov=A\b;
                v(x,y,t,1)=mov(1);
                v(x,y,t,2)=mov(2);
                movalue(x,y,t)=norm(mov);
                count=count+1;
            else
                v(x,y,t,1)=0;
                v(x,y,t,2)=0;
                movalue(x,y,t)=0;
            end
        end
    end
end

count

% plays a movie with motion vectors overlayed
for frameCount = 2:noframes
        imagesc(data(:,:,frameCount,1)); colormap(gray(256));
        hold on
        quiver(v(:,:,frameCount,1),v(:,:,frameCount,2));
        hold off
        pause%(0.8);
    end

% plays the movie where the gray level shows the motion magnitude with motion vectors overlayed
figure;
for frameCount = 2:noframes
        imagesc(movalue(:,:,frameCount)); colormap(gray(256));
        hold on
        quiver(v(:,:,frameCount,1),v(:,:,frameCount,2));
        hold off
        pause%(0.8);
    end


    
%     for frameCount = 800:1000
%         imagesc(data(:,:,frameCount,1));
%         pause(0.3);
%     end
    
