First working copy of the LIDAR to 2D grid map example

Mapping/lidar_to_grid_map/lidar_to_grid_map.py

@@ -0,0 +1,174 @@
+"""
+
+LIDAR to 2D grid map example
+
+author: Erno Horvath, Csaba Hajdu based on Atsushi Sakai's scripts (@Atsushi_twi)
+
+"""
+
+import math
+import numpy as np
+import cv2
+import matplotlib.pyplot as plt
+from math import cos, sin, radians, pi
+from scipy.ndimage.morphology import binary_closing, binary_fill_holes
+
+EXTEND_AREA = 1.0
+
+def file_read(f):
+    """
+    Reading LIDAR laser beams (angles and corresponding distance data)
+    """
+    measures = [line.split(",") for line in open(f)]
+    angles = []
+    distances = []
+    for measure in measures:
+        angles.append(float(measure[0]))
+        distances.append(float(measure[1]))
+    angles = np.array(angles)
+    distances = np.array(distances)
+    return angles, distances
+
+def bresenham(start, end):
+    """
+    Implementation of Bresenham's line drawing algorithm
+    See en.wikipedia.org/wiki/Bresenham's_line_algorithm
+    Bresenham's Line Algorithm
+    Produces a np.array from start and end (original from roguebasin.com)
+    >>> points1 = bresenham((4, 4), (6, 10))
+    >>> print(points1)
+    np.array([[4,4], [4,5], [5,6], [5,7], [5,8], [6,9], [6,10]])
+    """
+    # setup initial conditions
+    x1, y1 = start
+    x2, y2 = end
+    dx = x2 - x1
+    dy = y2 - y1
+    is_steep = abs(dy) > abs(dx) # determine how steep the line is
+    if is_steep: # rotate line
+        x1, y1 = y1, x1
+        x2, y2 = y2, x2
+    swapped = False # swap start and end points if necessary and store swap state
+    if x1 > x2:
+        x1, x2 = x2, x1
+        y1, y2 = y2, y1
+        swapped = True
+    dx = x2 - x1 # recalculate differentials
+    dy = y2 - y1 # recalculate differentials
+    error = int(dx / 2.0) # calculate error
+    ystep = 1 if y1 < y2 else -1
+    # iterate over bounding box generating points between start and end
+    y = y1
+    points = []
+    for x in range(x1, x2 + 1):
+        coord = [y, x] if is_steep else (x, y)
+        points.append(coord)   
+        error -= abs(dy)
+        if error < 0:
+            y += ystep
+            error += dx
+    if swapped: # reverse the list if the coordinates were swapped
+        points.reverse()
+    points = np.array(points)
+    return points
+
+def calc_grid_map_config(ox, oy, xyreso):
+    """
+    Calculates the size, and the maximum distances according to the the measurement center
+    """
+    minx = round(min(ox) - EXTEND_AREA / 2.0)
+    miny = round(min(oy) - EXTEND_AREA / 2.0)
+    maxx = round(max(ox) + EXTEND_AREA / 2.0)
+    maxy = round(max(oy) + EXTEND_AREA / 2.0)
+    xw = int(round((maxx - minx) / xyreso))
+    yw = int(round((maxy - miny) / xyreso))
+    print("The grid map is ", xw, "x", yw, ".")
+    return minx, miny, maxx, maxy, xw, yw
+
+def atan_zero_to_twopi(y, x):
+    angle = math.atan2(y, x)
+    if angle < 0.0:
+        angle += math.pi * 2.0
+    return angle
+
+def generate_ray_casting_grid_map(ox, oy, xyreso, yawreso, breshen = True):
+    """
+    The breshen boolean tells if it's computed with bresenham ray casting (True) or with flood fill (False)
+    """
+    minx, miny, maxx, maxy, xw, yw = calc_grid_map_config(ox, oy, xyreso)
+    pmap = np.ones((xw, yw))/2 # default 0.5 -- [[0.5 for i in range(yw)] for i in range(xw)] 
+    centix = int(round(-minx / xyreso)) # center x coordinate of the grid map
+    centiy = int(round(-miny / xyreso)) # center y coordinate of the grid map
+    #print(centix, centiy)
+    prev_ix, prev_iy = centix - 1, centiy
+    # occupancy grid computed with bresenham ray casting
+    if breshen:
+        for (x, y) in zip(ox, oy):
+            d = math.sqrt(x**2 + y**2)
+            angle = atan_zero_to_twopi(y, x)
+            angleid = int(math.floor(angle / yawreso))
+            ix = int(round((x - minx) / xyreso)) # x coordinte of the the occupied area
+            iy = int(round((y - miny) / xyreso)) # y coordinte of the the occupied area
+            laser_beams = bresenham((centix, centiy), (ix, iy)) # line form the lidar to the cooupied point
+            for laser_beam in laser_beams:
+                pmap[laser_beam[0]][laser_beam[1]] = 0.0 # free area 0.0
+            pmap[ix][iy] = 1.0     # occupied area 1.0
+            pmap[ix+1][iy] = 1.0   # extend the occupied area
+            pmap[ix][iy+1] = 1.0   # extend the occupied area
+            pmap[ix+1][iy+1] = 1.0 # extend the occupied area
+    # occupancy grid computed with with flood fill
+    else:
+        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
+        for (x, y) in zip(ox, oy):        
+            ix = int(round((x - minx) / xyreso)) # x coordinte of the the occupied area
+            iy = int(round((y - miny) / xyreso)) # y coordinte of the the occupied area
+            free_area = bresenham((prev_ix, prev_iy), (ix, iy))
+            for fa in free_area:
+                pmap[fa[0]][fa[1]] = 0 # free area 0.0                  
+            prev_ix = ix
+            prev_iy = iy
+        cv2.floodFill(pmap, None, (centix, centiy), 0) # filling the free spaces with 0, stating from the center
+        pmap = np.array(pmap, dtype=np.float)
+        pmap /= 10
+        for (x, y) in zip(ox, oy):        
+            ix = int(round((x - minx) / xyreso))
+            iy = int(round((y - miny) / xyreso))
+            pmap[ix][iy] = 1.0     # occupied area 1.0
+            pmap[ix+1][iy] = 1.0   # extend the occupied area
+            pmap[ix][iy+1] = 1.0   # extend the occupied area
+            pmap[ix+1][iy+1] = 1.0 # extend the occupied area
+    return pmap, minx, maxx, miny, maxy, xyreso
+
+def main():
+    """
+    Example usage
+    """
+    print(__file__, "start")
+    xyreso = 0.02  # x-y grid resolution
+    yawreso = math.radians(3.1)  # yaw angle resolution [rad]
+    ang, dist = file_read("lidar01.csv")
+    ox = np.sin(ang) * dist
+    oy = np.cos(ang) * dist
+    pmap, minx, maxx, miny, maxy, xyreso = generate_ray_casting_grid_map(ox, oy, xyreso, yawreso, True)
+    xyres = np.array(pmap).shape
+    plt.figure(1, figsize=(10,4))
+    plt.subplot(122)
+    plt.imshow(pmap, cmap = "PiYG_r") # cmap = "binary" "PiYG_r" "PiYG_r" "bone" "bone_r" "RdYlGn_r"
+    plt.clim(-0.4, 1.4)
+    plt.gca().set_xticks(np.arange(-.5, xyres[1], 1), minor = True)
+    plt.gca().set_yticks(np.arange(-.5, xyres[0], 1), minor = True)
+    plt.grid(True, which="minor", color="w", linewidth = .6, alpha = 0.5)
+    plt.colorbar()
+    plt.subplot(121)
+    plt.plot([oy, np.zeros(np.size(oy))], [ox, np.zeros(np.size(oy))], "ro-")
+    plt.axis("equal")
+    plt.plot(0.0, 0.0, "ob")
+    plt.gca().set_aspect("equal", "box")
+    bottom, top = plt.ylim()  # return the current ylim
+    plt.ylim((top, bottom)) # rescale y axis, to match the grid orientation
+    plt.grid(True)
+    plt.show()
+        
+if __name__ == '__main__':
+    main()     
+        

README.md

@@ -20,6 +20,7 @@ Python codes for robotics algorithm.
    * [Mapping](#mapping)
       * [Gaussian grid map](#gaussian-grid-map)
       * [Ray casting grid map](#ray-casting-grid-map)
+      * [Lidar to grid map](#lidar-to-grid-map)	  
       * [k-means object clustering](#k-means-object-clustering)
    * [SLAM](#slam)
       * [Iterative Closest Point (ICP) Matching](#iterative-closest-point-icp-matching)
@@ -177,6 +178,12 @@ This is a 2D ray casting grid mapping example.
 
 ![2](https://github.com/AtsushiSakai/PythonRobotics/raw/master/Mapping/raycasting_grid_map/animation.gif)
 
+## Lidar to grid map
+
+This example shows how to convert a 2D range measurement to a grid map.
+
+![2](Mapping/lidar_to_grid_map/animation.gif)
+
 ## k-means object clustering
 
 This is a 2D object clustering with k-means algorithm.