function [v_list,A_list,I_patches,J_patches,error_image,a_normalize,b_normalize] = track_affine(u_list,I,J,Lm,wintx,winty,v_guess,A_guess,affine,normalize,verbiose,DEBUG,resolution,MaxIter);

% [v,A,I_patches,J_patches,error_image] = track_affine(u,I,J,Lm,wintx,winty,v_guess,A_guess,affine,normalize,verbiose,DEBUG,resolution,MaxIter)
%
% Affine or Translation tracking a point u on image I to the image J (at location v)
% using a pyramidal implementation of (Affine) Lucas-Kanade feature tracker.
% Works with a list of points.
%
%
% INPUT VARIABLES:
% 
% u: List on image locations to track (2xNp matrix)
% I: Original image where the points u are located (2D matrix or 3D x 3 for color)
% J: Next image where to track thepoints (B&W or color)
% Lm: Deepest level of the pyramid (default value=0 -> no pyramid)
% (wintx,winty): values to set the size of the integration window
%                (default value=10 for affine trcking, and 5 for translation tracking)
%					  Note: these could be (Lm+1)xNp vectors of values if you want
%                      to use different window sizes for each pyramid level for each feature.
%                      First row entry -> level 0, last row entry -> level Lm
% v_guess: initial guess of position of points on J (optional)
%          If  v_guess is input, all points with [NaN;NaN] entries in v_guess will not be tracked.
%          By default, v_guess will be set to u.
% A_guess: Initial guess of the affine matrix. List of 2x2 matrices. (optional)
%          By default, A_guess is set to the identity matrix (for each point)
% affine: set to 1 for affine tracking, and 0 for translation tracking (optional - default = 1)
%         if affine is a vector of 6 binary entries, then it controls the estimation of each affine
%         tracking variable independently: dx,dy,theta,s,a,b 
%         (dx,dy): translation
%         theta: rotation angle
%         s: skew
%         (a,b): x and y scales
%
%         example of affine vector: affine = [1 1 1 1 1 1]   -> estimate everything
%                                   affine = [1 1 0 0 0 0]   -> translation only
%                                   affine = [1 1 1 1 0 0]   -> affine rotation + skew (no scale)
%
%         Expression of the affine matrix: A = [cos(theta)  -sin(theta);sin(theta) cos(theta)] * [1 s;0 1] * [a 0;0 b]
%
%
% normalize: set to 1 to apply normalization on the image patches (to handle changes of brightness)
%            optional entry. Default value = 0
% verbiose: set to 1 (default) to have comments printed out, 0 to have no comments (optional)
% DEBUG: set to 1 to enter debug mode (optional entry. Default value = 0.
% resolution: pixel resolution to reach before end of iterations (optional, default = 0.01)
% MaxIter: Maximum number of iterations of Lucas-Kanade at each pyramid level (optional, default = 100)
%
%
% All optional entries could be left unused, or set to the empty element []
%
%
% OUTPUT VARIABLES:
%
% v: list of the tracked points (on the image J)
%    All points with [NaN;NaN] have not been tracked (lost points or points not processed).
% A: Final affine matrices (list of matrices) - A cell array of size 1 x Np
% I_patches: Feature patches on image I (a cell matrix of size 1xNp)
% J_patches: Feature patches on image J (a cell matrix of size 1xNp) - constructred after normalization
% error_image: Mismatch error (on the image plane): the standard deviation of sum of (I_patch - J_patch)
% a_normalize,b_normalize: affine brightness normalization between the two sets of features:
%                              J_normalized = a_normalize * J + b_normalize
%
%
% Convention of coordinates: the first row of u (and v) contains the x coordinates of
% the points. The second row consists of the y coordinates.
% By convention, the center of the upper-left corner pixel is at location (0,0).
% This may appear as being slightly different than standard Matlab convention,
% however, this should be identical to any C-code convention.
%
% The purpose of this code is to present a simple, and intuitive implementation
% of the Affine Pyramidal Lucas-Kanade.
%
%
% Syntax (examples):
% [v,A] = track_affine(u,I,J);                                  %-> Affine tracking
% [v,A,I_patches,J_patches] = track_affine(u,I,J,wintx,winty);  %-> Affine tracking
% [v,A] = track_affine(u,I,J,wintx,winty,[],[],0);              %-> Translation tracking
% [v,A] = track_affine(u,I,J,wintx,winty,[],[],1);              %-> Translation tracking + no initial guess
% [v,A] = track_affine(u,I,J,wintx,winty,[],[],[],0);           %-> Affine tracking + no comments
% [v,A] = track_affine(u,I,J,wintx,winty,vg,Ag);                %-> Affine tracking + initial guess
% [v,A] = track_affine(u,I,J,wintx,winty,vg,Ag,0);              %-> Translation tracking + initial guess
%
% Observe that all optional inputs can be set to the empty set [].



% Intel Corporation - Microprocessor Research Labs
% (c) Jean-Yves Bouguet - June 2000
% Updated August 2002 (added variable window sizes option for every feature)


pt_debug = 0; % point to display for debug mode.


%-------------- Check for errors -------------------

if nargin < 1,
   error('ERROR: Need feature coordinates to track (together with images I and J)');
end;

if nargin < 3,
   error('ERROR: Need images I and J to track');
end;

%-------------- Check of input variables -------------------

% Just in case the images are in uint8 format (images):
I = double(I);
J = double(J);

% If the images are in color, the matrices I and J are 3D matrices:
Nc = size(I,3); % number of colors (2,3,...)

% Height of the pyramid
if ~exist('Lm'),
   Lm = 0;	% no pyramid by default
else
   if isempty(Lm),
      Lm = 0;	% no pyramid by default
   end;
end;


%-- Number of points to track:

Np = size(u_list,2);

if ~exist('affine');
   affine = 1;
else
   if isempty('affine'),
      affine = 1;
   end;
end;


if length(affine) == 6;
   if prod(affine),
       affine = 1;
   else
       if ~sum(affine),
           affine = 0;
       end;
   end;
end;


if length(affine) == 6,
    % affine is a vector of 
    new_param = 1;
    ind_model = find(affine);
    Ho = [1 0  0  0 0 0;
        0 1  0  0 0 0;
        0 0  0  0 1 0;
        0 0 -1  1 0 0;
        0 0  1  0 0 0;
        0 0  0  0 0 1];
    weight_o = [1 1 1 1 1 1];
    %weight_o = [1 1 1 .2 1 1];
    Ho = Ho(:,ind_model);
    weight_o = weight_o(ind_model);
else
    new_param = 0;
end;


if ~exist('normalize'),
   normalize = 0;
else
   if isempty(normalize),
      normalize = 0;
   end;
end;

if ~exist('DEBUG'),
   DEBUG = 0;
else
   if isempty(DEBUG),
      DEBUG = 0;
   end;
end;

if ~exist('verbiose'),
   verbiose = 1;
else
   if isempty(verbiose),
      verbiose = 1;
   end;
end;

if verbiose,
    if sum(affine),
        fprintf(1,'Affine tracking ',Np);   
    else
        fprintf(1,'Translation tracking ',Np);
    end;
    if (Np > 1),
        fprintf(1,'of %d points ',Np);   
    else
        fprintf(1,'of one point ');
    end;
    if (Nc > 1),
        fprintf(1,'on all %d color channels ',Nc);   
    else
        fprintf(1,'on a single channel (black and white) ');
    end;
    if normalize,
        fprintf(1,'with normalization of brightness (mean value and standard deviation).\n');   
    else
        fprintf(1,'without normalization of brightness.\n');
    end;
    fprintf(1,'Number of pyramid levels: Lm+1=%d\n',Lm+1);
    if new_param,
        fprintf(1,'Affine tracking model used: position + rotation + skew + (x,y) scales:\n');
        fprintf(1,'Estimated variables: ');
        if affine(1),
            fprintf(1,'- x position -');
        end;
        if affine(2),
            fprintf(1,'- y position -');
        end;
        if affine(3),
            fprintf(1,'- rotation -');
        end;
        if affine(4),
            fprintf(1,'- skew -');
        end;
        if affine(5),
            fprintf(1,'- x scale -');
        end;
        if affine(6),
            fprintf(1,'- y scale -');
        end;
        fprintf(1,'\n');
    else
        if sum(affine),
            fprintf(1,'Affine tracking model used: The four entries of the affine matrix\n');
        end;
    end;
    
    if DEBUG,
        disp('Debug mode activated - To continue, type return.');
    end;
end;


% Initial direction constraint:

if exist('A_guess'),
   if isempty(A_guess),
      A_guess = cell(1,Np);
      for k = 1:Np,
         A_guess{1,k} = eye(2);
      end;
   else
      A_guess = A_guess(1,:);
   end;
else
   A_guess = cell(1,Np);
   for k = 1:Np,
      A_guess{1,k} = eye(2);
   end;
end;


% Initial guess for optical flow:

if exist('v_guess'),
   if isempty(v_guess),
      v_guess = u_list;
   end;
else
   v_guess = u_list;
end;


if affine,
   win_default = 10;
else
   win_default = 5;
end;


if ~exist('wintx'),
   wintx = win_default*ones(Lm+1,Np);
else
   if isempty(wintx),
      wintx = win_default*ones(Lm+1,Np);
   end;
end;

if ~exist('winty'),
   winty = wintx;
else
   if isempty(winty),
      winty = wintx;
   end;
end;

wintx = round(wintx);
winty = round(winty);

if size(wintx,1) > (Lm+1),
   wintx = wintx(1:Lm+1,:);
end;

if size(winty,1) > (Lm+1),
   winty = winty(1:Lm+1,:);
end;


if size(wintx,2) ~= Np,
    wintx = repmat(wintx(:,1),[1 Np]);
end;

if size(winty,2) ~= Np,
    winty = repmat(winty(:,1),[1 Np]);
end;

if size(wintx,1) ~= (Lm+1),
    wintx = repmat(wintx(1,:),[(Lm+1) 1]);
end;

if size(winty,1) ~= (Lm+1),
    winty = repmat(winty(1,:),[(Lm+1) 1]);
end;


wintx_list = wintx;
winty_list = winty;



%------- Some parameters:
if ~exist('MaxIter'),
   MaxIter = 100;
else
   if isempty(MaxIter),
      MaxIter = 100; % Maximum number of iterations on the Lucas-Kanade
   end;
end;
if ~exist('resolution'),
   resolution = 0.01;
else
   if isempty(MaxIter),
      resolution = 0.01;% threshold for stopping the iteration
   end;
end;




%----------- Build the pyramidal representations of I and J:
if verbiose,
   fprintf(1,'Building the two pyramid representations\n');
end;

I_pyramid = cell(1,Lm+1);
J_pyramid = cell(1,Lm+1);

I_pyramid{1} = I;
J_pyramid{1} = J;

for L = 1:Lm,
   IL = downsample(I_pyramid{L});
   JL = downsample(J_pyramid{L});
   I_pyramid{L+1} = IL;
   J_pyramid{L+1} = JL;
end;


% The final solution (initially, no point is tracked):

v_list = v_guess/2^(Lm+1);
A_list = A_guess;


% Image patches:
I_patches = cell(1,Np);
J_patches = cell(1,Np);
a_normalize = zeros(1,Np);
b_normalize = zeros(1,Np);
error_image = NaN*ones(1,Np);
Ix_patches = cell(1,Np);
Iy_patches = cell(1,Np);
delta_I_patches = cell(Np,1);

% initially, decide that the feature has not diverged, unless it does not exist

lost = isnan(u_list(1,:));   

% Initial guess for optical flow (at the (Lm+1) level, because it is multiplied by 2 right after):

lost = lost | isnan(v_list(1,:));
ind_not_lost = find(~lost);

L = Lm;


if verbiose,
   fprintf(1,'Start of tracking\n');
end;

% Start the main pyramid level loop:

while (L>=0) & ~isempty(ind_not_lost);
   
   if verbiose,
      fprintf(1,'Pyramid level %d...',L);
   end;
   
   
   % Location of the point u on image IL:
   uL_list = u_list / 2^L;
   
   
   % Initial guess for displacement at level L (from previous level L+1):
   v_list = 2*v_list;
   
   
   % The current images at level L:
   IL = I_pyramid{L+1};
   JL = J_pyramid{L+1};
   
   % Current size of the image:
   [ny,nx,Nc] = size(IL);
   
   
   % Check of divergence:
   lost = lost | (uL_list(1,:) < -0.5)|(uL_list(1,:) > (nx - 0.5))|(uL_list(2,:) < -0.5)|(uL_list(2,:) > (ny - 0.5));
   
   % Integer part of the point uL:
   uL_int_list = floor(uL_list);
   
   % Remainder:
   alpha_list = uL_list - uL_int_list;
   
   
   
   ind_not_lost = find(~lost);
   
   
   for i = ind_not_lost,
       
       % Select the window size:
       wintxL = wintx_list(L+1,i);
       wintyL = winty_list(L+1,i);
       
      %%%%%%%%% This whole section takes care of computing the image neighborhood
      %%%%%%%%% of the first image I^L around all the points (subpixel computation)
      %%%%%%%%% This includes the image itself and the x and y derivatives
      
      uL_int = uL_int_list(:,i);
      alpha = alpha_list(:,i);
      
      a = (1-alpha(1))*(1-alpha(2));
      b = alpha(1)*(1-alpha(2));
      c = (1-alpha(1))*alpha(2);
      d = alpha(1)*alpha(2);
      
      % Extract information about image, and derivatives:
      II = NaN*ones(2*wintyL+1,2*wintxL+1,Nc);
      Ix = NaN*ones(2*wintyL+1,2*wintxL+1,Nc);
      Iy = NaN*ones(2*wintyL+1,2*wintxL+1,Nc);
      
      % Extract of the part of the image that will be used to compute image interpolation:
      x_list = ones(2*wintyL+4,1) * (uL_int(1) - wintxL - 1 : uL_int(1) + wintxL + 2);
      y_list = (uL_int(2) - wintyL - 1 : uL_int(2) + wintyL + 2)' * ones(1,2*wintxL+4);
      ind_list = x_list * ny + y_list + 1;
      ind_good = find((x_list>=0) & (x_list<=(nx-1)) & (y_list>=0) & (y_list<=(ny-1)));
      
      
      for ii = 1:Nc,
         IIi = NaN*ones(2*wintyL+4,2*wintxL+4);
         ILi = IL(:,:,ii);
         IIi(ind_good) = ILi(ind_list(ind_good));
         
         % Extract the image patch using bilinear interpolation:
         
         IIi = a*IIi(1:2*wintyL+3,1:2*wintxL+3) + b*IIi(1:2*wintyL+3,2:2*wintxL+4) + ...
            c*IIi(2:2*wintyL+4,1:2*wintxL+3) + d*IIi(2:2*wintyL+4,2:2*wintxL+4);
         
         % Image derivatives (Sharr operator):
         
         Ixi = 1/32 * conv2(conv2(IIi,[1 0 -1],'valid'),[3 10 3]','valid');
         Iyi = 1/32 * conv2(conv2(IIi,[1 0 -1]','valid'),[3 10 3],'valid');
         
         % Effective image patch:
         
         IIi = IIi(2:2*wintyL+2,2:2*wintxL+2);
                  
         % Extract the color components:
         II(:,:,ii) = IIi;
         Ix(:,:,ii) = Ixi;
         Iy(:,:,ii) = Iyi;
      end;
         
      I_patches{1,i} = II;
      Ix_patches{1,i} = Ix;
      Iy_patches{1,i} = Iy;
      
      %%%%%%%%%%%%%%%%%% End of the section computing original image 
      %%%%%%%%%%%%%%%%%% and derivatives (the last part saves the matrices into the cell array)
      
   end;
   
   
   %%%%%%%%%%%%%%%%%
   %%%%%%%%%%%%%%%%% It is now time to go through the LK iterations 
   %%%%%%%%%%%%%%%%%
   
   % Start of the iterative Lucas-Kanade (at the level L):
   
   % Initial guess:
   converged = zeros(1,Np); % no feature has converged yet
   ind_not_converged = find(~lost & ~converged);
   
   % iterative index:
   k = 1;
   
   %if verbiose,
   %   fprintf(1,'Iteration ');
   %end;
   
   if (L < 0),
      converged = ones(1,Np);
      ind_not_converged = find(~lost & ~converged);
   end;
   

	%%%% Loop around as long as needed...

   while (k <= MaxIter) & ~isempty(ind_not_converged) & ~isempty(ind_not_lost),
      
         
      for i = ind_not_converged,
         
         %%%%%%%%%%%%%
         %%%%%%%%%%%%% This whole section warps the second image, and build the mismatch vector
         %%%%%%%%%%%%%
         
         
         % Select the window size:
         wintxL = wintx_list(L+1,i);
         wintyL = winty_list(L+1,i);
         
         % Weight mask:
         coeff_weight = 0; %0.1; %.7;
         mask_weight = ones(2*wintyL+1,2*wintxL+1); %   exp(-coeff_weight*((-wintyL:wintyL)'/(wintyL)).^2) * exp(-coeff_weight*((-wintxL:wintxL)/(wintxL)).^2);
         
         % This is the window:
         window_box = [wintxL -wintxL -wintxL wintxL ; wintyL wintyL -wintyL -wintyL];
         
         
         % The current estimate:
         Ai = A_list{1,i};
         vLi = v_list(:,i);
         
         
         
         lost(i) = (vLi(1) < -0.5)|(vLi(1) > (nx - .5))|(vLi(2) < -0.5)|(vLi(2) > (ny - 0.5));
                  
         % Extract of the part of the image that will be used to compute image interpolation:
         
         JJ = NaN*ones(2*wintyL+1,2*wintxL+1,Nc);
          
         [x_I,y_I] = meshgrid((-wintxL:wintxL),(-wintyL:wintyL));
         [pt_J] = Ai * [x_I(:) y_I(:)]' + vLi*ones(1,(2*wintxL + 1)*(2*wintyL + 1));
         
         xJ = floor(pt_J(1,:));
         yJ = floor(pt_J(2,:));
         
         ind_good = find((xJ>=0) & (xJ<=(nx-2)) & (yJ>=0) & (yJ<=(ny-2)));
         
         
         if length(ind_good) < 5,
            
            lost(i) = 1;
            
         else
            
            alpha_x = pt_J(1,ind_good) - xJ(ind_good);
            alpha_y = pt_J(2,ind_good) - yJ(ind_good);
            
            ind_nw = xJ(ind_good) * ny + yJ(ind_good) + 1;
            ind_ne = (xJ(ind_good) + 1) * ny + yJ(ind_good) + 1;
            ind_sw = xJ(ind_good) * ny + yJ(ind_good) + 2;
            ind_se = (xJ(ind_good) + 1) * ny + yJ(ind_good) + 2;
            
            a = (1-alpha_x).*(1-alpha_y);
            b = (1-alpha_x).*alpha_y;
            c = alpha_x.*(1-alpha_y);
            d = alpha_x.*alpha_y;
            
            for ii = 1:Nc,
               JJi = NaN*ones(2*wintyL+1,2*wintxL+1);
               JLi = JL(:,:,ii);
               JJi(ind_good) = a.*JLi(ind_nw) + b.*JLi(ind_sw) + c.*JLi(ind_ne) + d.*JLi(ind_se);
               JJ(:,:,ii) = JJi;
            end;
            
            % Image difference:
            
            II = I_patches{1,i};
            
            
            if normalize,
               % Image Brightness Normalization (recompute delta_I):
               ind_valid2 = find(~isnan(II) & ~isnan(JJ));
               Imean = mean(II(ind_valid2));
               Jmean = mean(JJ(ind_valid2));
               Itemp = II(ind_valid2) - Imean;
               Jtemp = JJ(ind_valid2) - Jmean;
               Isc = median(abs(Itemp)); %/0.6745;
               Jsc = median(abs(Jtemp)); %/0.6745;
               JJ = (Isc/Jsc)*(JJ - Jmean) + Imean;
               
               alpha_normalize = (Isc/Jsc);
               beta_normalize = Imean - alpha_normalize*Jmean;
            else
               alpha_normalize = 1;
               beta_normalize = 0;
            end;
            
            delta_I = II - JJ;            
            J_patches{1,i} = JJ;
            delta_I_patches{1,i} = delta_I;
            
            a_normalize(1,i) = alpha_normalize;
            b_normalize(1,i) = beta_normalize;
            
         end;
         
         
         %%%%%%%%%%%
         %%%%%%%%%%% This ends the image warping section
         %%%%%%%%%%%
         
     end;
     
     
     ind_not_converged = find(~lost & ~converged);
     ind_not_lost = find(~lost);
     
     
     %%%%%%%%% TRACKING PROCEDURE: %%%%%%%%%%%%%%%%   
     
     % INPUT: delta_I_patches, Ix_patches, Iy_patches, (uL_list)      
     
     
     
     for i = ind_not_converged, %lost,
         
         
         % Select the window size:
         wintxL = wintx_list(L+1,i);
         wintyL = winty_list(L+1,i);
         
         % Weight mask:
         coeff_weight = 0; %0.1; %.7;
         mask_weight = ones(2*wintyL+1,2*wintxL+1); %   exp(-coeff_weight*((-wintyL:wintyL)'/(wintyL)).^2) * exp(-coeff_weight*((-wintxL:wintxL)/(wintxL)).^2);
         
         % This is the window:
         window_box = [wintxL -wintxL -wintxL wintxL ; wintyL wintyL -wintyL -wintyL];
         
         
         II = I_patches{1,i};
         JJ = J_patches{1,i};
         Ix = Ix_patches{1,i};
         Iy = Iy_patches{1,i};
         delta_I = delta_I_patches{1,i};
         x = uL_list(1,i);
         y = uL_list(2,i);
         vLi = v_list(:,i);
         Ai = A_list{1,i};   
         
         
         % Index of the pixels that are within the image (valid entries):
         
         ind_valid = find(~isnan(delta_I(:,:,1)) & ~isnan(Ix(:,:,1)) & ~isnan(Iy(:,:,1)));
         
         
         % The matrix G, and vector b:
         G = [];
         b = [];
         
         if (new_param | affine(1)) & (length(ind_valid)>10),
            
            % Matrix of x and y coordinates:
            [x_mat,y_mat] = meshgrid((-wintxL:wintxL),(-wintyL:wintyL));
            
            x_mat = x_mat(ind_valid);
            y_mat = y_mat(ind_valid);
            
            % Weights at each pixel:
            weights = mask_weight(ind_valid);

            
            for ii = 1:Nc,
               
               Ixi = Ix(:,:,ii);
               Iyi = Iy(:,:,ii);
               delta_Ii = delta_I(:,:,ii);
               
               Ixi = Ixi(ind_valid);
               Iyi = Iyi(ind_valid);
                              
               Gi = [weights.*Ixi weights.*Iyi weights.*x_mat.*Ixi  weights.*y_mat.*Ixi weights.*x_mat.*Iyi  weights.*y_mat.*Iyi ];
               
               bi = weights.*delta_Ii(ind_valid);
               
               G = [G;Gi];
               b = [b;bi];
               
            end;
            
            if ~new_param,
                
                hi = inv(G'*G)*G'*b; 
                
                % Translation term:
                dv = [hi(1);hi(2)];
                
                % Affine term:
                Ai_res = [1+hi(3) hi(4) ; hi(5) 1+hi(6)];
                
            else
                
                % The new parameterization:
                
                hi = zeros(6,1);
                
                hi(ind_model) = diag(weight_o) * (inv(Ho' * (G'*G) * Ho) * Ho' * (G'*b));

                % Translation term:
                dv = [hi(1);hi(2)];

                theta_model = hi(3); % rotation
                s_model = hi(4); % skew
                a_model = hi(5); % scale in x
                b_model = hi(6); % scale in y
                
                % Affine term:
                Ai_res = [cos(theta_model)  -sin(theta_model);sin(theta_model) cos(theta_model)] * [1 s_model;0 1] * [1+a_model 0;0 1+b_model];


            end;
        
         else
             
             % Weights at each pixel:
            weights = mask_weight(ind_valid);
            
            for ii = 1:Nc,
               
               Ixi = Ix(:,:,ii);
               Iyi = Iy(:,:,ii);
               delta_Ii = delta_I(:,:,ii);
               
               Ixi = Ixi(ind_valid);
               Iyi = Iyi(ind_valid);
               
               Gi = [weights.*Ixi weights.*Iyi];
               
               bi = weights.*delta_Ii(ind_valid);
               
               G = [G;Gi];
               b = [b;bi];
               
            end;
            
            G2 = G'*G;
            b2 = G'*b;
            
            % Translation term:
            dv = [G2(2,2)*b2(1)-G2(1,2)*b2(2) ; G2(1,1)*b2(2)-G2(1,2)*b2(1)]/(G2(1,1)*G2(2,2) - G2(1,2)^2); 
            
            % Affine term:
            Ai_res = eye(2);
            
         end;
                 
         
         % Update of A and v:
         vLi = vLi + Ai*dv;
         Ai = Ai * Ai_res;
         
         
         % Save results (for next round):
         v_list(:,i) = vLi;
         A_list{1,i} = Ai;    
         
         
         % Check that the point has converged or not: The max displacement of the 4 corners of
         % the window of integration should be less that the resolution threshold!
         converged(1,i) = (max(sum((Ai_res-eye(2))*window_box + dv*ones(1,4)).^2) < resolution^2);
         
         %converged(1,i) = ((dv(1)^2 + dv(2)^2)<resolution^2);
         
         
         if converged(1,i),
            error_image(1,i) = sqrt(sum(delta_I(ind_valid).^2)/(length(ind_valid)-1));
         end;
         
         
         if DEBUG,
            if (k == 1),
                figure(8);
                subplot(Np,2,2*(i-1) + 1);
                image(uint8(II));
                hold on;
                plot(wintxL+1,wintyL+1,'r+');
                hold off;
                axis('image');
                axis off;
                axis tight;
            end;
            
            figure(8);
            subplot(Np,2,2*i);
            image(uint8(JJ));
            axis('image');
            hold on;
            plot([0 dv(1)]+wintxL+1,[0 dv(2)]+wintyL+1,'y-');
            plot(dv(1)+wintxL+1,dv(2)+wintyL+1,'r+');
            title(['L=' num2str(L) ', k=' num2str(k) ',i=' num2str(i)]);
            hold off;
            axis off;
            axis tight;
            
            drawnow;
                        
         end;
         
      end;
            
      
      ind_not_lost = find(~lost);
      ind_not_converged = find(~lost & ~converged);
      
      
      k = k + 1;
      
      
   end;	% of the k loop
   
   
   if verbiose,
       fprintf(1,' Number of iterations: %d',k-1);
       if (k > MaxIter),
           fprintf(1,'   (Maximum number of iterations reached)\n');
       else
           fprintf(1,'\n');
       end;
   end;
      
   
   % Test of maximum flow possibly computed:
   ind_not_lost = find(~lost);
   
   L = L - 1; % move to the next level
      
end; % of the L loop



%---- Do not return garbage results for the lost features:

ind_lost = find(lost);

for i = ind_lost,
   v_list(:,i) = NaN*ones(2,1);
   A_list{1,i} = NaN*ones(2,2);
   J_patches{1,i} = [];
   error_image(1,i) = NaN;
   delta_I_patches{1,i} = [];
   Ix_patches{1,i} = [];
   Iy_patches{1,i} = [];
end;





function V = downsample(U)

% Try the 5x5 filter for building the pyramid:
% Now works with color images!

U = double(U);

[r,c,k] = size(U);

p = floor((r+1)/2); % row
q = floor((c+1)/2); % col


U2 = zeros(r+2,c+2,k);

for i=1:k

	U2(:,:,i) = [U(:,:,i) U(:,end,i)*ones(1,2); ones(2,1)*U(end,:,i) U(end,end,i)*ones(2,2)];
   
end;

cp = 2*(0:(p-1))+1; % row
cq = 2*(0:(q-1))+1; % col


r2 = length(cp);
c2 = length(cq);

e = cq+1;
ee = cq+2;
w = cq-1; w(1) = 1;
ww = cq-2; ww(1) = 1; ww(2) = 1;
n = cp-1; n(1) = 1;
nn = cp-2; nn(1) = 1; nn(2) = 1;
s = cp+1;
ss = cp+2;

V = zeros(r2,c2,k);

for i = 1:k,
   
   V(:,:,i) = (36*U2(cp,cq,i) + 24*(U2(n,cq,i) + U2(s,cq,i) + U2(cp,e,i) + U2(cp,w,i)) + ...
      16 * (U2(n,e,i) + U2(s,e,i) + U2(n,w,i) + U2(s,w,i)) + ...
      6 * (U2(nn,cq,i) + U2(ss,cq,i) + U2(cp,ee,i) + U2(cp,ww,i)) + ...
      4 * (U2(n,ww,i) + U2(nn,w,i) + U2(n,ee,i) + U2(nn,e,i) + U2(s,ww,i) + U2(ss,w,i) + U2(s,ee,i) + U2(ss,e,i)) + ...
   	(U2(nn,ee,i) + U2(ss,ee,i) + U2(nn,ww,i) + U2(ss,ww,i)))/256;
   
end;



