[3D] Ransac (get planes from point clouds)

This post has been moved to HERE

I have made two alrogithms, Ransac and Local_ransac.
They are used to get a planes, or a plane, or the best planes, from a 3d point cloud. this is nice, because most of our world exists out of planes. Both of these algorithms are highly efficient.

Example point cloud with some extracted planes

Local Ransac

% Tim Zaman (1316249), 2010, TU Delft, MSc:ME:BME:BMD:BR
%
% This local_ransac algorythm uses the 'no' amount of closest variables to
% the randomly chosen point:
%
% Usage:
% [p_best,n_best,ro_best,X_best,Y_best,Z_best,error_best,sample_best]=local_ransac_tim(p,no,k,t,d)
%
% no smallest number of points required
% k number of iterations
% t threshold used to id a point that fits well
% d number of nearby points required

function [p_best,n_best,ro_best,X_best,Y_best,Z_best,error_best,sample_best]=local_ransac_tim(p,no,k,t,d)

succes=0;
%Make sure we get a result =) (yeah i know its not common for RANSAC)
while succes==0
%Initialize variables
iterations=0;
%Until k iterations have occurrec
while iterations < k
ii=0;
clear p_close dist p_new p_in p_out sample_in loc_dist sample_out

%Pick a point
sample_in(1)=randi(length(p)); %point location
p_in(1,:)=p(sample_in(1),:); %point value

%Compute all local distances to this point
loc_dist=sqrt((p_in(1)-p(:,1)).^2+(p_in(2)-p(:,2)).^2+(p_in(3)-p(:,3)).^2);

%Initialize sample out
sample_out=[1:1:length(p)];

%Exclude the sample_in's
sample_out=sample_out(sample_out~=sample_in(1)); %remove first

for n=1:no

%Make sure the first sample can not be chosen anymore
loc_dist(sample_in(n))=inf;
[~,sample_in(n+1)]=min(loc_dist); %point location
p_in(n+1,:)=p(sample_in(n+1),:); %point value

%Exclude the sample_in's
sample_out=sample_out(sample_out~=sample_in(n+1)); %remove

end

%Fit to that set of n points
[n_est_in ro_est_in]=LSE(p_in);

p_new=p_in;

%For each data point oustide the sample
for i=sample_out
dist=dot(n_est_in,p(i,:))-ro_est_in;
%Test distance d to t
abs(dist);
if abs(dist) d
%Refit the line using all these points
[n_est_new ro_est_new X Y Z]=LSE(p_new);
for iii=1:length(p_new)
dist(iii)=dot(n_est_new,p_new(iii,:))-ro_est_new;
end
%Use the fitting error as error criterion (ive used SAD for ease)
error(iterations+1)=sum(abs(dist));
else
error(iterations+1)=inf;
end

if length(p_new) > d %made this up myself too
if iterations >1
%Use the best fit from this collection
if error(iterations+1) <= min(error)
succes=1;
p_best=p_new;
n_best=n_est_new;
ro_best=ro_est_new;
X_best=X;
Y_best=Y;
Z_best=Z;
error_best=error(iterations+1);
sample_best=sample_in;
end
end
end

 

iterations=iterations+1;
end
end
end

Ransac

%
%no smallest number of points required
%k number of iterations
%t threshold used to id a point that fits well
%d number of nearby points required

function [p_best,n_best,ro_best,X_best,Y_best,Z_best,error_best]=ransac_tim(p,no,k,t,d)

%Initialize variables
iterations=0;
%Until k iterations have occurred
while iterations < k
ii=0;
clear p_close dist p_new p_in p_out

%Draw a sample of n points from the data
perm=randperm(length(p));
sample_in=perm(1:no);
p_in=p(sample_in,:);
sample_out=perm(no+1:end);
p_out=p(sample_out,:);

%Fit to that set of n points
[n_est_in ro_est_in]=LSE(p_in);

%For each data point oustide the sample
for i=sample_out
dist=dot(n_est_in,p(i,:))-ro_est_in;
%Test distance d to t
abs(dist);
if abs(dist) d
%Refit the line using all these points
[n_est_new ro_est_new X Y Z]=LSE(p_new);
for iii=1:length(p_new)
dist(iii)=dot(n_est_new,p_new(iii,:))-ro_est_new;
end
%Use the fitting error as error criterion (ive used SAD for ease)
error(iterations+1)=sum(abs(dist));
else
error(iterations+1)=inf;
end

if iterations >1
%Use the best fit from this collection
if error(iterations+1) <= min(error)
p_best=p_new;
n_best=n_est_new;
ro_best=ro_est_new;
X_best=X;
Y_best=Y;
Z_best=Z;
error_best=error(iterations+1);
end
end

 

iterations=iterations+1;
end
end

Example

%Tim Zaman (1316249), 2010, TU Delft, MSc:ME:BME:BMD:BR
clc
close all
clear all

%Define variables
nrs=100; %amount of numbers
maxnr=20; %max amount of X or Y

n(1,:)=[1 1 1]; %1st plane normal vector
n(2,:)=[1 1 -1]; %2nd plane normal vector
n(3,:)=[-1 0 1]; %3rd plane normal vector
n(4,:)=[1 -1 1]; %4th plane normal vector
ro=[10 20 10 40];

%Make random points...
for i=1:4 %for 4 different planes

%Declarate 100 random x's and y's (p1 and p2)
p(1:nrs,1:2,i)=randi(maxnr,nrs,2);
%From the previous, calculate the Z
p(1:nrs,3,i)=(ro(i)-n(i,1)*p(1:nrs,1,i)-n(i,2).*p(1:nrs,2,i))/n(i,3);

%Add some random points
for ii=1:20 %10 points
randpt=p(randi(nrs),1:3,i); %take an random existing point
randvar=(randi(14,1,3)-7); %adapt that randomly +-7
p(nrs+ii,1:3,i)=randpt+randvar; %put behind pointmatrix
end

end

%combine the four dataset-planes we have made into one
p_tot=[p(:,:,1);p(:,:,2);p(:,:,3);p(:,:,4)];

figure
plot3(p(:,1,1),p(:,2,1),p(:,3,1),'.r')
hold on; grid on
plot3(p(:,1,2),p(:,2,2),p(:,3,2),'.g')
plot3(p(:,1,3),p(:,2,3),p(:,3,3),'.b')
plot3(p(:,1,4),p(:,2,4),p(:,3,4),'.k')

no=3;%smallest number of points required
k=5;%number of iterations
t=2;%threshold used to id a point that fits well
d=70;%number of nearby points required

%Initialize samples to pick from
samples_pick=[1:1:length(p_tot)];
p_pick=p_tot;
for i=1:4 %Search for 4 planes

[p_best,n_best,ro_best,X_best,Y_best,Z_best,error_best,samples_used]=local_ransac_tim(p_pick,no,k,t,d);

samples_pick=[1:1:length(p_pick)];

%Remove just used points from points for next plane
for ii=1:length(samples_used) %In lack for a better way to do it used a loop
samples_pick=samples_pick(samples_pick~=samples_used(ii)); %remove first
end

p_pick=p_pick(samples_pick,:);

pause(.5)
plot3(p_best(:,1),p_best(:,2),p_best(:,3),'ok')
mesh(X_best,Y_best,Z_best);colormap([.8 .8 .8])
beep
end

LSE() Function, Least squares estimation function for matlab for planes.

function [n_est ro_est X Y Z]=LSE(p)
%© Tim Zaman 2010, input: p (points)
% Works like [n_est ro_est X Y Z]=LSE(p)
% p should be a Mx3; [points x [X Y Z]]

%Calculate mean of all points
pbar=mean(p);

for i=1:length(p)
A(:,:,i)=(p(i,:)-pbar)'*(p(i,:)-pbar);
end

%Sum up all entries in A
Asum=sum(A,3);
[V ~]=eig(Asum);

%Calculate new normal vector
n_est=V(:,1);

%Calculate new ro
ro_est=dot(n_est,pbar);

[X,Y]=meshgrid(min(p(:,1)):max(p(:,1)),min(p(:,2)):max(p(:,2)));
Z=(ro_est-n_est(1)*X-n_est(2).*Y)/n_est(3);
end

9 responses

9 11 2011
figutka

Hi
I have a point cloud containing flat surfaces and I’m trying to find the parameters describing the plans. For this purpose I tried to use your code.

To start I tried an example that you put. In fact Matlab crashes and I see the value of parameters d.

Could you help me??? I just started working with Matlab and I’m really zero.

9 11 2011
figutka

Hi
I have a point cloud containing flat surfaces and I’m traying to find the parameters describing the plans. For this purpose I tried to use your code.

To start I tried an example that you put. In fact Matlab crashes and I see the value of parameters d.

Could you help me??? I just started working with Matlab and I am I’m really zero.

25 03 2011
nttputus

Haha sure! Thanks

25 03 2011
nttputus

I suppose ‘if abs(dist) d’ should be ‘if abs(dist)<d'. And your program is not so fast, huh? lol

25 03 2011
Tim Zaman

being fast is not its purpose! being clear and structured is. i have made this half a year ago but maybe it makes you happy :)

25 03 2011
Tim Zaman

if yoy want things fast you shouldnt usr matlab :)

25 03 2011
nttputus

Hi,

You not define the function LSE(), so program can’t run.

25 03 2011
Tim Zaman

updated. I am fast, huh?

24 03 2011
More code released « Tim Zaman

[…] [3D] Ransac (get planes from point clouds) […]

Leave a Reply

Please log in using one of these methods to post your comment:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s




Follow

Get every new post delivered to your Inbox.

%d bloggers like this: