%COMP 256 Assignment 3
%Adrian Ilie

function res=ransac(data,sigma,inliers);

%data=datagen(sigma,inliers);

%set parameters
s=2;%line, need 2 points
n=2;%line, need 2 points
e=0.2;%worst case, 20%inliers
t=sqrt(3.84)*sigma;%distance threshold
k=power(2,14);%infinity
%d=(100-inliers)/100;%at least 50% the inliers should fit
d=inliers*(10*(sigma*100+3))/100;%if sigma is greater, more inliers should fit

%initialize best estimate to a line fitted through all points
bestline=linefit(data);
extdata=[data;ones(1,100)];
bestresidual=bestline'*extdata;
bestresidual=sqrt(bestresidual*bestresidual'/100);

c=0;
found=0;
while (c<k)|(found==0)
    indices=randperm(100);%generate a random permutation
    points=data(:,indices(1:n));%pick the first n points
    line=linefit(points);
    close=0;
    good=[];
    for p=n+1:100%compute distance to other points
        dist=line'*extdata(:,p);
        if abs(dist)<=t%keep the ones that fit the model
            close=close+1;
            good=[good data(:,p)];
        end
    end
    if close>d%model looks good, fit again
        line=linefit(good);
        residual=line'*[points;ones(1,n)];
        residual=sqrt(residual*residual'/n);
        if residual<bestresidual%model is best so far
            bestresidual=residual;
            bestline=line;
            found=1;
        end
    end
    if close>0
        k=round(log(1-0.99)/log(1-power(close/100,s)));
    end    
    if close>=inliers%terminate, acceptable consensus reached
        c=k;
    end
    c=c+1;
end

figure(1);
hold on;
drawline2(bestline(1),bestline(2),bestline(3),'-g');

res=bestline;