Multi-scale feature tracking by using optical flow

The homework I have asks me to use optical flow to track features from frame 1 to frame 2. Say I have get the features from frame 1 by using detectHarrisFeatures(I1). And I also have used Lucas Kanade to get the optical flow for the entire frame 1. How can I move the features from frame 1 to frame 2 by using this optical flow?

below is the function to get the optical flow between two pictures

function [u_f,v_f] = optical_flow_LK(I0, I1, windowSize, pyramid_level)

%% start the pytramid

for level = 1:pyramid_level

% set ratios for each layer pyramid downsize or upsize

up_ratio = 2.^(pyramid_level - level);

down_ratio = 1./(2.^(pyramid_level - level));

% the first layer, only downsize

I0_d = imresize(I0, down_ratio);% return the image I0_d that is down_ratio times the size of I0

I1_d = imresize(I1, down_ratio);

% for layer 2 and 3, (u,v) need to downsize, warp with image then upsize

if(level>=2)

% I0_d has been downsized already

u_r = imresize(u_tmp{level-1},size(I0_d)); % u_tmp{level-1} and v_tmp{level-1} are the flow field from previous layer

v_r = imresize(v_tmp{level-1},size(I1_d));

% return L-K

u_up_tmp = u_r * up_ratio;

v_up_tmp = v_r * up_ratio;

% meshgrid: returns 2-D grid coordinates based on the coordinates

% contained in vectors x and y

[X,Y] = meshgrid(1:size(I0_d,2),1:size(I0_d,1));

% apply the flow field to warp the first frame to second frame

I0_d = interp2(X,Y,I0_d,X-u_up_tmp,Y-v_up_tmp);

end

xd_tmp = zeros(size(I0_d));

yd_tmp = zeros(size(I0_d));

%set values for initial iteration and

iteration = 0;

con_factor = 10;

window_x_tmp = round(windowSize(1)/2)*(down_ratio);

window_y_tmp = round(windowSize(2)/2)*(down_ratio);

window_x = floor(window_x_tmp);

window_y = floor(window_y_tmp);

%% start iteration

while iteration < 10 & con_factor > 6 % iteration and con_factor can be changed

iteration = iteration + 1;

if(iteration < 2)

[dx_I0_d,dy_I0_d] = gradient(I0_d);

% get the difference from

diff_I = I0_d - I1_d;

end

if(iteration >= 2)

[X,Y] = meshgrid(1:size(I0_d,2),1:size(I0_d,1));

% Start to warp the image

I0_d_warp = interp2(X,Y,I0_d,X-xd_tmp,Y-yd_tmp); %interpolation for 2-D gridded data in meshgrid format

[dx_I0_d,dy_I0_d] = gradient(I0_d_warp);

diff_I = I0_d_warp - I1_d;

end

%% Apply Lucas Kanade

u = zeros(size(I0_d));

v = zeros(size(I0_d));

% in window ws * ws for each pixel

for i = window_x+1:size(dx_I0_d,1)-window_x %[dx_I0_d,dy_I0_d] = gradient(I0_d);

for j = window_y+1:size(dx_I0_d,2)-window_y

Ix = dx_I0_d(i-window_x:i+window_x, j-window_y:j+window_y);

Iy = dy_I0_d(i-window_x:i+window_x, j-window_y:j+window_y);

It = diff_I(i-window_x:i+window_x, j-window_y:j+window_y);

Ix = Ix(:);

Iy = Iy(:);

b = -It(:); % get b here

A = [Ix Iy]; % get A here

% get the optical flow at balid region where rank(G)=ATA=2

B=transpose(A); %get A transposed

G=B*A;

nu = pinv(A)*b; % the displacement or the velocity

if rank(G)==2

u(i,j)=nu(1);

v(i,j)=nu(2);

else

u(i,j)=0;

v(i,j)=0;

end

end

end

xd_tmp = xd_tmp + u;

yd_tmp = yd_tmp + v;

% the threshold of convergence

con_factor = max([max(max(u)) max(max(v))]);

end

% Save the temp (u,v) from each level

u_tmp{level} = -xd_tmp;

v_tmp{level} = -yd_tmp;

end

u_final=zeros(size(I0));

v_final=zeros(size(I0));

for level= 1:pyramid_level

up_ratio = 2.^(pyramid_level - level);

u_final=u_final + imresize(u_tmp{level},size(I0))*up_ratio;

v_final=v_final + imresize(v_tmp{level},size(I0))*up_ratio;

end

% downsize u and v, show every other 5

u_f = u_final(1:5:end, 1:5:end);

v_f = v_final(1:5:end, 1:5:end);

end