% Author : Sasakthi Abeysinghe, Timothy Simpson
% Date   : 02/15/2006

function points = FindMatchingPoints(image1, image2)
%points = FindMatchingPoints(image1, image2)
%finds a set of matching points between image 1 and image 2

% Converting to grayscale and then equalizing histograms to try to fix
% picture exposure errors.
image1 = histeq(rgb2gray(image1));
image2 = histeq(rgb2gray(image2));

noOfSlices = 100.0;
maxNoOfPeaks = 1;
minNoOfPeaks = 1;
gridSize = int16(floor(min(size(image1,1), size(image1,2)) / 8)) ;
pointCount = 0;
points = [];
while pointCount < 15,  % Loop until you find 15 correspondances
    proceed = false;
    while ~proceed,  % Loop until you find a point which has not been chosen before
        proceed = false;
        x1 = int16(floor(rand(1) * (size(image1, 2) - gridSize - 1)) + 1);
        y1 = int16(floor(rand(1) * (size(image1, 1) - gridSize - 1)) + 1);
        img1Box = image1(y1 : y1+gridSize-1, x1: x1+gridSize-1);
        
        firstColor = img1Box(1,1);
        for i=1:size(img1Box, 1),
            for j=1:size(img1Box, 2),
                proceed = proceed | (img1Box(i,j) ~= firstColor);
                if(proceed), break; end
            end
            if proceed, break; end
        end
        for i=1:pointCount,
            proceed = (proceed & (x1 ~= points(i, 1)) &  (y1 ~= points(i, 2)));
        end

    end
        
    

    % Finding the correlation between a random box in the 1st image and the
    % 2nd image
    correlation = normxcorr2(img1Box, image2);
    [maxCorrelation, maxIndex] = max(abs(correlation(:)));

    topSlice = maxCorrelation - ((maxCorrelation - min(correlation(:))) / noOfSlices);
    % Counting the number of potential hits...  To make sure that we dont
    % chose a bad control point.  Well at least most of the time :D
    noOfPeaks = sum(sum((correlation > (ones(size(correlation, 1), size(correlation, 2)) * topSlice))));
    if (noOfPeaks <= maxNoOfPeaks) & (noOfPeaks >= minNoOfPeaks),   % Is this a good control point
        [yPeak, xPeak] = ind2sub(size(correlation), maxIndex(1));
        correlationOffset = [(xPeak-size(img1Box,2)) (yPeak-size(img1Box,1))];
        if (correlationOffset(1) >= 1) & (correlationOffset(1) < size(image2, 2)) & (correlationOffset(2) >= 1) & (correlationOffset(2) < size(image2, 1)),
            points = [points; x1, y1, correlationOffset(1), correlationOffset(2)];  
            pointCount = pointCount + 1;
        end
    end  
    disp([int2str(pointCount), ' point(s) found so far... ', int2str(noOfPeaks), ' peak(s) in the last pick.']);
end

% Cleaning the points which were found
points = CleanPoints(points);
% Displaying the points to see whether we did a decent job at finding the
% points.
cpselect(image1, image2, double(points(:, 1:2)), double(points(:, 3:4)));