|
| 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 | + |
0 commit comments