-
-
Save imsrgadich/ec1b59126afd66fa18dcced00074119d to your computer and use it in GitHub Desktop.
particle filter visualization
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| # http://www.udacity-forums.com/cs373/questions/15720/display-hw3-6-particle-filter-in-a-gui?page=1&focusedAnswerId=16593#16593 | |
| # Creator: http://www.udacity-forums.com/cs373/users/1217/berthy424 | |
| # Additions by http://www.udacity-forums.com/cs373/users/149/marcello79 | |
| # Ruined by http://www.udacity-forums.com/cs373/users/485/alan | |
| # http://www.udacity-forums.com/cs373/questions/15720/display-hw3-6-particle-filter-in-a-gui/17348 | |
| import Tkinter as tk | |
| import particle_filter_hw3_6 as pf | |
| import math | |
| import random | |
| import itertools | |
| class DispParticleFilter(tk.Tk): | |
| def __init__(self, N = 100, | |
| delay = 250, displayRealRobot = True, displayGhost = False): | |
| tk.Tk.__init__(self) | |
| self.title( 'Display Particle Filter CS373-HW03.06') | |
| self.N = N | |
| self.delay = delay | |
| self.displayRealRobot = displayRealRobot | |
| self.displayGhost = displayGhost | |
| self.particle_stream = particle_stream() | |
| self.path = [] | |
| self.path_index = 0 | |
| self.init_filter() | |
| # Drawing | |
| self.grid() | |
| self.margin = 50 | |
| self.zoom_factor = 2.0 | |
| self.playing = False | |
| self.canvas = DisplayParticles(self.margin, self.zoom_factor) | |
| self.canvas.configure(bg = 'ivory', bd = 2, relief = tk.SUNKEN) | |
| self.canvas.grid(row = 0, padx = 5, pady = 5) | |
| self.canvas.draw_all(self.particles, self.robot, self.path, | |
| self.displayRealRobot, self.displayGhost) | |
| self.mousex = self.mousey = 0 | |
| self.canvas.bind('<Motion>', self.on_motion) | |
| self.canvas.bind('<B1-Motion>', self.on_drag) | |
| self.canvas.bind('<ButtonRelease-1>', self.play_or_pause) | |
| self.canvas.bind('<Button-3>', self.reset_filter) | |
| # Parameter frame | |
| self.parameterFrame = tk.LabelFrame(self, text = 'Parameters:') | |
| self.parameterFrame.grid(row = 2, padx = 5, pady = 5) | |
| self.nslider = tk.Scale(self.parameterFrame, | |
| from_ = 1, to = 1000, orient = tk.HORIZONTAL, | |
| command = self.set_n, | |
| label = 'num particles:') | |
| self.nslider.set(500) | |
| self.nslider.grid(row = 0, column = 0, padx = 5, pady = 5) | |
| self.delay_slider = tk.Scale( | |
| self.parameterFrame, from_ = 1, to = 1000, orient = tk.HORIZONTAL, | |
| command = self.set_delay, | |
| label = 'frame delay:') | |
| self.delay_slider.set(500) | |
| self.delay_slider.grid(row = 0, column = 1, padx = 5, pady = 5) | |
| self.bearing_slider = tk.Scale( | |
| self.parameterFrame, from_ = 0.01, to = 1.0, orient = tk.HORIZONTAL, | |
| command = self.set_bearing_noise, | |
| resolution = 0.01, | |
| label = 'bearing noise:') | |
| self.bearing_slider.set(0.1) | |
| self.bearing_slider.grid(row = 1, column = 0, padx = 5, pady = 5) | |
| self.steering_slider = tk.Scale( | |
| self.parameterFrame, from_ = 0, to = 1.0, orient = tk.HORIZONTAL, | |
| command = self.set_steering_noise, | |
| resolution = 0.01, | |
| label = 'steering noise:') | |
| self.steering_slider.set(0.1) | |
| self.steering_slider.grid(row = 1, column = 1, padx = 5, pady = 5) | |
| self.distance_slider = tk.Scale( | |
| self.parameterFrame, from_ = 0, to = 10.0, orient = tk.HORIZONTAL, | |
| command = self.set_distance_noise, | |
| label = 'distance noise:') | |
| self.distance_slider.set(5.0) | |
| self.distance_slider.grid(row = 1, column = 2, padx = 5, pady = 5) | |
| # Error frame | |
| self.errorFrame = tk.LabelFrame(self, text = 'Error:') | |
| self.errorFrame.grid(row = 1, padx = 5, pady = 5) | |
| self.posErrorStr = tk.StringVar() | |
| self.posErrorStr.set('Pos: N/A') | |
| self.posError = tk.Label(self.errorFrame, textvariable = self.posErrorStr) | |
| self.posError.grid(row = 0, column = 0, padx = 5, pady = 5) | |
| self.angleErrorStr = tk.StringVar() | |
| self.angleErrorStr.set('Angle: N/A') | |
| self.angleError = tk.Label(self.errorFrame, textvariable = self.angleErrorStr) | |
| self.angleError.grid(row = 0, column = 1, padx = 5, pady = 5) | |
| def on_drag(self, event): | |
| self.path.append((self.to_world(event.x), self.to_world(event.y))) | |
| self.canvas.plot_landmarks(self.path[-1:], self.canvas.path_radius, | |
| self.canvas.path_color) | |
| def set_n(self, event): | |
| self.N = int(self.nslider.get()) | |
| def set_delay(self, event): | |
| self.delay = int(self.delay_slider.get()) | |
| def set_bearing_noise(self, event): | |
| pf.bearing_noise = float(self.bearing_slider.get()) | |
| self.reset_noise() | |
| def set_steering_noise(self, event): | |
| pf.steering_noise = float(self.steering_slider.get()) | |
| self.reset_noise() | |
| def set_distance_noise(self, event): | |
| pf.distance_noise = float(self.distance_slider.get()) | |
| self.reset_noise() | |
| def reset_noise(self): | |
| self.robot.set_noise(pf.bearing_noise, pf.steering_noise, pf.distance_noise) | |
| for particle in self.particles: | |
| particle.set_noise( | |
| pf.bearing_noise, pf.steering_noise, pf.distance_noise) | |
| def reset_filter(self, event = None): | |
| self.path = [] | |
| self.path_index = 0 | |
| self.init_filter() | |
| self.canvas.draw_all(self.particles, self.robot, self.path, | |
| self.displayRealRobot, self.displayGhost) | |
| def init_filter(self): | |
| # New robot's position | |
| self.robot = pf.robot() | |
| self.robot.set_noise(pf.bearing_noise, pf.steering_noise, pf.distance_noise) | |
| # Make particles | |
| self.particles = list(itertools.islice(self.particle_stream, self.N)) | |
| def to_world(self, x): | |
| return float(x)/self.zoom_factor - self.margin | |
| def on_motion(self, event): | |
| self.mousex = self.to_world(event.x) | |
| self.mousey = self.to_world(event.y) | |
| def track_mouse(self): | |
| if self.path: | |
| for i in range(len(self.path)): | |
| x, y = self.path[self.path_index] | |
| dx = x - self.robot.x | |
| dy = y - self.robot.y | |
| dist = math.sqrt(dx**2 + dy**2) | |
| if dist > 25: | |
| break | |
| else: | |
| self.path_index = (self.path_index+1)%len(self.path) | |
| else: | |
| dx = self.mousex - self.robot.x | |
| dy = self.mousey - self.robot.y | |
| alpha = ((math.atan2(dy, dx) - self.robot.orientation + math.pi) % (2*math.pi) | |
| - math.pi) | |
| if not -pf.max_steering_angle <= alpha <= pf.max_steering_angle: | |
| alpha = math.copysign(pf.max_steering_angle, alpha) | |
| dist = min(math.sqrt(dx**2 + dy**2), 15) | |
| motion = (alpha, dist) | |
| return motion | |
| def nparticles(self, n): | |
| ''' | |
| Return a list of n particles | |
| If self.nslider has changed the value of self.N, here is our chance to | |
| update the number of particles in our simulation. | |
| ''' | |
| if len(self.particles)<n: | |
| # Add new random particles | |
| return list(itertools.islice(itertools.chain( | |
| self.particles, self.particle_stream), n)) | |
| else: | |
| # Truncate self.particles | |
| return self.particles[:n] | |
| def display_error(self): | |
| ''' | |
| Set the error Labels | |
| ''' | |
| ghost = pf.get_position(self.particles) | |
| error_x, error_y, error_orientation = compute_error(self.robot, ghost) | |
| if error_x < pf.tolerance_xy and error_y < pf.tolerance_xy: | |
| fg = 'blue' | |
| else: | |
| fg = 'red' | |
| error_xy = math.sqrt(error_x**2+error_y**2) | |
| self.posErrorStr.set('Pos: {e:.2f}'.format(e = error_xy)) | |
| self.posError.configure(fg = fg) | |
| self.angleErrorStr.set('Angle: {e:.2f}'.format(e = error_orientation)) | |
| fg = ('blue' if error_orientation < pf.tolerance_orientation else 'red') | |
| self.angleError.configure(fg = fg) | |
| def next_step(self, event = None): | |
| if self.playing: | |
| motion = self.track_mouse() | |
| # motion update (prediction) | |
| self.robot = self.robot.move(motion) | |
| self.particles = [particle.move(motion) for particle in self.particles] | |
| # measurement update | |
| Z = self.robot.sense() | |
| w = [particle.measurement_prob(Z) for particle in self.particles] | |
| # resampling | |
| self.particles = wheelchoice(self.particles, w) | |
| # resize | |
| self.particles = self.nparticles(self.N) | |
| # replot all | |
| self.display_error() | |
| self.canvas.draw_all( | |
| self.particles, self.robot, self.path, | |
| self.displayRealRobot, self.displayGhost) | |
| self.update_idletasks() | |
| self.after(self.delay, self.next_step) | |
| def play_or_pause(self, event = None): | |
| if self.playing ^ True: | |
| self.playing = True | |
| self.after(self.delay, self.next_step) | |
| else: | |
| self.playing = False | |
| class DisplayParticles(tk.Canvas): | |
| def __init__(self, margin, zoom_factor): | |
| tk.Canvas.__init__(self) | |
| self.margin = margin | |
| self.zoom_factor = zoom_factor | |
| self.larg = self.haut = (2*margin + pf.world_size) * zoom_factor | |
| self.configure(width = self.larg, height = self.haut) | |
| # Landmarks | |
| self.landmarks_radius = 2 | |
| self.landmarks_color = 'green' | |
| # Particles | |
| self.particle_radius = 1 | |
| self.particle_color = 'red' | |
| # Robot | |
| self.robot_radius = 4 | |
| self.robot_color = 'blue' | |
| self.ghost_color = None | |
| # Path | |
| self.path_radius = 1 | |
| self.path_color = 'orange' | |
| def draw_all(self, p, realRob, path, displayRealRobot, displayGhost): | |
| self.configure(bg = 'ivory', bd = 2, relief = tk.SUNKEN) | |
| self.delete(tk.ALL) | |
| self.particles = p | |
| self.plot_particles() | |
| if displayGhost: | |
| ghost = pf.get_position(self.particles) | |
| self.plot_robot(ghost[0], ghost[1], ghost[2], | |
| self.robot_radius, self.ghost_color) | |
| self.plot_landmarks(reverse_xy(pf.landmarks), | |
| self.landmarks_radius, self.landmarks_color) | |
| if displayRealRobot: | |
| self.plot_robot(realRob.x, realRob.y, realRob.orientation, | |
| self.robot_radius, self.robot_color) | |
| if path: | |
| self.plot_landmarks(path, self.path_radius, self.path_color) | |
| def to_canvas(self, x): | |
| return (self.margin + x) * self.zoom_factor | |
| def plot_landmarks(self, lms, radius, l_color): | |
| for lx, ly in lms: | |
| x0 = self.to_canvas(lx - radius) | |
| y0 = self.to_canvas(ly - radius) | |
| x1 = self.to_canvas(lx + radius) | |
| y1 = self.to_canvas(ly + radius) | |
| self.create_oval(x0, y0, x1, y1, fill = l_color) | |
| def plot_particles(self): | |
| for particle in self.particles: | |
| self.draw_particle(particle, self.particle_radius, self.particle_color) | |
| def draw_particle(self, particle, radius, p_color, cos = math.cos, sin = math.sin): | |
| x2 = self.to_canvas(particle.x) | |
| y2 = self.to_canvas(particle.y) | |
| x3 = self.to_canvas(particle.x + 2*radius*cos(particle.orientation)) | |
| y3 = self.to_canvas(particle.y + 2*radius*sin(particle.orientation)) | |
| self.create_line(x2, y2, x3, y3, fill = p_color, width = self.zoom_factor, | |
| arrow = tk.LAST, arrowshape = (2*self.zoom_factor, | |
| 3*self.zoom_factor, | |
| 1*self.zoom_factor)) | |
| def plot_robot(self, x, y, orientation, radius, r_color, | |
| cos = math.cos, sin = math.sin): | |
| x0 = self.to_canvas(x - radius) | |
| y0 = self.to_canvas(y - radius) | |
| x1 = self.to_canvas(x + radius) | |
| y1 = self.to_canvas(y + radius) | |
| self.create_oval(x0, y0, x1, y1, fill = r_color) | |
| x2 = self.to_canvas(x) | |
| y2 = self.to_canvas(y) | |
| x3 = self.to_canvas(x + 2*radius*cos(orientation)) | |
| y3 = self.to_canvas(y + 2*radius*sin(orientation)) | |
| self.create_line(x2, y2, x3, y3, | |
| fill = r_color, width = self.zoom_factor, arrow = tk.LAST) | |
| def compute_error(final_robot, estimated_position): | |
| error_x = abs(final_robot.x - estimated_position[0]) | |
| error_y = abs(final_robot.y - estimated_position[1]) | |
| error_orientation = abs(final_robot.orientation - estimated_position[2]) | |
| error_orientation = (error_orientation + math.pi) % (2.0 * math.pi) - math.pi | |
| return error_x, error_y, error_orientation | |
| def particle_stream(): | |
| while True: | |
| r = pf.robot() | |
| r.set_noise(pf.bearing_noise, pf.steering_noise, pf.distance_noise) | |
| yield r | |
| def wheelchoice(p, weights, n = None): | |
| assert len(p) == len(weights) | |
| if n is None: | |
| n = len(p) | |
| result = [] | |
| index = random.randrange(n) | |
| beta = 0.0 | |
| mw = max(weights) | |
| for i in range(n): | |
| beta += random.uniform(0, 2.0*mw) | |
| while beta > weights[index]: | |
| beta -= weights[index] | |
| index = (index + 1) % n | |
| result.append(p[index]) | |
| return result | |
| def reverse_xy(path): | |
| for x, y in path: | |
| yield y, x | |
| if __name__ == "__main__": | |
| wind = DispParticleFilter(N = 500, displayRealRobot = True, displayGhost = True) | |
| wind.mainloop() |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment