%% function [dx,dy] = lucaskanade(I1,I2,xl,yt,xr,yb,wsize)
%% computes the optical flow over an image region (xl->xr, yt->yb) 
%% from I1 to I2 using a patch of size wsize by solving the Lucas Kanade
%% BCCE (brightness constant constraint equation)

function [lkdx,lkdy] = lucaskanade(I1,I2,xl,yt,xr,yb,wsize)

function W = getWindow(image,x,y)
   %% returns window of image centered at (x,y) of size
   %% (wsize)x(wsize)
   half = floor(wsize/2);
   W = image([y-half:y+half],[x-half:x+half]);
end;

function [bcce_dx,bcce_dy] = BCCE(x,y)
    patch_dx = getWindow(i1dx,x,y);
    patch_dy = getWindow(i1dy,x,y);
    patch_dt = getWindow(dI,x,y);

    ix2 = sum(sum(patch_dx.^2));
    iy2 = sum(sum(patch_dy.^2));
    ixiy = sum(sum(patch_dx.*patch_dy));
    A = [ix2 ixiy; ixiy iy2];
    
    ixit = sum(sum(patch_dx.*patch_dt));
    iyit = sum(sum(patch_dy.*patch_dt));
    b = -1 * [ixit; iyit];

    u = pinv(A)*b; % A\b;
    bcce_dx = u(1); bcce_dy = u(2);
end;

i1 = I1; %blurGauss(I1,2.0);   
[i1dx i1dy] = gradient(i1);
dI = blurGauss(I2,2.0)-blurGauss(I1,2.0);
lkdx = []; lkdy = [];

for xi=xl:xr,
  for yi=yt:yb,
    [dix,diy]= BCCE(xi,yi);
    lkdx(yi-yt+1,xi-xl+1)= dix;   lkdy(yi-yt+1,xi-xl+1)= diy;
  end;
end;

%% kill nans
lkdx(isnan(lkdx)) = 0;
lkdy(isnan(lkdy)) = 0;

end



