% Practice homework: line and plane fitting

% Here is a set of points in 2D which comes from line 

x = 5*rand(1,30); 
y = 3*x + 2 + 0.5*randn(1,30);

plot(x,y,'.'); 

% find the least squares estimate of the line
% and plot resulting line 

% YOUR CODE GOES HERE

% plane fitting exercise
n = [1,2,1.5] % plane normal 
d = 4; 
npoints = 50;
x = 5*rand(1,npoints); 
y = 5*rand(1,npoints); 
z = repmat(d/n(3),1,npoints) - x*n(1)/n(3) - y*n(2)/n(3) + 0.5*randn(1,npoints); 

plot3(x,y,z,'.'); box on; 


% YOUR CODE GOES HERE


% robust plane fitting excercise using RANSAC 
clear x, y, z; 
n = [1,2,1.5] % plane normal 
d = 4; 

% generating synthetic plane and outliers
xin = 5*rand(1,npoints); 
yin = 5*rand(1,npoints); 
zin = repmat(d/n(3),1,npoints) - xin*n(1)/n(3) - yin*n(2)/n(3) + 0.2*randn(1,npoints);
xout = 5*rand(1,npoints/2); 
yout = 5*rand(1,npoints/2); 
zout = repmat(d/n(3),1,npoints/2) - xout*n(1)/n(3) - yout*n(2)/n(3) + (4*rand(1,npoints/2)-2);

% pick some number of uniformly distributed outliers
% outliers = 5*rand(3,round(npoints/2)) % - repmat(2.5*ones(3,1),1,round(npoints/2)); 

perm = randperm(npoints + round(npoints/2)); 

points = [xin xout; yin yout; zin zout];

x = points(1,perm); 
y = points(2,perm); 
z = points(3,perm); 

plot3(x,y,z,'.'); hold on; 
plot3(xout, yout, zout,'r.');

% Given all the points compute the best plane and separate the points into 
% inliers and outliers