diff --git a/Localization/histogram_filter/histogram_filter.py b/Localization/histogram_filter/histogram_filter.py index 1e43fee8..f7696e6b 100644 --- a/Localization/histogram_filter/histogram_filter.py +++ b/Localization/histogram_filter/histogram_filter.py @@ -11,12 +11,13 @@ author: Atsushi Sakai (@Atsushi_twi) """ -import math -import numpy as np -import matplotlib.pyplot as plt import copy -from scipy.stats import norm +import math + +import matplotlib.pyplot as plt +import numpy as np from scipy.ndimage import gaussian_filter +from scipy.stats import norm # Parameters EXTEND_AREA = 10.0 # [m] grid map extention length @@ -41,11 +42,11 @@ NOISE_SPEED = 0.5 # [m/s] 1σ speed noise parameter show_animation = True -class grid_map(): +class GridMap: def __init__(self): self.data = None - self.xyreso = None + self.xy_reso = None self.minx = None self.miny = None self.maxx = None @@ -117,7 +118,7 @@ def motion_model(x, u): def draw_heatmap(data, mx, my): maxp = max([max(igmap) for igmap in data]) - plt.pcolor(mx, my, data, vmax=maxp, cmap=plt.cm.Blues) + plt.pcolor(mx, my, data, vmax=maxp, cmap=plt.cm.get_cmap("Blues")) plt.axis("equal") @@ -157,18 +158,17 @@ def normalize_probability(gmap): def init_gmap(xyreso, minx, miny, maxx, maxy): + gmap = GridMap() - gmap = grid_map() - - gmap.xyreso = xyreso + gmap.xy_reso = xyreso gmap.minx = minx gmap.miny = miny gmap.maxx = maxx gmap.maxy = maxy - gmap.xw = int(round((gmap.maxx - gmap.minx) / gmap.xyreso)) - gmap.yw = int(round((gmap.maxy - gmap.miny) / gmap.xyreso)) + gmap.xw = int(round((gmap.maxx - gmap.minx) / gmap.xy_reso)) + gmap.yw = int(round((gmap.maxy - gmap.miny) / gmap.xy_reso)) - gmap.data = [[1.0 for i in range(gmap.yw)] for i in range(gmap.xw)] + gmap.data = [[1.0 for _ in range(gmap.yw)] for _ in range(gmap.xw)] gmap = normalize_probability(gmap) return gmap @@ -183,7 +183,7 @@ def map_shift(gmap, xshift, yshift): nix = ix + xshift niy = iy + yshift - if nix >= 0 and nix < gmap.xw and niy >= 0 and niy < gmap.yw: + if 0 <= nix < gmap.xw and 0 <= niy < gmap.yw: gmap.data[ix + xshift][iy + yshift] = tgmap[ix][iy] return gmap