2d Particle filter example with Visualization

pf.py

# Make a robot called myrobot that starts at
# coordinates 30, 50 heading north (pi/2).
# Have your robot turn clockwise by pi/2, move
# 15 m, and sense. Then have it turn clockwise
# by pi/2 again, move 10 m, and sense again.
#
# Your program should print out the result of
# your two sense measurements.
#
# Don't modify the code below. Please enter
# your code at the bottom.

from math import *
import random
import cv2
import numpy as np


landmarks  = [[20.0, 20.0], [280.0, 280.0], [320.0, 80.0], [80.0, 320.0]]
world_size = 500.0

class drawing:
    def __init__(self):
        self.drawMap = np.zeros((world_size , world_size , 3))

    def drawLandmarks(self):
        for landmark in landmarks:
            cv2.circle(self.drawMap,(int(landmark[0]),int(landmark[1])), 5, (0,0,255), -1)

    def clearMap(self):
        self.drawMap = np.zeros((world_size , world_size , 3))
        self.drawLandmarks()

    def showMap(self, timeout):
        expandedMap = cv2.resize(self.drawMap, (0,0), fx=500/world_size, fy=500/world_size)
        cv2.imshow("map",expandedMap)
        cv2.waitKey(timeout)        

    def drawParticle(self, x, y, orientation):
        self.draw_arrow(self.drawMap, (int(x),int(y)),orientation,10,(0,255,0))

    def draw_arrow(self, image, p, angle, mag, color, arrow_magnitude=9, thickness=1, line_type=8, shift=0):
        q=[0,0]
        q[0] = p[0] + mag * np.cos(angle);
        q[1] = p[1] + mag * np.sin(angle);
        q=(int(q[0]),int(q[1]))
        cv2.line(image, p, q, color, thickness, line_type, shift)
        angle = np.arctan2(p[1]-q[1], p[0]-q[0])
        p = (int(q[0] + arrow_magnitude * np.cos(angle + np.pi/4)),
        int(q[1] + arrow_magnitude * np.sin(angle + np.pi/4)))
        cv2.line(image, p, q, color, thickness, line_type, shift)
        p = (int(q[0] + arrow_magnitude * np.cos(angle - np.pi/4)),
        int(q[1] + arrow_magnitude * np.sin(angle - np.pi/4)))
        cv2.line(image, p, q, color, thickness, line_type, shift)

    def draw_arrow_vector(self, image, p, vector, color, magnitude=1):
        dx = vector[0]*magnitude
        dy = vector[1]*magnitude
        rads = np.arctan2(-dy,dx)
        degs = np.degrees(rads)
        dist = np.sqrt( (dx)**2 + (dy)**2 )
        self.draw_arrow(image, p, np.radians(degs+180), dist*100, color)

class robot:
    def __init__(self, x=None, y=None, orientation=None):
        if x is None:
            self.x = random.random() * world_size
        else:
            self.x = x
        if y is None:
            self.y = random.random() * world_size
        else:
            self.y = y
        if orientation is None:
            self.orientation = random.random() * 2.0 * pi
        else:
            self.orientation = orientation
        self.forward_noise = 0.0;
        self.turn_noise    = 0.0;
        self.sense_noise   = 0.0;
    
    def set(self, new_x, new_y, new_orientation):
        if new_x < 0 or new_x >= world_size:
            raise ValueError, 'X coordinate out of bound'
        if new_y < 0 or new_y >= world_size:
            raise ValueError, 'Y coordinate out of bound'
        if new_orientation < 0 or new_orientation >= 2 * pi:
            raise ValueError, 'Orientation must be in [0..2pi]'
        self.x = float(new_x)
        self.y = float(new_y)
        self.orientation = float(new_orientation)
    
    
    def set_noise(self, new_f_noise, new_t_noise, new_s_noise):
        # makes it possible to change the noise parameters
        # this is often useful in particle filters
        self.forward_noise = float(new_f_noise);
        self.turn_noise    = float(new_t_noise);
        self.sense_noise   = float(new_s_noise);
    
    
    def sense(self):
        Z = []
        for i in range(len(landmarks)):
            dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2)
            dist += random.gauss(0.0, self.sense_noise)
            Z.append(dist)
        return Z
    
    
    def move(self, turn, forward):
        if forward < 0:
            raise ValueError, 'Robot cant move backwards'         
        
        # turn, and add randomness to the turning command
        orientation = self.orientation + float(turn) + random.gauss(0.0, self.turn_noise)
        orientation %= 2 * pi
        
        # move, and add randomness to the motion command
        dist = float(forward) + random.gauss(0.0, self.forward_noise)
        x = self.x + (cos(orientation) * dist)
        y = self.y + (sin(orientation) * dist)
        x %= world_size    # cyclic truncate
        y %= world_size
        
        # set particle
        res = robot()
        res.set(x, y, orientation)
        res.set_noise(self.forward_noise, self.turn_noise, self.sense_noise)
        return res
    
    def Gaussian(self, mu, sigma, x):
        
        # calculates the probability of x for 1-dim Gaussian with mean mu and var. sigma
        return exp(- ((mu - x) ** 2) / (sigma ** 2) / 2.0) / sqrt(2.0 * pi * (sigma ** 2))
    
    
    def measurement_prob(self, measurement):
        
        # calculates how likely a measurement should be
        
        prob = 1.0;
        for i in range(len(landmarks)):
            dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2)
            prob *= self.Gaussian(dist, self.sense_noise, measurement[i])
        return prob
    
    
    
    def __repr__(self):
        return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y), str(self.orientation))



def eval(r, p):
    sum = 0.0;
    for i in range(len(p)): # calculate mean error
        dx = (p[i].x - r.x + (world_size/2.0)) % world_size - (world_size/2.0)
        dy = (p[i].y - r.y + (world_size/2.0)) % world_size - (world_size/2.0)
        err = sqrt(dx * dx + dy * dy)
        sum += err
    return sum / float(len(p))



####   DON'T MODIFY ANYTHING ABOVE HERE! ENTER CODE BELOW ####



N = 500
T = 100

myrobot = robot(x=world_size/2, y=world_size/2, orientation=0)
mydraw = drawing()

p = []
for i in range(N):
    r = robot()
    r.set_noise(0.05, 0.05, 105.0) # forward, turn, measure
    p.append(r)
    mydraw.drawParticle(p[i].x, p[i].y, p[i].orientation)
mydraw.showMap(100)
mydraw.clearMap()

for t in range(T):
    myrobot = myrobot.move(0.1, 5.0)
    Z = myrobot.sense()

    p2 = []
    for i in range(N):
        p2.append(p[i].move(0.1, 5.0))
    p = p2

    w = []
    for i in range(N):
        w.append(p[i].measurement_prob(Z))

    p3 = []
    index = int(random.random()*N)
    beta = 0.0
    mw =  max(w)
    for i in range(N):
        beta += random.random() * 2.0 * mw
        while beta > w[index]:
            beta -= w[index]
            index = (index + 1) % N
        p3.append(p[index])
    p = p3

    for i in range(N):
        mydraw.drawParticle(p[i].x, p[i].y, p[i].orientation)
    mydraw.showMap(30)
    mydraw.clearMap()

    print eval(myrobot, p)
    if eval(myrobot, p) > 15.0:
        for i in range(N):
            print '#', i, p[i]
        print 'R', myrobot

# print p

while(1):
    cv2.waitKey(15)








# myrobot = robot()
# myrobot.set_noise(5.0,0.1,5.0) # forward, turn, measure
# mydraw = drawing()

# myrobot.set(30.0, 50.0, pi/2)
# mydraw.draw(myrobot.x, myrobot.y, myrobot.orientation)
# myrobot = myrobot.move(-pi/2, 15.0)
# print myrobot.sense()
# mydraw.draw(myrobot.x, myrobot.y, myrobot.orientation)
# myrobot = myrobot.move(-pi/2, 10.0)
# print myrobot.sense()
# mydraw.draw(myrobot.x, myrobot.y, myrobot.orientation)

#################

# mydraw = drawing()

# N = 1000
# p = []
# for i in range(N):
#     x = robot()
#     p.append(x)
#     mydraw.drawParticle(x.x, x.y, x.orientation)

# mydraw.showMap(100)
# mydraw.clearMap()

# for m in range(100):

#     p2 = []
#     for i in range(N):
#         p2.append(p[i].move(0.1,5.0))
#         mydraw.drawParticle(p2[i].x, p2[i].y, p2[i].orientation)
#     p = p2

#     mydraw.showMap(30)
#     mydraw.clearMap()

#################

# mydraw = drawing()
# myrobot = robot()
# myrobot = myrobot.move(0.1,5.0)
# Z = myrobot.sense()

# N = 1000
# p = []
# for i in range(N):
#     x = robot()
#     x.set_noise(0.05, 0.05, 5.0)
#     p.append(x)
#     mydraw.drawParticle(x.x, x.y, x.orientation)
# mydraw.showMap(100)
# mydraw.clearMap()

# p2 = []
# for i in range(N):
#     p2.append(p[i].move(0.1,5.0))
#     mydraw.drawParticle(p2[i].x, p2[i].y, p2[i].orientation)
# p = p2
# mydraw.showMap(100)
# mydraw.clearMap()

# w = []
# for i in range(N):
#     w.append(p[i].measurement_prob(Z))

# p3 = []
# index = int(random.random()*N) # Start at a random index on the wheel
# beta = 0.0
# mw = max(w)
# for i in range(N):
#     beta += random.random()*2.0*mw # new beta is some uniform dist of twice of max w
#     while beta > w[index]:
#         beta -= w[index]
#         index = (index + 1) % N
#     p3.append(p[index]) # Assign index that i chose from sampling
#     mydraw.drawParticle(p3[i].x, p3[i].y, p3[i].orientation)
# p = p3
# mydraw.showMap(100)
# mydraw.clearMap()
# for particle in p:
#     print particle

#################