Skip to content

Commit eb4e9af

Browse files
committed
First working copy of the LIDAR to 2D grid map example
1 parent ac21135 commit eb4e9af

File tree

5 files changed

+434
-0
lines changed

5 files changed

+434
-0
lines changed
279 KB
Loading
372 KB
Loading
Lines changed: 174 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,174 @@
1+
"""
2+
3+
LIDAR to 2D grid map example
4+
5+
author: Erno Horvath, Csaba Hajdu based on Atsushi Sakai's scripts (@Atsushi_twi)
6+
7+
"""
8+
9+
import math
10+
import numpy as np
11+
import cv2
12+
import matplotlib.pyplot as plt
13+
from math import cos, sin, radians, pi
14+
from scipy.ndimage.morphology import binary_closing, binary_fill_holes
15+
16+
EXTEND_AREA = 1.0
17+
18+
def file_read(f):
19+
"""
20+
Reading LIDAR laser beams (angles and corresponding distance data)
21+
"""
22+
measures = [line.split(",") for line in open(f)]
23+
angles = []
24+
distances = []
25+
for measure in measures:
26+
angles.append(float(measure[0]))
27+
distances.append(float(measure[1]))
28+
angles = np.array(angles)
29+
distances = np.array(distances)
30+
return angles, distances
31+
32+
def bresenham(start, end):
33+
"""
34+
Implementation of Bresenham's line drawing algorithm
35+
See en.wikipedia.org/wiki/Bresenham's_line_algorithm
36+
Bresenham's Line Algorithm
37+
Produces a np.array from start and end (original from roguebasin.com)
38+
>>> points1 = bresenham((4, 4), (6, 10))
39+
>>> print(points1)
40+
np.array([[4,4], [4,5], [5,6], [5,7], [5,8], [6,9], [6,10]])
41+
"""
42+
# setup initial conditions
43+
x1, y1 = start
44+
x2, y2 = end
45+
dx = x2 - x1
46+
dy = y2 - y1
47+
is_steep = abs(dy) > abs(dx) # determine how steep the line is
48+
if is_steep: # rotate line
49+
x1, y1 = y1, x1
50+
x2, y2 = y2, x2
51+
swapped = False # swap start and end points if necessary and store swap state
52+
if x1 > x2:
53+
x1, x2 = x2, x1
54+
y1, y2 = y2, y1
55+
swapped = True
56+
dx = x2 - x1 # recalculate differentials
57+
dy = y2 - y1 # recalculate differentials
58+
error = int(dx / 2.0) # calculate error
59+
ystep = 1 if y1 < y2 else -1
60+
# iterate over bounding box generating points between start and end
61+
y = y1
62+
points = []
63+
for x in range(x1, x2 + 1):
64+
coord = [y, x] if is_steep else (x, y)
65+
points.append(coord)
66+
error -= abs(dy)
67+
if error < 0:
68+
y += ystep
69+
error += dx
70+
if swapped: # reverse the list if the coordinates were swapped
71+
points.reverse()
72+
points = np.array(points)
73+
return points
74+
75+
def calc_grid_map_config(ox, oy, xyreso):
76+
"""
77+
Calculates the size, and the maximum distances according to the the measurement center
78+
"""
79+
minx = round(min(ox) - EXTEND_AREA / 2.0)
80+
miny = round(min(oy) - EXTEND_AREA / 2.0)
81+
maxx = round(max(ox) + EXTEND_AREA / 2.0)
82+
maxy = round(max(oy) + EXTEND_AREA / 2.0)
83+
xw = int(round((maxx - minx) / xyreso))
84+
yw = int(round((maxy - miny) / xyreso))
85+
print("The grid map is ", xw, "x", yw, ".")
86+
return minx, miny, maxx, maxy, xw, yw
87+
88+
def atan_zero_to_twopi(y, x):
89+
angle = math.atan2(y, x)
90+
if angle < 0.0:
91+
angle += math.pi * 2.0
92+
return angle
93+
94+
def generate_ray_casting_grid_map(ox, oy, xyreso, yawreso, breshen = True):
95+
"""
96+
The breshen boolean tells if it's computed with bresenham ray casting (True) or with flood fill (False)
97+
"""
98+
minx, miny, maxx, maxy, xw, yw = calc_grid_map_config(ox, oy, xyreso)
99+
pmap = np.ones((xw, yw))/2 # default 0.5 -- [[0.5 for i in range(yw)] for i in range(xw)]
100+
centix = int(round(-minx / xyreso)) # center x coordinate of the grid map
101+
centiy = int(round(-miny / xyreso)) # center y coordinate of the grid map
102+
#print(centix, centiy)
103+
prev_ix, prev_iy = centix - 1, centiy
104+
# occupancy grid computed with bresenham ray casting
105+
if breshen:
106+
for (x, y) in zip(ox, oy):
107+
d = math.sqrt(x**2 + y**2)
108+
angle = atan_zero_to_twopi(y, x)
109+
angleid = int(math.floor(angle / yawreso))
110+
ix = int(round((x - minx) / xyreso)) # x coordinte of the the occupied area
111+
iy = int(round((y - miny) / xyreso)) # y coordinte of the the occupied area
112+
laser_beams = bresenham((centix, centiy), (ix, iy)) # line form the lidar to the cooupied point
113+
for laser_beam in laser_beams:
114+
pmap[laser_beam[0]][laser_beam[1]] = 0.0 # free area 0.0
115+
pmap[ix][iy] = 1.0 # occupied area 1.0
116+
pmap[ix+1][iy] = 1.0 # extend the occupied area
117+
pmap[ix][iy+1] = 1.0 # extend the occupied area
118+
pmap[ix+1][iy+1] = 1.0 # extend the occupied area
119+
# occupancy grid computed with with flood fill
120+
else:
121+
pmap = (np.ones((xw, yw), dtype=np.uint8)) * 5 # food fill does not work with float numbers such as 0.5; so 5 is the default and later it is divided by 10
122+
for (x, y) in zip(ox, oy):
123+
ix = int(round((x - minx) / xyreso)) # x coordinte of the the occupied area
124+
iy = int(round((y - miny) / xyreso)) # y coordinte of the the occupied area
125+
free_area = bresenham((prev_ix, prev_iy), (ix, iy))
126+
for fa in free_area:
127+
pmap[fa[0]][fa[1]] = 0 # free area 0.0
128+
prev_ix = ix
129+
prev_iy = iy
130+
cv2.floodFill(pmap, None, (centix, centiy), 0) # filling the free spaces with 0, stating from the center
131+
pmap = np.array(pmap, dtype=np.float)
132+
pmap /= 10
133+
for (x, y) in zip(ox, oy):
134+
ix = int(round((x - minx) / xyreso))
135+
iy = int(round((y - miny) / xyreso))
136+
pmap[ix][iy] = 1.0 # occupied area 1.0
137+
pmap[ix+1][iy] = 1.0 # extend the occupied area
138+
pmap[ix][iy+1] = 1.0 # extend the occupied area
139+
pmap[ix+1][iy+1] = 1.0 # extend the occupied area
140+
return pmap, minx, maxx, miny, maxy, xyreso
141+
142+
def main():
143+
"""
144+
Example usage
145+
"""
146+
print(__file__, "start")
147+
xyreso = 0.02 # x-y grid resolution
148+
yawreso = math.radians(3.1) # yaw angle resolution [rad]
149+
ang, dist = file_read("lidar01.csv")
150+
ox = np.sin(ang) * dist
151+
oy = np.cos(ang) * dist
152+
pmap, minx, maxx, miny, maxy, xyreso = generate_ray_casting_grid_map(ox, oy, xyreso, yawreso, True)
153+
xyres = np.array(pmap).shape
154+
plt.figure(1, figsize=(10,4))
155+
plt.subplot(122)
156+
plt.imshow(pmap, cmap = "PiYG_r") # cmap = "binary" "PiYG_r" "PiYG_r" "bone" "bone_r" "RdYlGn_r"
157+
plt.clim(-0.4, 1.4)
158+
plt.gca().set_xticks(np.arange(-.5, xyres[1], 1), minor = True)
159+
plt.gca().set_yticks(np.arange(-.5, xyres[0], 1), minor = True)
160+
plt.grid(True, which="minor", color="w", linewidth = .6, alpha = 0.5)
161+
plt.colorbar()
162+
plt.subplot(121)
163+
plt.plot([oy, np.zeros(np.size(oy))], [ox, np.zeros(np.size(oy))], "ro-")
164+
plt.axis("equal")
165+
plt.plot(0.0, 0.0, "ob")
166+
plt.gca().set_aspect("equal", "box")
167+
bottom, top = plt.ylim() # return the current ylim
168+
plt.ylim((top, bottom)) # rescale y axis, to match the grid orientation
169+
plt.grid(True)
170+
plt.show()
171+
172+
if __name__ == '__main__':
173+
main()
174+

Mapping/lidar_to_grid_map/lidar_to_grid_map_tutorial.ipynb

Lines changed: 253 additions & 0 deletions
Large diffs are not rendered by default.

README.md

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,7 @@ Python codes for robotics algorithm.
2020
* [Mapping](#mapping)
2121
* [Gaussian grid map](#gaussian-grid-map)
2222
* [Ray casting grid map](#ray-casting-grid-map)
23+
* [Lidar to grid map](#lidar-to-grid-map)
2324
* [k-means object clustering](#k-means-object-clustering)
2425
* [SLAM](#slam)
2526
* [Iterative Closest Point (ICP) Matching](#iterative-closest-point-icp-matching)
@@ -177,6 +178,12 @@ This is a 2D ray casting grid mapping example.
177178

178179
![2](https://github.com/AtsushiSakai/PythonRobotics/raw/master/Mapping/raycasting_grid_map/animation.gif)
179180

181+
## Lidar to grid map
182+
183+
This example shows how to convert a 2D range measurement to a grid map.
184+
185+
![2](Mapping/lidar_to_grid_map/animation.gif)
186+
180187
## k-means object clustering
181188

182189
This is a 2D object clustering with k-means algorithm.

0 commit comments

Comments
 (0)