Merge pull request AtsushiSakai#198 from horverno/master

Lidar measurement to grid map

Author: AtsushiSakai

Mapping/lidar_to_grid_map/animation.gif

@@ -0,0 +1,154 @@
+0.008450416037156572,0.5335
+0.046902201120156306,0.5345
+0.08508127850753233,0.537
+0.1979822644959155,0.2605
+0.21189035697274505,0.2625
+0.2587960806200922,0.26475
+0.3043382657893199,0.2675
+0.34660795861105775,0.27075
+0.43632879047139106,0.59
+0.4739624524675188,0.60025
+0.5137777760286397,0.611
+0.5492297764597742,0.6265
+0.5895905154121426,0.64
+0.6253152235389017,0.6565
+0.6645851317087743,0.676
+0.6997644244442851,0.6975
+0.7785769484796541,0.3345
+0.7772134100015329,0.74575
+0.8652979956881222,0.3315
+0.8996591653367609,0.31775
+0.9397471965935056,0.31275
+0.9847439663714841,0.31125
+1.0283771976713423,0.31325
+1.0641019057981014,0.31975
+1.1009174447073562,0.3335
+1.2012738766970301,0.92275
+1.2397256617800307,0.95325
+1.2779047391674068,0.9865
+1.316629231946031,1.01775
+1.3561718478115274,1.011
+1.3948963405901518,1.0055
+1.4330754179775278,1.00225
+1.4731634492342724,0.99975
+1.5113425266216485,0.9975
+1.5517032655740168,1.001
+1.5896096352657691,1.00275
+1.6288795434356418,1.008
+1.6684221593011381,1.0135
+1.7066012366885142,1.022
+1.7453257294671385,1.02875
+1.7862318838107551,0.9935
+1.8257744996762515,1.0025
+1.8661352386286207,0.96075
+1.9045870237116205,0.92125
+1.9465840088377355,0.8855
+1.986944747790103,0.85725
+2.025669240568728,0.832
+2.065757271825472,0.80675
+2.1066634261690886,0.78875
+2.1464787497302105,0.7705
+2.1865667809869542,0.75625
+2.2261093968524506,0.74475
+2.2683790896741876,0.68275
+2.3090125363221823,0.6375
+2.3510095214482956,0.59925
+2.3916429680962885,0.5665
+2.4330945378311526,0.538
+2.4783640153047557,0.50825
+2.5203610004308707,0.4875
+2.562903400948233,0.46825
+2.599173524466238,0.45
+2.642806755766097,0.4355
+2.685076448587836,0.42275
+2.722437402888339,0.4125
+2.766888757275069,0.40125
+2.8007045115324587,0.39525
+2.841883373571701,0.385
+2.8819714048284446,0.3805
+2.922332143780814,0.38575
+2.9637837135156797,0.38425
+3.0005992524249336,0.36575
+3.0401418682904318,0.3765
+3.079957191851552,0.3915
+3.115409192282687,0.408
+3.154679100452558,0.4265
+3.184949654666836,0.447
+3.2242195628367085,0.4715
+3.2574899017028507,0.49875
+3.2959416867858504,0.52875
+3.3292120256519926,0.564
+3.3665729799524957,0.6055
+3.4031158111661277,0.6515
+3.438022396206014,0.70675
+3.4756560582021407,0.771
+3.513562427893893,0.77075
+3.5522869206725183,0.7785
+3.621827383056667,0.79575
+3.65918833735717,0.8045
+3.697367414744546,0.81725
+3.7377281536969154,0.83325
+3.775634523388667,0.8415
+3.8135408930804187,0.85575
+3.8522653858590425,0.87325
+3.8898990478551703,0.88725
+3.9299870791119154,0.906
+3.9665299103255465,0.9265
+4.006072526191043,0.94575
+4.043978895882795,0.97175
+4.081885265574547,1.02075
+4.1206097583531704,1.046
+4.1587888357405465,1.07025
+4.196422497736674,1.097
+4.234874282819675,1.12575
+4.286688744988257,0.73475
+4.324322406984384,0.72225
+4.364410438241129,0.731
+4.405862007975994,0.7405
+4.44267754688525,0.749
+4.484129116620115,0.76025
+4.522853609398739,0.76825
+4.560759979090491,0.77125
+4.5989390564778665,0.77725
+4.640117918517108,0.782
+4.679115118991357,0.78425
+4.717294196378733,0.789
+4.757109519939853,0.78825
+4.796652135805349,0.7855
+4.8337403824102285,0.786
+4.871646752101981,0.78275
+4.9109166602718535,0.7785
+4.950186568441726,0.7635
+4.990274599698471,0.74725
+5.028180969390222,0.737
+5.0677235852557185,0.72575
+5.109720570381833,0.71525
+5.149263186247329,0.70625
+5.1863514328522085,0.69975
+5.230530079543315,0.693
+5.269799987713188,0.68925
+5.307979065100563,0.68425
+5.347248973270435,0.68275
+5.386518881440308,0.68075
+5.426606912697053,0.68175
+5.465604113171301,0.67825
+5.502419652080556,0.6835
+5.543871221815422,0.6885
+5.580959468420302,0.67925
+5.624319992024535,0.6555
+5.660044700151294,0.639
+5.700950854494911,0.623
+5.740220762664784,0.6075
+5.783581286269018,0.59475
+5.820124117482649,0.58475
+5.861848394913139,0.57325
+5.899209349213642,0.565
+5.938751965079138,0.55525
+5.9782945809446355,0.55175
+6.017564489114507,0.546
+6.059288766544997,0.5405
+6.097467843932373,0.53825
+6.139464829058487,0.534
+6.176825783358991,0.5325
+6.215822983833238,0.53125
+6.252911230438118,0.53075

Mapping/lidar_to_grid_map/grid_map_example.png

@@ -0,0 +1,220 @@
+"""
+
+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 matplotlib.pyplot as plt
+from collections import deque
+
+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 init_floodfill(cpoint,  opoints, xypoints, mincoord, xyreso):
+    """
+    cpoint: center point
+    opoints: detected obstacles points (x,y)
+    xypoints: (x,y) point pairs
+    """
+    centix, centiy = cpoint
+    prev_ix, prev_iy = centix - 1, centiy
+    ox, oy = opoints
+    xw, yw = xypoints
+    minx, miny = mincoord
+    pmap = (np.ones((xw, yw))) * 0.5
+    for (x, y) in zip(ox, oy):
+        ix = int(round((x - minx) / xyreso))  # x coordinate of the the occupied area
+        iy = int(round((y - miny) / xyreso))  # y coordinate 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
+    return pmap
+
+
+def flood_fill(cpoint, pmap):
+    """
+    cpoint: starting point (x,y) of fill
+    pmap: occupancy map generated from Bresenham ray-tracing
+    """
+    # Fill empty areas with queue method
+    sx, sy = pmap.shape
+    fringe = deque()
+    fringe.appendleft(cpoint)
+    while fringe:
+        n = fringe.pop()
+        nx, ny = n
+        # West
+        if nx > 0:
+            if pmap[nx - 1, ny] == 0.5:
+                pmap[nx - 1, ny] = 0.0
+                fringe.appendleft((nx - 1, ny))
+        # East
+        if nx < sx - 1:
+            if pmap[nx + 1, ny] == 0.5:
+                pmap[nx + 1, ny] = 0.0
+                fringe.appendleft((nx + 1, ny))
+        # North
+        if ny > 0:
+            if pmap[nx, ny - 1] == 0.5:
+                pmap[nx, ny - 1] = 0.0
+                fringe.appendleft((nx, ny - 1))
+        # South
+        if ny < sy - 1:
+            if pmap[nx, ny + 1] == 0.5:
+                pmap[nx, ny + 1] = 0.0
+                fringe.appendleft((nx, ny + 1))
+
+
+def generate_ray_casting_grid_map(ox, oy, xyreso, 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
+    # occupancy grid computed with bresenham ray casting
+    if breshen:
+        for (x, y) in zip(ox, oy):
+            ix = int(round((x - minx) / xyreso)) # x coordinate of the the occupied area
+            iy = int(round((y - miny) / xyreso)) # y coordinate 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 = init_floodfill((centix, centiy), (ox, oy), (xw, yw),  (minx, miny), xyreso)
+        flood_fill((centix, centiy), pmap)
+        pmap = np.array(pmap, dtype=np.float)
+        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
+    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, 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=0.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()