pyramidlk.m

Recovering 3-D structure from motion in noisy 2-D images is a problem addressed by many vision syste

function  [cornersT, goodFeat] = ...
    pyramidLK(imgBase, newImg, sampleSize, corners, goodFeat, maxFeat,...
	       maxLevel, wX, wY, subPixRes, bBox, thresh)
% Purpose: Let u be a point on image I. Find its corresponding location v on image J. 
%          Implementation of the psuedo code from subsection 2.4 Summary of the pyramidal 
%          tracking algorithm in "Pyramidal Implementation of the Lucas Kanade Feature 
%          Tracker Description of the algorithm", Jean-Yves Bouguet, Intel Corporation
% input:
%        imgBase: pyramid image I
%        newImg: pyramid image J
%        sampleSize: vector of subsample sizes
%        corners: locations of trackable points in image I
%        goodFeat: locations of corners in imageBase
%        maxFeat: max number of features you want to track
%        maxLevel: maximum number of pyramid levels in the input images
%        wX: tracking window size X-direction
%        wY: tracking window size Y-direction
%        subPixRes: sub-pixel resolution accuracy threshold
%        bBox: pixels around the screen
%        thresh: 
%       threshold: threshold ratio multiplier for the selection of pixels greater 
%                  than the threshold*(maximum of the minimum eigenvalues for eaach pixel)
% output:
%        cornersT: The trackable points selected by the geature selector.
%        goodFeat: locations of the corners tracked
% history:
%         05/01/04 SLB initial revision
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% local initialization
trackingF = length(find(goodFeat));
pointLoc = zeros(maxFeat,1);
goodFeatureTmp = goodFeat;
subIndex = 0;
maxCorner = 0;  
level = 0;
dAffine = zeros(2,maxFeat);
trackable = zeros(maxFeat,1);

% Build pyramid representations of I and J using levels 0->Lm
while ( (level <= maxLevel) & trackingF)
  
  % optical flow vector d
  d = zeros(2, maxFeat);
  % residuals
  residuals = zeros(maxFeat,1);
  % divergent points (i.e. feature points lost during tracking)
  divergentPts = zeros(maxFeat,1);
  % ratio of image difference per original image
  ratios = zeros(maxFeat,1);
  
  
  % Loop thru the image pyramid levels Lm downto 0
  for L = maxLevel:-1:0
    
    % get the (row.col) widths for the current subsample
    subR = sampleSize(1,L+1);
    subC = sampleSize(2,L+1);
    
    % initialize STK displacement guess Vk
    Vk = zeros(2, maxFeat);
    
    % corners detected in the first image subsample
    subCorner = (corners - 1)/(2^L) + 1;
    d = 2*d; %scale displacement by 2X
    
    % look at each of the corners individually
    for i = 1:maxFeat
      
      % only at the trackable corners
      if (goodFeat(i) & (~divergentPts(i)))
	
	% compute image brightness at subpixel locations
	% using bilinear interpolation coordinates of the
	% point in the first image subsample
	subCX = subCorner(1,i);
	subCY = subCorner(2,i);
	
	% location of the integer part of the point
	subCXI = round(subCX);
	subCYI = round(subCY);
	
	% if the image point is within the bounding box for filtering
	if ((subCXI >=  wX + bBox + 3) & ...
	    (subCXI <= subR - wX - bBox - 2) & ...
	    (subCYI >=  wY + bBox + 3) & ...
	    (subCYI <= subC - wY - bBox - 2))
	  
	  % separate the subpixel alpha's out
	  alphaX = subCX - subCXI;
	  alphaY = subCY - subCYI;
	  
	  if (alphaX > 0)
	    maskX = [alphaX 1-alphaX 0]';
	  else
	    maskX = [0 1+alphaX -alphaX]';
	  end
	  if (alphaY > 0)
	    maskY = [alphaY 1-alphaY 0];
	  else
	    maskY = [0 1+alphaY -alphaY];
	  end
	  
	  intensityI = imgBase(subCXI-wX-2:subCXI+wX+2,...
			       subCYI-wY-2:subCYI+wY+2,L+1);
	  % Extract the image patch using bilinear interpolation
	  intensityI = conv2(conv2(intensityI,maskX,'same'),maskY, 'same');
	  % remove outer edge to reduce noise
	  intensityI = intensityI(2:2*wX+4,2:2*wY+4);
	  % Derivative of IL with respect to x,y
	  [Iy, Ix] = gradient(intensityI);
	  Ix = Ix(2:2*wX+2,2:2*wY+2);
	  Iy = Iy(2:2*wX+2,2:2*wY+2);
	  
	  % Calculate the spatial gradient matrix
	  a = sum(sum(Ix.*Ix));
	  b = sum(sum(Ix.*Iy));
	  c = sum(sum(Iy.*Iy));
	  determinant = a*c - b^2;
	  G = (1/determinant) .* [a b; b c];
	  
	  intensityI = intensityI(2:2*wX+2,2:2*wY+2);
	  	  
% 	  intensityI = imgBase(subCXI-wX-2:subCXI+wX+2,...
% 	                        subCYI-wY-2:subCYI+wY+2,L+1);
% 	  aa = (1-alphaX)*(1-alphaY);
% 	  bb = alphaX*(1-alphaY);
% 	  cc = (1-alphaX)*alphaY;
% 	  dd = alphaX*alphaY;
% 	  % Extract the image patch using bilinear interpolation:                    
% 	  IntensityI = aa*intensityI(1:2*wX+3,1:2*wY+3) + bb*intensityI(2:2*wX+4,1:2*wY+3) + ...
% 	      cc*intensityI(1:2*wX+3,2:2*wY+4) + dd*intensityI(2:2*wX+4,2:2*wY+4);                   
% 	  % Image derivatives (Sharr operator):                   
% 	  Ix = 1/32 * conv2(conv2(intensityI,[1 0 -1],'valid'),[3 10 3]','valid');
% 	  Iy = 1/32 * conv2(conv2(intensityI,[1 0 -1]','valid'),[3 10 3],'valid');
%  	  Ix = Ix(2:2*wX+2,2:2*wY+2);
%  	  Iy = Iy(2:2*wX+2,2:2*wY+2);
% 	  %                     
% 	  a = sum(sum(Ix.*Ix));
% 	  b = sum(sum(Ix.*Iy));
% 	  c = sum(sum(Iy.*Iy));
% 	  intensityI = intensityI(2:2*wX+2,2:2*wY+2);
% 	  determinant = a*c - b^2;
	  
	  % Initialization of iterative L-K:    
	  Vk(:,i) = [0 0]';
	  naidaK = 1 + subPixRes;
	  diverge = 0;
	  if (L == 0)
	    K = 1;
	  else
	    K = 15;
	  end
	  k = 0;	  
	  
	  % for k = 1 to K with step of 1...
	  % (or until ||Nk|| < accuracy threshold)
	  while ( (k < K) & (norm(naidaK) > subPixRes) & (~diverge))
	    
	    % Image difference                        
	    subCJX = subCX + d(1,i) + Vk(1,i);
	    subCJY = subCY + d(2,i) + Vk(2,i);
	    subCJXI = round(subCJX);
	    subCJYI = round(subCJY);
	    
	    if ((subCJXI >=  wX + bBox + 2) & ...
		(subCJXI <= subR - wX - bBox - 1) & ...
		(subCJYI >=  wY + bBox + 2) & ...
		(subCJYI <= subC - wY - bBox - 1))
	      
	      alphaXJ = subCJX - subCJXI;
	      alphaYJ = subCJY - subCJYI;
	      
	      if (alphaXJ > 0)
		maskXJ = [alphaXJ 1-alphaXJ 0]';
	      else
		maskXJ = [0 1+alphaXJ -alphaXJ]';
	      end
	      if (alphaYJ > 0)
		maskYJ = [alphaYJ 1-alphaYJ 0];
	      else
		maskYJ = [0 1+alphaYJ -alphaYJ];
	      end
	      
	      intensityJ = newImg(subCJXI-wX-1:subCJXI+wX+1,...
				  subCJYI-wY-1:subCJYI+wY+1,L+1);
	      intensityJ = conv2(conv2(intensityJ,maskXJ,'same'),...
				 maskYJ, 'same');
	      intensityJ = intensityJ(2:2*wX+2,2:2*wY+2);
	      deltaI = intensityI - intensityJ;
	      
	      % image mismatch vector
	      bK = [sum(sum(deltaI .* Ix)); sum(sum(deltaI .* Iy))];
	      
	      % optical flow (L-K)
	      bK1 = sum(sum(deltaI .* Ix));
	      bK2 = sum(sum(deltaI .* Iy));  
	      dt = determinant;
	      naidaK = [(c*bK1 - b*bK2)/dt; (a*bK2 - b*bK1)/dt];
	      
	      k = k+1;
	      Vk(:,i) = Vk(:,i) + naidaK;
	      
	    else
	      diverge = 1; % corner out of range
	    end
	  end  % end of k loop
	  
	  % compute residual
	  if (L == 0)
	    intensityI = intensityI - mean(intensityI(:));
	    intensityJ = intensityJ - mean(intensityJ(:));
	    deltaI = intensityI - intensityJ;
	    
	    residuals(i) = sqrt(sum(deltaI(:).^2)/((2*wX+1)*(2*wY+1)-1));
	    ratios(i) = sum(deltaI(:).^2)/sum(intensityI(:).^2);
	  end
	  
	else    
	  diverge = 1; % corner out of range
	end
	divergentPts(i) = divergentPts(i) | diverge;
	
      end  % end if
    end  % for loop i
    
    d = d + Vk;  % optical flow vector d updated
    
  end
  
  
  cornersT = corners + d; % induced feature points
  eigVals = zeros(length(goodFeat),1);
  indEig = find(goodFeat);
  N = length(indEig);
  lambdaM = zeros(N,1);
  cornersT2 = cornersT(:,indEig);
  
  [rowX, rowY] = size(newImg);
  
  for i=1:N
      subCXF = cornersT2(1,i);
      subCYF = cornersT2(2,i);
      
      % do I really need this?
      subCXFI = round(subCXF);
      subCYFI = round(subCYF);
      
      if ((subCXFI > wX+2) & (subCXFI < rowX - wX - 1) & (subCYFI > wY+2) ...
              & (subCYFI < rowY - wY -1))
          intensityI = newImg(subCXFI-wX-2:subCXFI+wX+2,subCYFI-wY-2:subCYFI+wY+2);
          
          alphaX = subCXF - subCXFI;
          alphaY = subCYF - subCYFI;
          
          if (alphaX > 0)
              maskX = [alphaX 1-alphaX 0]';
          else
              maskX = [0 1+alphaX -alphaX]';
          end
          if (alphaY > 0)
              maskY = [alphaY 1-alphaY 0]';
          else
              maskY = [0 1+alphaY -alphaY]';
          end
          
          intensityI = conv2(conv2(intensityI,maskX,'same'), maskY, 'same');
          intensityI = intensityI(2:2*wX+4,2:2*wY+4);
          
          [gy, gx] = gradient(intensityI);
          gx = gx(2:2*wX+2,2:2*wY+2);
          gy = gy(2:2*wX+2,2:2*wY+2);
          a = sum(sum(gx.*gx));
          b = sum(sum(gx.*gy));
          c = sum(sum(gy.*gy));
          envelope = (a+c)/2;
          %m = (a+c)/2;
          determ = a*c - b^2;
          %d = a*c - b^2;
          radical = (envelope.^2 - determ).^(0.5);
          %n = sqrt(m^2 - d);
          lambda1 = envelope - radical;
          lambda2 = envelope + radical;
          
          % Find the minimum eigenvalue at every pixel of the image
          lambdaM(i) = min(abs(lambda1), abs(lambda2));
          %Q2(i) = min(abs(m-n), abs(m+n));
      end
  end
  
  eigVals(indEig) = lambdaM;
  
  
  
  
  trackedF = (~divergentPts) & (eigVals > 0) & (residuals < 18) & (ratios < 2);
  eigValsc = eigVals(find(trackedF));
  Q_max = [eigValsc; maxCorner];
  maxCorner = max(Q_max);
  trackedF = trackedF & (eigVals > thresh*maxCorner);
  trackable = trackable | trackedF;
  
  
  % keep the good feature and qualities
  dAffine = dAffine + [trackedF trackedF]' .* cornersT;
  pointLoc = pointLoc + trackedF .* eigVals;
  Nkept = size(find(trackedF),1);
  
  % for the remaining features try the following level
  goodFeat = goodFeatureTmp & (~trackable);
  trackingF = size(find(goodFeat),1);
  
  level = level + 1;
  
end  % end sublevel loop

cornersT = dAffine;
goodFeat = trackable;