foe(Vx, Vy)

% calculates focus of expansion (FOE)
%
%   input: optical flow field
%   output: FOE coordinates (cx,cy)
%
%   assumption: the camera moves forward and has only a translation in Z axis   

function [cx, cy] = foe(Vx, Vy)
  
  ofs = 10;
  
  % get sub image (using offsets at border)    
  subVx = Vx(ofs:end-ofs, ofs:end-ofs);
  subVy = Vy(ofs:end-ofs, ofs:end-ofs);
  
  % compute vertical and horizontal magnitudes     
  subVx = subVx.^2;
  subVy = subVy.^2;
  
  % find index of minimum sums for vertical and horizontal magnitudes  
  [v, cy] = min(sum(subVx'));  
  [v, cx] = min(sum(subVy));
  
  cy = cy + ofs;
  cx = cx + ofs;