particle filter visualization

berthy.py

# 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()