def do_sampling_step(self, ballness, new_time):
"""
Importance sampling step of Bootstrap Filter.
See http://www.cs.ubc.ca/%7Earnaud/doucet_defreitas_gordon_smcbookintro.ps (page 11).
"""
likelihood = numpy.exp(ballness)
(w, h) = likelihood.shape
transform = numpy.dot(transformation.rectangle_to_pixels(w, h), transformation.scale(1.0/metrics.FIELD_WIDTH, 1.0/metrics.FIELD_HEIGHT))
for particle in self.particles:
# Evolve particles
if self.time is None:
timedelta = 0.0
else:
timedelta = new_time - self.time
particle.evolve(timedelta, self.model_settings)
# Update weights
particle.update_weight(likelihood, transform, self.model_settings)
print self.particles
# Normalize weights
weights_sum = sum([particle.weight for particle in self.particles])
for particle in self.particles:
particle.weight /= weights_sum
import cv2
from particlefilter import *
import transformation
import metrics
def generate_ballness(w, h):
return numpy.array([random.random() for i in xrange(w*h)]).reshape((w,h))
pf = ParticleFilter()
w = 500
h = 1000
time = 0.0
transform = numpy.dot(transformation.scale(metrics.FIELD_WIDTH, metrics.FIELD_HEIGHT), transformation.rectangle_to_pixels(w, h))
while True:
ballness = generate_ballness(w, h)
img = numpy.array([[[x,x,x] for x in row] for row in ballness]).reshape((w,h,3))
time += 0.5
n = pf.settings.num_particles
# Sampling step
pf.do_sampling_step(ballness, time)
for particle in pf.particles:
if particle.is_present():
point = tuple([int(x) for x in transformation.apply_projectivity(transform, particle.position)])
cv2.circle(img=img, center=point, radius=15, color=(2.0/n - particle.weight, 2.0/n - particle.weight, 1.0), thickness=3)
