|
| 1 | + |
| 2 | +This simple tutorial shows how to read LIDAR (range) measurements from a |
| 3 | +file and convert it to occupancy grid. |
| 4 | + |
| 5 | +Occupancy grid maps (*Hans Moravec, A.E. Elfes: High resolution maps |
| 6 | +from wide angle sonar, Proc. IEEE Int. Conf. Robotics Autom. (1985)*) |
| 7 | +are a popular, probabilistic approach to represent the environment. The |
| 8 | +grid is basically discrete representation of the environment, which |
| 9 | +shows if a grid cell is occupied or not. Here the map is represented as |
| 10 | +a ``numpy array``, and numbers close to 1 means the cell is occupied |
| 11 | +(*marked with red on the next image*), numbers close to 0 means they are |
| 12 | +free (*marked with green*). The grid has the ability to represent |
| 13 | +unknown (unobserved) areas, which are close to 0.5. |
| 14 | + |
| 15 | +.. figure:: lidar_to_grid_map_tutorial_files/grid_map_example.png |
| 16 | + :alt: Example |
| 17 | + |
| 18 | +In order to construct the grid map from the measurement we need to |
| 19 | +discretise the values. But, first let’s need to ``import`` some |
| 20 | +necessary packages. |
| 21 | + |
| 22 | +.. code:: ipython3 |
| 23 | +
|
| 24 | + import math |
| 25 | + import numpy as np |
| 26 | + import matplotlib.pyplot as plt |
| 27 | + from math import cos, sin, radians, pi |
| 28 | +
|
| 29 | +The measurement file contains the distances and the corresponding angles |
| 30 | +in a ``csv`` (comma separated values) format. Let’s write the |
| 31 | +``file_read`` method: |
| 32 | + |
| 33 | +.. code:: ipython3 |
| 34 | +
|
| 35 | + def file_read(f): |
| 36 | + """ |
| 37 | + Reading LIDAR laser beams (angles and corresponding distance data) |
| 38 | + """ |
| 39 | + measures = [line.split(",") for line in open(f)] |
| 40 | + angles = [] |
| 41 | + distances = [] |
| 42 | + for measure in measures: |
| 43 | + angles.append(float(measure[0])) |
| 44 | + distances.append(float(measure[1])) |
| 45 | + angles = np.array(angles) |
| 46 | + distances = np.array(distances) |
| 47 | + return angles, distances |
| 48 | +
|
| 49 | +From the distances and the angles it is easy to determine the ``x`` and |
| 50 | +``y`` coordinates with ``sin`` and ``cos``. In order to display it |
| 51 | +``matplotlib.pyplot`` (``plt``) is used. |
| 52 | + |
| 53 | +.. code:: ipython3 |
| 54 | +
|
| 55 | + ang, dist = file_read("lidar01.csv") |
| 56 | + ox = np.sin(ang) * dist |
| 57 | + oy = np.cos(ang) * dist |
| 58 | + plt.figure(figsize=(6,10)) |
| 59 | + plt.plot([oy, np.zeros(np.size(oy))], [ox, np.zeros(np.size(oy))], "ro-") # lines from 0,0 to the |
| 60 | + plt.axis("equal") |
| 61 | + bottom, top = plt.ylim() # return the current ylim |
| 62 | + plt.ylim((top, bottom)) # rescale y axis, to match the grid orientation |
| 63 | + plt.grid(True) |
| 64 | + plt.show() |
| 65 | +
|
| 66 | +
|
| 67 | +
|
| 68 | +.. image:: lidar_to_grid_map_tutorial_files/lidar_to_grid_map_tutorial_5_0.png |
| 69 | + |
| 70 | + |
| 71 | +The ``lidar_to_grid_map.py`` contains handy functions which can used to |
| 72 | +convert a 2D range measurement to a grid map. For example the |
| 73 | +``bresenham`` gives the a straight line between two points in a grid |
| 74 | +map. Let’s see how this works. |
| 75 | + |
| 76 | +.. code:: ipython3 |
| 77 | +
|
| 78 | + import lidar_to_grid_map as lg |
| 79 | + map1 = np.ones((50, 50)) * 0.5 |
| 80 | + line = lg.bresenham((2, 2), (40, 30)) |
| 81 | + for l in line: |
| 82 | + map1[l[0]][l[1]] = 1 |
| 83 | + plt.imshow(map1) |
| 84 | + plt.colorbar() |
| 85 | + plt.show() |
| 86 | +
|
| 87 | +
|
| 88 | +
|
| 89 | +.. image:: lidar_to_grid_map_tutorial_files/lidar_to_grid_map_tutorial_7_0.png |
| 90 | + |
| 91 | + |
| 92 | +.. code:: ipython3 |
| 93 | +
|
| 94 | + line = lg.bresenham((2, 30), (40, 30)) |
| 95 | + for l in line: |
| 96 | + map1[l[0]][l[1]] = 1 |
| 97 | + line = lg.bresenham((2, 30), (2, 2)) |
| 98 | + for l in line: |
| 99 | + map1[l[0]][l[1]] = 1 |
| 100 | + plt.imshow(map1) |
| 101 | + plt.colorbar() |
| 102 | + plt.show() |
| 103 | +
|
| 104 | +
|
| 105 | +
|
| 106 | +.. image:: lidar_to_grid_map_tutorial_files/lidar_to_grid_map_tutorial_8_0.png |
| 107 | + |
| 108 | + |
| 109 | +To fill empty areas, a queue-based algorithm can be used that can be |
| 110 | +used on an initialized occupancy map. The center point is given: the |
| 111 | +algorithm checks for neighbour elements in each iteration, and stops |
| 112 | +expansion on obstacles and free boundaries. |
| 113 | + |
| 114 | +.. code:: ipython3 |
| 115 | +
|
| 116 | + from collections import deque |
| 117 | + def flood_fill(cpoint, pmap): |
| 118 | + """ |
| 119 | + cpoint: starting point (x,y) of fill |
| 120 | + pmap: occupancy map generated from Bresenham ray-tracing |
| 121 | + """ |
| 122 | + # Fill empty areas with queue method |
| 123 | + sx, sy = pmap.shape |
| 124 | + fringe = deque() |
| 125 | + fringe.appendleft(cpoint) |
| 126 | + while fringe: |
| 127 | + n = fringe.pop() |
| 128 | + nx, ny = n |
| 129 | + # West |
| 130 | + if nx > 0: |
| 131 | + if pmap[nx - 1, ny] == 0.5: |
| 132 | + pmap[nx - 1, ny] = 0.0 |
| 133 | + fringe.appendleft((nx - 1, ny)) |
| 134 | + # East |
| 135 | + if nx < sx - 1: |
| 136 | + if pmap[nx + 1, ny] == 0.5: |
| 137 | + pmap[nx + 1, ny] = 0.0 |
| 138 | + fringe.appendleft((nx + 1, ny)) |
| 139 | + # North |
| 140 | + if ny > 0: |
| 141 | + if pmap[nx, ny - 1] == 0.5: |
| 142 | + pmap[nx, ny - 1] = 0.0 |
| 143 | + fringe.appendleft((nx, ny - 1)) |
| 144 | + # South |
| 145 | + if ny < sy - 1: |
| 146 | + if pmap[nx, ny + 1] == 0.5: |
| 147 | + pmap[nx, ny + 1] = 0.0 |
| 148 | + fringe.appendleft((nx, ny + 1)) |
| 149 | +
|
| 150 | +This algotihm will fill the area bounded by the yellow lines starting |
| 151 | +from a center point (e.g. (10, 20)) with zeros: |
| 152 | + |
| 153 | +.. code:: ipython3 |
| 154 | +
|
| 155 | + flood_fill((10, 20), map1) |
| 156 | + map_float = np.array(map1)/10.0 |
| 157 | + plt.imshow(map1) |
| 158 | + plt.colorbar() |
| 159 | + plt.show() |
| 160 | +
|
| 161 | +
|
| 162 | +
|
| 163 | +.. image:: lidar_to_grid_map_tutorial_files/lidar_to_grid_map_tutorial_12_0.png |
| 164 | + |
| 165 | + |
| 166 | +Let’s use this flood fill on real data: |
| 167 | + |
| 168 | +.. code:: ipython3 |
| 169 | +
|
| 170 | + xyreso = 0.02 # x-y grid resolution |
| 171 | + yawreso = math.radians(3.1) # yaw angle resolution [rad] |
| 172 | + ang, dist = file_read("lidar01.csv") |
| 173 | + ox = np.sin(ang) * dist |
| 174 | + oy = np.cos(ang) * dist |
| 175 | + pmap, minx, maxx, miny, maxy, xyreso = lg.generate_ray_casting_grid_map(ox, oy, xyreso, False) |
| 176 | + xyres = np.array(pmap).shape |
| 177 | + plt.figure(figsize=(20,8)) |
| 178 | + plt.subplot(122) |
| 179 | + plt.imshow(pmap, cmap = "PiYG_r") |
| 180 | + plt.clim(-0.4, 1.4) |
| 181 | + plt.gca().set_xticks(np.arange(-.5, xyres[1], 1), minor = True) |
| 182 | + plt.gca().set_yticks(np.arange(-.5, xyres[0], 1), minor = True) |
| 183 | + plt.grid(True, which="minor", color="w", linewidth = .6, alpha = 0.5) |
| 184 | + plt.colorbar() |
| 185 | + plt.show() |
| 186 | +
|
| 187 | +
|
| 188 | +.. parsed-literal:: |
| 189 | +
|
| 190 | + The grid map is 150 x 100 . |
| 191 | +
|
| 192 | +
|
| 193 | +
|
| 194 | +.. image:: lidar_to_grid_map_tutorial_files/lidar_to_grid_map_tutorial_14_1.png |
| 195 | + |
0 commit comments