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)