function corners = featureSelection(img, wX, wY, threshold)
% file: featureSelection.m
% purpose: Implement the feature selection methodology detailed in 
%          subsection 3 of "Pyramidal Implementation of the Lucas
%          Kanade Feature Tracker Description of the algorithm"
%          Jean-Yves Bouguet, Intel Corporation
% usage: corners = featureSelection(img, wX, wY, threshold)
% input:
%       img: grayscale image
%       wX: selection window size X-direction
%       wY: selection window size Y-direction
%       threshold: threshold ratio multiplier for the selection of pixels greater 
%                  than the threshold*(maximum of the minimum eigenvalues for each pixel)
% output:
%        corners: The trackable points selected by the feature selector.
% history:
%         04/29/04 SLB initial revision
%-----------------------------------------------------------------------------

% take the 2-D gradient of the image
[gY, gX] = gradient(img);

% another method could use is to low pass filter, then take the derivative
% get about the same result, but computationally more expensive
% lpF = [1/4 1/2 1/4];
% derF = [-1/2 0 1/2];
% gX = conv2( conv2( img, lpF', 'same' ), derF, 'same' );
% gY = conv2( conv2( img, lpF, 'same' ), derF', 'same' );
% 

% median masks for integration window
xFilt = ones(2*wX+1,1);
yFilt = ones(2*wY+1,1);
xyFilt = ones(2*wX+1, 2*wY+1);

% apply 2-D median smoothing mask for (dI/dx)^2, (dI/dy)^2, (dI/dx)(dI/dy)
%dIx2 = conv2( conv2(gX .* gX, xFilt, 'same'), yFilt, 'same');
dIx2 = conv2(gX .* gX, xyFilt, 'same');
%dIxy2 = conv2( conv2(gX .* gY, xFilt, 'same'), yFilt, 'same');
dIxy2 = conv2(gX .* gY, xyFilt, 'same');
%dIy2 = conv2( conv2(gY .* gY, xFilt, 'same'), yFilt, 'same');
dIy2 = conv2(gY .* gY, xyFilt, 'same');

% compute the eigenvalues of each pixel using the matrix C = [(Ix)^2 Ixy; Ixy (Iy)^2 ];
% i.e. lambda(1,2) = ((Ix)^2 + (Iy)^2)/2  +/- sqrt(((Ix)^2 + (Iy)^2)^2/4 - (Ix)^2*(Iy)^2 + (Ixy)^2)
envelope = (dIx2 + dIy2)/2;
determ = dIx2.*dIy2 - dIxy2.^2;
radical = (envelope.^2 - determ).^(0.5);

lambda1 = envelope - radical;
lambda2 = envelope + radical;

% Find the minimum eigenvalue at every pixel of the image
lambdaM = min(abs(lambda1), abs(lambda2));

% find the maximum of lambdaM over the entire image
lambdaMMax = max(max(lambdaM));
% retain the image pixels that > {5,10}% of lambdaMMax
threshPixels = threshold*lambdaMMax;

% from those pixels, retain the local max pixels within the 3x3 neighborhood
% Y = ordfilt2(X,9,ones(3,3)) implements a 3-by-3 maximum filter.
mx = ordfilt2(lambdaM,9,ones(3,3)); 
[X,Y] = find(lambdaM == mx);
[nx,ny] = size(img);
maxMask = zeros(size(lambdaM));
for pp=1:length(X)
    maxMask(X(pp),Y(pp)) = 1;
end
Q = (lambdaM > threshPixels) & maxMask;

% keep the subset of these pixels whos minimum distance between any pair of pixels
% is greater than a given threshold distance (e.g. 5 or 10 pixels)
Qvect = find(Q(:));
[Qval, index] = sort(lambdaM(Qvect));
QX = ceil(Qvect(index)/nx); % Get the row pixel number from the array
QY = rem((Qvect(index)-1), nx) + 1; % Get the col pixel number from the array
QXDist = QX*ones(1,length(QX)) - (QX*ones(1,length(QX)))';
QYDist = QY*ones(1,length(QY)) - (QY*ones(1,length(QY)))';
QDist = (QXDist.^2 + QYDist.^2).^(0.5);
% Since the QDist matrix is symmetric -> only need to look at the data either
% above/below the main diagonal. Also want to subtract out the threshold here.
threshSpace = 5;
QDistL = tril((QDist - threshSpace^2), -1);
indexgood = find(sum(QDistL >= 0));

fCol = QX(indexgood);
fRow = QY(indexgood);

corners = [fRow fCol]';