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

function newPoints = CleanPoints(points)
% function newPoints = CleanPoints(points)
% Gets the point set and tries to heuristically remove invalid points.
% This assumes that at least half of the points are correct.

pointCount = size(points, 1);
% Generate a set of masks so that we pick 4 points in each mask
masks = zeros(pointCount, pointCount*4, 'int16');
for i=1:pointCount*4,
    maskCount = 0;
    while maskCount < 4,
        pos = floor(rand(1) * pointCount) + 1;
        masks(pos, i) = 1;
        maskCount = sum(masks);
        maskCount = maskCount(i);        
    end
end

% Generating an error histogram to find out which points end up messing up
% the others.
errorHistogram = zeros(pointCount, 2);
for i=1:pointCount,
    errorHistogram(i,1) = i;
end
for i=1:pointCount*4,
    tempPoints = points .* [masks(:,i), masks(:,i), masks(:,i), masks(:,i)];
    for j=pointCount:-1:1,
        if(tempPoints(j, 1) == 0),
            tempPoints = [tempPoints(1:j-1,:); tempPoints(j+1:size(tempPoints,1), :)];
        end   
    end
    tempHom = GetHomography(tempPoints);
    
    Q1 = double([transpose(points(:,1));transpose(points(:,2)); ones(1, size(points, 1))]);
    Q2 = double([transpose(points(:,3));transpose(points(:,4)); ones(1, size(points, 1))]);
    Q3 = tempHom * Q1;
    Q3 = Q3 ./ [Q3(3,:); Q3(3,:); Q3(3,:)];
    error = [zeros(pointCount, 1), transpose(sum(abs(Q2 - Q3)))];
    errorHistogram = errorHistogram + error;        
end

% Sorting the error histogram to find out the best points, and chosing the
% best points.
errorHistogram = sort(errorHistogram, 2, 'ascend');
newPointCount = 4; %floor(pointCount / 2);
newPoints = zeros(newPointCount,4);
for i = 1:newPointCount,
    newPoints(i,:) = points(int16(round(errorHistogram(i,1))), :);
end