# [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.

## 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.

### 19 Responses

1. Ludovic Magerand says:

``` %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?

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. Suddhasheel Ghosh says:

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

• nttputus says:

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

• Tim Zaman says:

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 🙂

• Tim Zaman says:

if yoy want things fast you shouldnt usr matlab 🙂

9. nttputus says:

Hi,

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

• Tim Zaman says:

updated. I am fast, huh?