From 1f2cef56840264f2744b81fb03dfff053e1d4437 Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Sun, 18 Feb 2018 09:28:40 -0800
Subject: [PATCH] gaussian grid map released

---
 .../gaussian_grid_map/gaussian_grid_map.py    | 123 ++++--------------
 1 file changed, 22 insertions(+), 101 deletions(-)

diff --git a/Mapping/gaussian_grid_map/gaussian_grid_map.py b/Mapping/gaussian_grid_map/gaussian_grid_map.py
index 154c8fc2..3e9d0ece 100644
--- a/Mapping/gaussian_grid_map/gaussian_grid_map.py
+++ b/Mapping/gaussian_grid_map/gaussian_grid_map.py
@@ -1,6 +1,6 @@
 """
 
-2D grid map sample
+2D gaussian grid map sample
 
 author: Atsushi Sakai (@Atsushi_twi)
 
@@ -11,17 +11,16 @@ import numpy as np
 import matplotlib.pyplot as plt
 from scipy.stats import norm
 
-AREA_WIDTH = 10.0
+EXTEND_AREA = 10.0  # [m] grid map extention length
 
-STD = 10.0  # standard diviation
+show_animation = True
 
 
-def generate_gaussian_grid_map(ox, oy, xyreso):
+def generate_gaussian_grid_map(ox, oy, xyreso, std):
 
     minx, miny, maxx, maxy, xw, yw = calc_grid_map_config(ox, oy, xyreso)
 
-    # calc each potential
-    pmap = [[0.0 for i in range(yw)] for i in range(xw)]
+    gmap = [[0.0 for i in range(yw)] for i in range(xw)]
 
     for ix in range(xw):
         for iy in range(yw):
@@ -36,104 +35,23 @@ def generate_gaussian_grid_map(ox, oy, xyreso):
                 if mindis >= d:
                     mindis = d
 
-            pdf = (1.0 - norm.cdf(mindis, 0.0, STD))
-            pmap[ix][iy] = pdf
+            pdf = (1.0 - norm.cdf(mindis, 0.0, std))
+            gmap[ix][iy] = pdf
 
-    draw_heatmap(pmap, minx, maxx, miny, maxy, xyreso)
-    plt.plot(ox, oy, "xr")
-    plt.plot(0.0, 0.0, "ob")
+    return gmap, minx, maxx, miny, maxy
 
 
 def calc_grid_map_config(ox, oy, xyreso):
-    minx = round(min(ox) - AREA_WIDTH / 2.0)
-    miny = round(min(oy) - AREA_WIDTH / 2.0)
-    maxx = round(max(ox) + AREA_WIDTH / 2.0)
-    maxy = round(max(oy) + AREA_WIDTH / 2.0)
+    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))
 
     return minx, miny, maxx, maxy, xw, yw
 
 
-class precastDB:
-
-    def __init__(self):
-        self.px = 0.0
-        self.py = 0.0
-        self.d = 0.0
-        self.angle = 0.0
-        self.ix = 0
-        self.iy = 0
-
-    def __str__(self):
-        return str(self.px) + "," + str(self.py) + "," + str(self.d) + "," + str(self.angle)
-
-
-def precasting(minx, miny, xw, yw, xyreso, yawreso):
-
-    precast = [[] for i in range(round((math.pi * 2.0) / yawreso) + 1)]
-
-    for ix in range(xw):
-        for iy in range(yw):
-            px = ix * xyreso + minx
-            py = iy * xyreso + miny
-
-            d = math.sqrt(px**2 + py**2)
-            angle = math.atan2(py, px)
-            if angle < 0.0:
-                angle += math.pi * 2.0
-
-            angleid = math.floor(angle / yawreso)
-
-            pc = precastDB()
-
-            pc.px = px
-            pc.py = py
-            pc.d = d
-            pc.ix = ix
-            pc.iy = iy
-            pc.angle = angle
-
-            precast[angleid].append(pc)
-
-    return precast
-
-
-def generate_ray_casting_grid_map(ox, oy, xyreso):
-
-    minx, miny, maxx, maxy, xw, yw = calc_grid_map_config(ox, oy, xyreso)
-
-    pmap = [[0.0 for i in range(yw)] for i in range(xw)]
-
-    yawreso = math.radians(10.0)
-
-    precast = precasting(minx, miny, xw, yw, xyreso, yawreso)
-
-    for (x, y) in zip(ox, oy):
-
-        d = math.sqrt(x**2 + y**2)
-        angle = math.atan2(y, x)
-        if angle < 0.0:
-            angle += math.pi * 2.0
-
-        angleid = math.floor(angle / yawreso)
-
-        gridlist = precast[angleid]
-
-        ix = int(round((x - minx) / xyreso))
-        iy = int(round((y - miny) / xyreso))
-
-        for grid in gridlist:
-            if grid.d > (d):
-                pmap[grid.ix][grid.iy] = 0.5
-
-        pmap[ix][iy] = 1.0
-
-    draw_heatmap(pmap, minx, maxx, miny, maxy, xyreso)
-    plt.plot(ox, oy, "xr")
-    plt.plot(0.0, 0.0, "ob")
-
-
 def draw_heatmap(data, minx, maxx, miny, maxy, xyreso):
     x, y = np.mgrid[slice(minx - xyreso / 2.0, maxx + xyreso / 2.0, xyreso),
                     slice(miny - xyreso / 2.0, maxy + xyreso / 2.0, xyreso)]
@@ -144,18 +62,21 @@ def draw_heatmap(data, minx, maxx, miny, maxy, xyreso):
 def main():
     print(__file__ + " start!!")
 
-    xyreso = 0.5
+    xyreso = 0.5  # xy grid resolution
+    STD = 5.0  # standard diviation for gaussian distribution
 
     for i in range(5):
         ox = (np.random.rand(4) - 0.5) * 10.0
         oy = (np.random.rand(4) - 0.5) * 10.0
-        plt.cla()
-        generate_gaussian_grid_map(ox, oy, xyreso)
-        plt.pause(1.0)
+        gmap, minx, maxx, miny, maxy = generate_gaussian_grid_map(
+            ox, oy, xyreso, STD)
 
-        plt.cla()
-        generate_ray_casting_grid_map(ox, oy, xyreso)
-        plt.pause(1.0)
+        if show_animation:
+            plt.cla()
+            draw_heatmap(gmap, minx, maxx, miny, maxy, xyreso)
+            plt.plot(ox, oy, "xr")
+            plt.plot(0.0, 0.0, "ob")
+            plt.pause(1.0)
 
 
 if __name__ == '__main__':