How to fit a line using RANSAC in Cartesian coordinates?
I am using a 2D Lidar and getting the data as angle and distance with respect to lidar Position. I have to create a floor plan using Lidar and the data is given bellow is represent a room. I want to use the RANSAC algorithm to find the wall of the room. If I could fit RANSAC then I believe somehow I could find the floor flan. I have written a code but it's not fitting into my wall. what modification should I do so that it will fit into my room wall?
#(angle, distance)
0,942
0.62,3469
1.25,3350
2.5,3410
3.12,3404
3.75,3403
4.37,3464
5,3441
5.62,3445
6.25,3444
6.87,3455
7.5,3464
8.12,3464
8.75,3477
9.37,3470
10,3504
10.62,3505
11.25,3505
11.87,3516
12.5,3529
13.12,3541
13.75,3543
14.37,3552
15,3559
15.62,3565
16.25,3578
16.87,3591
17.5,3607
18.12,3624
18.75,3634
19.37,3630
20,3651
20.62,3673
21.25,3678
21.87,3697
22.5,3711
23.12,3726
23.75,3744
24.37,3765
25,3780
25.62,3796
26.25,3809
26.87,3830
27.5,3867
28.12,3881
28.75,3854
29.37,3751
30,3689
30.62,3637
31.25,3589
31.87,4029
37.5,3483
50,2734
53.75,1686
54.37,1656
55,1631
55.62,1621
56.25,1608
56.87,1600
57.5,1595
58.12,1598
58.75,1596
59.37,1604
60,1611
60.62,1622
61.25,1644
61.87,1673
65,2212
65.62,2221
66.25,1037
66.87,1299
67.5,2086
67.5,130
83.12,2002
83.75,1995
84.37,1993
85.62,2061
86.25,2043
86.87,2046
87.5,2040
89.37,2082
90,2713
90.62,2245
91.25,2046
93.75,2092
102.5,327
109.37,1349
110,4279
110.62,2177
111.25,2175
111.87,2136
113.12,2151
113.75,2170
114.37,2186
123.12,2066
123.75,2080
124.37,2087
125,2110
125.62,1778
126.25,1732
126.87,428
127.5,1650
128.12,1093
128.75,2206
129.37,2219
130,2243
130.62,2276
131.25,2317
131.87,2319
132.5,2305
133.12,2276
133.75,2253
135,2224
135.62,2202
136.25,2181
136.87,2156
137.5,2131
138.12,2108
138.75,2081
139.37,2068
140,2046
140.62,2028
141.25,1982
141.87,2001
142.5,1985
143.12,2030
152.5,1727
153.12,1728
153.75,1722
154.37,1711
155,1700
155.62,1691
156.25,1683
156.87,1672
157.5,1666
158.12,1655
158.75,1645
159.37,1637
160,1633
160.62,1622
161.25,1621
161.87,1611
162.5,1602
163.12,1597
163.75,1592
164.37,1583
165,1579
165.62,1578
166.25,1571
166.87,1564
167.5,1558
168.12,1552
168.75,1551
169.37,1550
170,1545
170.62,1543
171.25,1540
171.87,1537
172.5,1529
173.12,1527
173.75,1527
174.37,1524
175,1522
175.62,1518
176.25,1519
176.87,1517
177.5,1513
178.12,1510
178.75,1514
180,1514
180.62,1511
181.25,1519
181.87,1513
182.5,1514
183.12,1513
183.75,1513
184.37,1514
185,1518
185.62,1517
186.25,1519
186.87,1517
187.5,1526
188.12,1522
188.75,1526
189.37,1527
190,1530
190.62,1536
191.25,1536
191.87,1541
192.5,1544
193.12,1549
193.75,1553
194.37,1555
195,1553
195.62,1563
196.25,1569
196.87,1570
197.5,1581
198.12,1583
198.75,1591
199.37,1597
200,1601
200.62,1606
202.5,1615
203.12,1626
203.75,1625
204.37,1644
205,1646
205.62,1658
206.25,1663
206.87,1674
207.5,1685
208.12,1703
208.75,1703
209.37,1717
210,1732
210.62,1743
211.25,1750
211.87,1766
212.5,1776
213.12,1791
213.75,1808
214.37,1814
215,1835
215.62,1844
216.25,1854
216.87,1870
217.5,1892
218.12,1909
218.75,1918
219.37,1934
220,1952
220.62,1972
221.87,2023
222.5,2039
223.12,2059
223.75,2082
225,2101
225.62,2122
226.25,2148
226.87,2173
227.5,2190
228.12,2214
228.75,2241
229.37,2275
230,2295
230.62,2324
231.25,2348
231.87,2160
232.5,2416
233.12,2445
233.75,2479
234.37,2520
235.62,2607
236.25,2649
236.87,2156
237.5,2726
238.12,2768
238.75,2806
239.37,2865
240,2912
240.62,2962
241.25,3026
241.87,3078
242.5,3147
243.12,3210
243.75,3276
244.37,3315
245,3307
245.62,3288
246.25,3267
246.87,3253
247.5,3153
248.75,4678
249.37,4563
250,4560
250.62,4504
251.25,4523
251.87,4478
252.5,4452
253.12,4465
253.75,4434
254.37,4421
255,4391
255.62,4371
256.25,4361
256.87,4356
257.5,4348
258.12,4326
258.75,4326
259.37,4331
260,4341
260.62,4270
261.25,4263
261.87,4281
262.5,2992
263.12,2984
263.75,2976
264.37,2983
265,2971
265.62,2963
266.25,2963
266.87,2967
267.5,2968
268.12,2951
268.75,2951
270,2959
270.62,2953
271.25,2500
271.87,5514
272.5,2839
273.12,2706
273.75,2721
274.37,2693
288.12,3010
288.75,2999
289.37,2998
290,3020
290.62,3036
291.25,3083
291.87,3169
292.5,3170
293.12,3196
293.75,3212
294.37,3230
295,3234
295.62,3262
296.25,3273
296.87,3298
297.5,3318
298.12,3333
298.75,3356
299.37,3374
300,3394
300.62,3417
301.25,3427
301.87,3453
302.5,3474
303.12,3490
303.75,3516
304.37,3552
305,3571
305.62,3581
307.5,5224
308.12,5271
308.75,5316
309.37,5411
310,3843
310.62,3892
311.25,3907
311.87,3922
312.5,3985
313.12,4016
313.75,4058
315,4081
315.62,4143
316.25,4190
316.87,4230
317.5,4291
318.12,4353
318.75,4406
319.37,4460
320,4512
320.62,4563
321.25,4507
321.87,4473
322.5,4426
323.12,4398
323.75,4371
324.37,4321
325,4274
325.62,4256
326.25,4215
326.87,4194
327.5,4148
328.12,4104
328.75,4077
329.37,4051
330,4024
330.62,3995
331.25,3971
331.87,3932
332.5,3909
333.12,3898
333.75,3884
334.37,3858
335,3840
335.62,3818
336.25,3791
337.5,3765
338.12,3747
338.75,3720
339.37,3715
340,3689
340.62,3687
341.25,3635
341.87,3632
342.5,3624
343.12,3613
343.75,3613
344.37,3594
345,3595
345.62,3560
346.25,3570
346.87,3543
347.5,3555
348.12,3527
348.75,3512
349.37,3512
350,3521
350.62,3486
351.25,3496
351.87,3477
352.5,3487
353.12,3461
353.75,3460
354.37,3458
355,3453
355.62,3460
356.25,3448
357.5,998
358.12,3442
I have converted this data into cartesian and plotted this using Matplotlib.
After I have done the RANSAC, it shows a blue line in the picture that is not detecting the wall properly. I should find out 4 walls. I do not know which modification I should do in my code. Here is the code:
import numpy as np
from matplotlib import pyplot as plt
import pandas as pd
from sklearn import linear_model, datasets
import math
# scan data is stored in a txt file and getting data from that text file
df = pd.read_csv('scanData.txt',delimiter=',')
angle = df.values[:,0]
distance = df.values[:,1]
cartesian = [(r*math.cos(phi*math.pi/180), r*math.sin(phi*math.pi/180)) for r, phi in zip(distance, angle)]
x, y = map(list, zip(*cartesian))
#print(x)
# coverting this into 2d array
x= np.array(x)
y= np.array(y)
x=x.reshape(-1, 1)
y=y.reshape(-1, 1)
lr = linear_model.LinearRegression()
lr.fit(x, y)
ransac = linear_model.RANSACRegressor(max_trials=1000,min_samples=300)
ransac.fit(x, y)
# Predict data of estimated models
line_X = np.arange(x.min(), x.max())[:, np.newaxis]
print(line_X)
line_y = lr.predict(line_X)
line_y_ransac = ransac.predict(line_X)
print(line_y_ransac)
plt.scatter(x,y, color='yellowgreen', marker='.',
label='Inliers')
plt.plot(line_X, line_y_ransac, color='cornflowerblue', linewidth=1,
label='RANSAC regressor')
plt.legend(loc='lower right')
plt.xlabel("Input")
plt.ylabel("Response")
plt.show()
I will be so glad if you could suggest me to achieve the floor plane and surface area. If you have any questions, please drop it into the comment section.
I spent some time trying different things and manage with relative ease to get the following result. The thought i had was:
- Divide points into section.
- Use RANSAC on each section to get a line estimate.
The dividing part was done quite trivialt by comparing distance between incrementing measurements. Not that this is the part that needs to be worked more on, you can see it's flaws on the "yellow part" where two lines are estimated as one section.
The result I got was the following (note that chaning parameters will improve the result):
This is the code (note that I am not a professional programmer):
import matplotlib.pyplot as plt
import numpy as np
from sklearn import linear_model, datasets
from skimage.measure import LineModelND, ransac
import pandas as pd
import math
df = pd.read_csv('scanData.txt',delimiter=',')
angle = df.values[:,0]
distance = df.values[:,1]
cartesian = [(r*math.cos(phi*math.pi/180), r*math.sin(phi*math.pi/180)) for r, phi in zip(distance, angle)]
x, y = map(list, zip(*cartesian))
# coverting this into 2d array
x_data = np.array(x)
y_data = np.array(y)
def plot_ransac(segment_data_x, segment_data_y):
data = np.column_stack([segment_data_x, segment_data_y])
# fit line using all data
model = LineModelND()
model.estimate(data)
# robustly fit line only using inlier data with RANSAC algorithm
model_robust, inliers = ransac(data, LineModelND, min_samples=2,
residual_threshold=5, max_trials=1000)
outliers = inliers == False
# generate coordinates of estimated models
line_x = np.array([segment_data_x.min(), segment_data_x.max()])
line_y = model.predict_y(line_x)
line_y_robust = model_robust.predict_y(line_x)
k = (line_y_robust[1] - line_y_robust[0])/(line_x[1]- line_x[0])
m = line_y_robust[0] - k*line_x[0]
x0 = (segment_data_y.min() - m)/k
x1 = (segment_data_y.max() - m)/k
line_x_y = np.array([x0, x1])
line_y_robust_y = model_robust.predict_y(line_x_y)
if (distance(line_x[0], line_y_robust[0], line_x[1], line_y_robust[1]) <
distance(line_x_y[0], line_y_robust_y[0], line_x_y[1], line_y_robust_y[1])):
plt.plot(line_x, line_y_robust, '-b', label='Robust line model')
else:
plt.plot(line_x_y, line_y_robust_y, '-b', label='Robust line model')
x_segments = []
y_segments = []
def distance(x1,y1,x2,y2):
return np.sqrt((x1-x2)**2 + (y1-y2)**2)
start = 0
distances = []
for i in range(len(x_data)-1):
distance_to_point = distance(x_data[i], y_data[i], x_data[i+1], y_data[i+1])
distances.append(distance_to_point)
if distance_to_point > 200:
if i-start>10:
x_segments.append(x_data[start:i])
y_segments.append(y_data[start:i])
start = i+1
if i == len(x_data)-2:
if i-start>10:
x_segments.append(x_data[start:i])
y_segments.append(y_data[start:i])
plt.plot(x_data, y_data, '.', color = 'grey')
for x_seg, y_seg in zip(x_segments, y_segments):
plt.plot(x_seg, y_seg,'.', markersize = 10)
plot_ransac(x_seg, y_seg)
print('Line is:', distance(x_seg[0], y_seg[0],x_seg[1], y_seg[1]), 'units long')
plt.axis('equal')
plt.show()
Hope this is somewhat helpful for you.
I could not find the solution for sklearn, But thankfully there is another library from sci-image. And Ski-image detects the line properly. Here is the solution that I was looking for.
import numpy as np
import matplotlib.pyplot as plt
import pandas as pd
from skimage.measure import ransac, LineModelND, CircleModel
import math
df = pd.read_csv('scanData.txt',delimiter=',')
angle = df.values[:,0]
distance = df.values[:,1]
x= angle
y= distance
cartesian = [(r*math.cos(phi*math.pi/180), r*math.sin(phi*math.pi/180)) for r,
phi in zip(distance, angle)]
x, y = map(list, zip(*cartesian))
# coverting this into 2d array
x= np.array(x)
y= np.array(y)
x=x.reshape(-1, 1)
y=y.reshape(-1, 1)
data = np.column_stack([x, y])
model = LineModelND()
model.estimate(data)
# robustly fit line only using inlier data with RANSAC algorithm
model_robust, inliers = ransac(data, LineModelND, min_samples=2,
residual_threshold=10, max_trials=1000)
outliers = inliers == False
# generate coordinates of estimated models
line_x = np.arange(x.min(),x.max()) #[:, np.newaxis]
line_y = model.predict_y(line_x)
line_y_robust = model_robust.predict_y(line_x)
fig, ax = plt.subplots()
ax.plot(data[outliers, 0], data[outliers, 1], '.r', alpha=0.6,
label='Outlier data')
ax.plot(data[inliers, 0], data[inliers, 1], '.b', alpha=0.6,
label='Inlier data')
print("data: ", data)
print(data[inliers, 0], data[inliers, 1])
#ax.plot(line_x, line_y, '-k', label='Line model from all data')
#ax.plot(line_x, line_y_robust, '-b', label='Robust line model')
#ax.legend(loc='lower left')
plt.show()
And here is the resulting image that I am getting:
You are most welcome to modify or edit this answer. I would love to get a different answer.