% 3.10 RANSAC

clear;
close all;

% Data (columns vectors)
x = [1 2 3 4 5 6 7 8 9 10]';
y = [2.6892 3.3476 7 5.1878 5.6323 6.5843 6.7507 7.2644 8.5423 9.0124]';

% Model: y = a1*x+a2
% State: s = [a1 a2];

l = length(x);

% Solve for the minimum LSE error with all points
A = [x ones(l,1)];
s = inv(A'*A)*A'*y;

% Plot the result of LSE
% Final estimated line (with inliers only)
yest = s(1)*x+s(2);

figure;
plot(x,yest);
hold;
plot(x,y,'b.');

%%%% RANSAC

nmin = l;
for(k=1:100)
    % Pick two points at random
    i1 = floor(l*rand(1,1)+1);
    i2 = floor(l*rand(1,1)+1);
    % Make sure that they are different!
    while(i2==i1)
        i2 = floor(l*rand(1,1)+1);               
    end
    
    % Fit a line (exact solution with 2 points)
    A0 = [x(i1) 1; x(i2) 1];
    s0 = inv(A0)*[y(i1); y(i2)];
    
    % Compute the residual error
    y0 = s0(1)*x+s0(2);
    err = y0-y;
    % Compute the standard deviation of the residual error
    err_std = std(err);
    
    % Compute the number of outliers:
    % points with error higher than the standard deviation
    out0 = find(abs(err)>err_std);
    nout = length(out0);

    % Search for the case with least number of outliers
    if(nout<nmin)
        i1ok = i1;
        i2ok = i2;
        sok = s0;
        yok = y0;
        outl = out0;
        inl = find(abs(err)<=err_std);
        nmin = nout;
    end
end

% Plot the result of RANSAC
% Estimated line with two points (best case)
yest = sok(1)*x+sok(2);

figure;
plot(x,yest,'r');
hold;
plot(x(inl),y(inl),'b.');
plot(x(outl),y(outl),'r.');
plot(x(i1ok),y(i1ok),'g.');
plot(x(i2ok),y(i2ok),'g.');

% Solve for the minimum LSE error with inlier points only
A = [x(inl) ones(length(inl),1)];
s = inv(A'*A)*A'*y(inl);

% Plot the result of LSE
% Final estimated line (with inliers only)
yest = s(1)*x(inl)+s(2);

figure;
plot(x(inl),yest);
hold;
plot(x(inl),y(inl),'b.');
