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

With this code in Matlab you can get planes or surfaces from point clouds (points in space). It uses the RANSAC algorithm and a LSQ function in the bottom.

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.

Example (semi-random) point cloud with some extracted planes

Local Ransac

  1. % Tim Zaman, 2010, TU Delft, MSc:ME:BME:BMD:BR
  2. %
  3. % This local_ransac algorythm uses the 'no' amount of closest variables to
  4. %  the randomly chosen point:
  5. %
  6. % Usage:
  7. % [p_best,n_best,ro_best,X_best,Y_best,Z_best,error_best,sample_best]=local_ransac_tim(p,no,k,t,d)
  8. %
  9. % no smallest number of points required
  10. % k number of iterations
  11. % t threshold used to id a point that fits well
  12. % d number of nearby points required
  13.  
  14.  
  15. 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)
  16.  
  17.  
  18. succes=0;
  19. %Make sure we get a result =) (yeah i know its not common for RANSAC)
  20. while succes==0
  21.     %Initialize variables
  22.     iterations=0;
  23.     %Until k iterations have occurrec
  24.     while iterations < k
  25.         ii=0;
  26.         clear p_close dist p_new p_in p_out sample_in loc_dist sample_out
  27.  
  28.         %Pick a point
  29.         sample_in(1)=randi(length(p)); %point location
  30.         p_in(1,:)=p(sample_in(1),:); %point value
  31.  
  32.         %Compute all local distances to this point
  33.         loc_dist=sqrt((p_in(1)-p(:,1)).^2+(p_in(2)-p(:,2)).^2+(p_in(3)-p(:,3)).^2);
  34.  
  35.         %Initialize sample out
  36.         sample_out=[1:1:length(p)];
  37.  
  38.         %Exclude the sample_in's
  39.         sample_out=sample_out(sample_out~=sample_in(1)); %remove first
  40.  
  41.         for n=1:no
  42.  
  43.             %Make sure the first sample can not be chosen anymore
  44.             loc_dist(sample_in(n))=inf;
  45.             [~,sample_in(n+1)]=min(loc_dist);     %point location
  46.             p_in(n+1,:)=p(sample_in(n+1),:);        %point value
  47.  
  48.             %Exclude the sample_in's
  49.             sample_out=sample_out(sample_out~=sample_in(n+1)); %remove
  50.  
  51.         end
  52.  
  53.  
  54.         %Fit to that set of n points
  55.         [n_est_in ro_est_in]=LSE(p_in);
  56.  
  57.         p_new=p_in;
  58.  
  59.         %For each data point oustide the sample
  60.         for i=sample_out
  61.             dist=dot(n_est_in,p(i,:))-ro_est_in;
  62.             %Test distance d to t
  63.             abs(dist);
  64.             if abs(dist)<t %If d<t, the point is close
  65.                 ii=ii+1;
  66.                 %p_close(ii,:)=;
  67.                 p_new=[p_new;p(i,:)];
  68.                 %And remember the location of this succesful number and add
  69.                 sample_in=[sample_in i];
  70.             end
  71.         end
  72.  
  73.  
  74.         %If there are d or more points close to the line
  75.         if length(p_new) > d
  76.             %Refit the line using all these points
  77.             [n_est_new ro_est_new X Y Z]=LSE(p_new);
  78.             for iii=1:length(p_new)
  79.                 dist(iii)=dot(n_est_new,p_new(iii,:))-ro_est_new;
  80.             end
  81.             %Use the fitting error as error criterion (ive used SAD for ease)
  82.             error(iterations+1)=sum(abs(dist));
  83.         else
  84.             error(iterations+1)=inf;
  85.         end
  86.  
  87.         if length(p_new) > d %made this up myself too
  88.             if iterations >1
  89.                 %Use the best fit from this collection
  90.                 if error(iterations+1) <= min(error)
  91.                     succes=1;
  92.                     p_best=p_new;
  93.                     n_best=n_est_new;
  94.                     ro_best=ro_est_new;
  95.                     X_best=X;
  96.                     Y_best=Y;
  97.                     Z_best=Z;
  98.                     error_best=error(iterations+1);
  99.                     sample_best=sample_in;
  100.                 end
  101.             end
  102.         end
  103.  
  104.         iterations=iterations+1;
  105.     end
  106. end
  107.  
  108.  
  109.  
  110.  
  111. end

Ransac

  1. %
  2. %no smallest number of points required
  3. %k number of iterations
  4. %t threshold used to id a point that fits well
  5. %d number of nearby points required
  6.  
  7.  
  8. function [p_best,n_best,ro_best,X_best,Y_best,Z_best,error_best]=ransac_tim(p,no,k,t,d)
  9.  
  10. %Initialize variables
  11. iterations=0;
  12. %Until k iterations have occurrec
  13. while iterations < k
  14.     ii=0;
  15.     clear p_close dist p_new p_in p_out
  16.  
  17.     %Draw a sample of n points from the data
  18.     perm=randperm(length(p));
  19.     sample_in=perm(1:no);
  20.     p_in=p(sample_in,:);
  21.     sample_out=perm(no+1:end);
  22.     p_out=p(sample_out,:);
  23.  
  24.     %Fit to that set of n points
  25.     [n_est_in ro_est_in]=LSE(p_in);
  26.  
  27.     %For each data point oustide the sample
  28.     for i=sample_out
  29.         dist=dot(n_est_in,p(i,:))-ro_est_in;
  30.         %Test distance d to t
  31.         abs(dist);
  32.         if abs(dist)<t %If d<t, the point is close
  33.             ii=ii+1;
  34.             p_close(ii,:)=p(i,:);
  35.         end
  36.     end
  37.  
  38.     p_new=[p_in;p_close];
  39.  
  40.     %If there are d or more points close to the line
  41.     if length(p_new) > d
  42.         %Refit the line using all these points
  43.         [n_est_new ro_est_new X Y Z]=LSE(p_new);
  44.         for iii=1:length(p_new)
  45.             dist(iii)=dot(n_est_new,p_new(iii,:))-ro_est_new;
  46.         end
  47.         %Use the fitting error as error criterion (ive used SAD for ease)
  48.         error(iterations+1)=sum(abs(dist));
  49.     else
  50.         error(iterations+1)=inf;
  51.     end
  52.  
  53.  
  54.     if iterations >1
  55.         %Use the best fit from this collection
  56.         if error(iterations+1) <= min(error)
  57.             p_best=p_new;
  58.             n_best=n_est_new;
  59.             ro_best=ro_est_new;
  60.             X_best=X;
  61.             Y_best=Y;
  62.             Z_best=Z;
  63.             error_best=error(iterations+1);
  64.         end
  65.     end
  66.  
  67.     iterations=iterations+1;
  68. end
  69.  
  70.  
  71. end

Example

  1. %Tim Zaman (1316249), 2010, TU Delft, MSc:ME:BME:BMD:BR
  2. clc
  3. close all
  4. clear all
  5.  
  6. %Define variables
  7. nrs=100; %amount of numbers
  8. maxnr=20; %max amount of X or Y
  9.  
  10. n(1,:)=[1 1 1];  %1st plane normal vector
  11. n(2,:)=[1 1 -1]; %2nd plane normal vector
  12. n(3,:)=[-1 0 1]; %3rd plane normal vector
  13. n(4,:)=[1 -1 1]; %4th plane normal vector
  14. ro=[10 20 10 40];
  15.  
  16. %Make random points...
  17. for i=1:4 %for 4 different planes
  18.  
  19. %Declarate 100 random x's and y's (p1 and p2)
  20. p(1:nrs,1:2,i)=randi(maxnr,nrs,2);
  21. %From the previous, calculate the Z
  22. 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);
  23.  
  24. %Add some random points
  25. for ii=1:20 %10 points
  26. randpt=p(randi(nrs),1:3,i);  %take an random existing point
  27. randvar=(randi(14,1,3)-7);       %adapt that randomly +-7
  28. p(nrs+ii,1:3,i)=randpt+randvar; %put behind pointmatrix
  29. end
  30.  
  31. end
  32.  
  33. %combine the four dataset-planes we have made into one
  34. p_tot=[p(:,:,1);p(:,:,2);p(:,:,3);p(:,:,4)];
  35.  
  36. figure
  37. plot3(p(:,1,1),p(:,2,1),p(:,3,1),'.r')
  38. hold on; grid on
  39. plot3(p(:,1,2),p(:,2,2),p(:,3,2),'.g')
  40. plot3(p(:,1,3),p(:,2,3),p(:,3,3),'.b')
  41. plot3(p(:,1,4),p(:,2,4),p(:,3,4),'.k')
  42.  
  43. no=3;%smallest number of points required
  44. k=5;%number of iterations
  45. t=2;%threshold used to id a point that fits well
  46. d=70;%number of nearby points required
  47.  
  48. %Initialize samples to pick from
  49. samples_pick=[1:1:length(p_tot)];
  50. p_pick=p_tot;
  51. for i=1:4 %Search for 4 planes
  52.  
  53. [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);
  54.  
  55. samples_pick=[1:1:length(p_pick)];
  56.  
  57. %Remove just used points from points for next plane
  58. for ii=1:length(samples_used) %In lack for a better way to do it used a loop
  59. samples_pick=samples_pick(samples_pick~=samples_used(ii)); %remove first
  60. end
  61.  
  62. p_pick=p_pick(samples_pick,:);
  63.  
  64. pause(.5)
  65. plot3(p_best(:,1),p_best(:,2),p_best(:,3),'ok')
  66. mesh(X_best,Y_best,Z_best);colormap([.8 .8 .8])
  67. beep
  68. end

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

  1. function [n_est ro_est X Y Z]=LSE(p)
  2. %© Tim Zaman 2010, input: p (points)
  3. % Works like [n_est ro_est X Y Z]=LSE(p)
  4. % p should be a Mx3; [points x [X Y Z]]</code>
  5.  
  6. %Calculate mean of all points
  7. pbar=mean(p);
  8. for i=1:length(p)
  9. A(:,:,i)=(p(i,:)-pbar)'*(p(i,:)-pbar);
  10. end
  11.  
  12. %Sum up all entries in A
  13. Asum=sum(A,3);
  14. [V ~]=eig(Asum);
  15.  
  16. %Calculate new normal vector
  17. n_est=V(:,1);
  18.  
  19. %Calculate new ro
  20. ro_est=dot(n_est,pbar);
  21.  
  22. [X,Y]=meshgrid(min(p(:,1)):max(p(:,1)),min(p(:,2)):max(p(:,2)));
  23. Z=(ro_est-n_est(1)*X-n_est(2).*Y)/n_est(3);
  24. end


See Suddhasheel Ghosh’s website for a quick review of the code, in comparison to some better ones.

Tim Zaman

MSc Biorobotics. Specialization in computer vision and deep learning. Works at NVIDIA.

You may also like...

19 Responses


  1. %Remove just used points from points for next plane
    samples_pick = setdiff(samples_pick,samples_used);

    is probably what you want instead of


    %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

    anyway, thanks for this bunch of code, I needed that !

  2. Patrick says:

    Hy!

    Nice code. Works fine. A question: Do you have a good Paper or a book titel where I can find some mathematics and theory about the code.

    Thanks

  3. Dapo says:

    Hi, i am trying to implement you algorithm in matlab for detection of planes from my own point cloud.
    I don’t understand this function. I know its an external function. I am new to matlab, please can you explain.
    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)

    • Tim Zaman says:

      Sorry to reply like this, but you should first get acquainted with Matlab before using 3rd party functionality like this one! If you then still can’t figure it out, let me know!

  4. Pankaj Kumar says:

    Hi Tim,

    thanks for this code as i have found it very useful.

    Is this your latest updated code, so that I could use it?

    waiting for your reply.

    Pankaj

  5. Xiao says:

    Hi,

    May consider revising the line 22 in LSE() as

    [X,Y]=meshgrid(linspace(min(p(:,1)),max(p(:,1)),20),linspace(min(p(:,2)),max(p(:,2)),20));

    for data that are less than unity.

    Thank you for your work.

    Xiao

  6. Dear Tim,

    Please have a look at my website now. It would show you the different outputs obtained from the various algorithms.

    Regards

    Suddhasheel

  7. Suddhasheel Ghosh says:

    Hi,

    Thanks for putting up your code here. However, I observe two things:

    (a) The threshold “t” has not been used in the code for Local_Ransac
    (b) What does “if abs(dist) d” mean?
    (c) In case I use abs(dist) < d, then the code generates an error: undefined function or variable n_est_new.

    Could I request you to clarify these issues?

    Regards

    Suddhasheel

    • Tim says:

      Whoops sorry, it seems the website had scambled some code. i have now updated it.
      (a) threshold is not a Ransac requirement if i remember correctly.
      (b) that was a typo

      • Suddhasheel Ghosh says:

        The RANSAC algo does require an input of the threshold “t” to ensure the inliers.

      • Tim says:

        Oh yes you are right. i was talking about another threshold (that iterated until it found a plane) which you could need when you are certain there is a plane. But as you can see, both codes now include the threshold ‘t’.

      • Suddhasheel Ghosh says:

        The code here for RANSAC is giving some strange outputs for a dataset. How would it be possible for me to show those errors to you?

      • Suddhasheel Ghosh says:

        Hello Again,

        I have put up this website.

        http://lidarprocessing.blogspot.com/

        I have also given reference to your website on this blog. In the next post I would point out the errors faced by me using your code and other codes.

        Regards

  8. nttputus says:

    Haha sure! Thanks

  9. nttputus says:

    Hi,

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