function [T,best_consensus,best_error] = GetTransformRANSAC( p1, p2, normalize )
%Code written by Jim Patrick for project 2 in CEG725 2/16/2009
%GetTransformRANSAC tries to determine assumes outliers exist in
%correspondence points and tries to find inlier sets by using projective
%geometry constraints
%code based on http://en.wikipedia.org/wiki/RANSAC
[m npts] = size(p1);  %number of points

if m < 3
    p1 = [p1; ones(1,npts)];    %convert to homogeneous coordinates
    p2 = [p2; ones(1,npts)];
end

npt_threshold=.2;
n=10;                       %the minimum number of data required to fit the model
k=20000;                   %the maximum number of iterations allowed in the algorithm
t=2;                       %a threshold value for determining when a datum fits a model
d=npt_threshold*npts;      %the number of close data values required to assert that a model fits well to data
if d < 4, d=4; end

error=inf;
best_consensus=0;
best_error=inf;

%init T with least squares solution
T=GetTransform(p1,p2,normalize);

% while 1
    for iterations=1:k
        ind=randperm(npts); %get random indices for sample set
        pt1=p1(:,ind(1:n));
        pt2=p2(:,ind(1:n));
        tform=GetTransform(pt1, pt2, normalize);
        model1=tform*p1;
        
        for i = 1:size(model1,2)
            model1(:,i) = model1(:,i) / model1(3,i);
        end
        %model2=inv(tform)*p2;
        dist1=sqrt(sum((p2-model1).^2));    
        %dist2=sqrt(sum((p1-model2).^2));    
        %dist = (dist1 + dist2)/2;
        dist = dist1;
        consensus=union(find(dist<t), ind(1:n));

        cnt = length(consensus);
        if cnt > d
            pt1=p1(:,consensus);
            pt2=p2(:,consensus);
            tform=GetTransform(pt1, pt2, 0);
            model1=tform*pt1;
            for i = 1:size(model1,2)
                model1(:,i) = model1(:,i) / model1(3,i);
            end            
            %model2=inv(tform)*p2;
            dist1=sqrt(sum((pt2-model1).^2));    
            %dist2=sqrt(sum((p1-model2).^2));    
            %dist = (dist1 + dist2)/2;
            dist = dist1;
            curr_error = mean(dist);
            
            if curr_error < error            
                error = curr_error;
                T = tform;
                best_consensus=consensus;
                best_error=curr_error;                     
            end    
        end
    end
%     if best_error > 5
%         npt_threshold=npt_threshold-.1;
%         d=npt_threshold*npts;         %the number of close data values required to assert that a model fits well to data
%         if d < 4, break; end
%     else
%         break
%     end
% end