def update_weight(self, likelihood, transform, model_settings): """ Update the importance weight, according to what is observed. """ if self.is_present(): transformed_point = transformation.apply_projectivity(transform, self.position).reshape((2,1)) self.weight = ndimage.map_coordinates(likelihood, transformed_point)[0] # Points outside the image get likelihood=0.0 else: self.weight = 1 - model_settings.presence_probability
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) print 'Particles at time', time, '(after sampling step)' for particle in pf.particles: print particle # Selection step pf.do_selection_step() 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=10, color=(0.0, 1.0, 0.0), thickness=3) print 'Particles at time', time, '(after selection step)'
def track_ball(block): ball = block.ball n = ball.particles rec = block.rectification w, h = block.table.ground.size # TODO: move this elsewhere p_appear = 5.0 / block.frame_rate p_disappear = 5.0 / block.frame_rate ball_speed_sigma = 10.0 # m/s ball_volatility = ball_speed_sigma / block.frame_rate for f in xrange(block.frames): prev_location = np.empty_like(ball.particle_location[f]) prev_location[:, 0] = np.random.uniform(-w / 2, +w / 2, n) prev_location[:, 1] = np.random.uniform(-h / 2, +h / 2, n) prev_present = np.full_like(ball.particle_present[f], False) if f > 0: prev_present[:] = ball.particle_present[f - 1] prev_location[prev_present, :] = ball.particle_location[f - 1, prev_present] location = prev_location + np.random.normal(0, ball_volatility, (n, 2)) present = np.where(prev_present, np.random.rand(n) > p_disappear, np.random.rand(n) < p_appear) # print present image_location = transformation.apply_projectivity(rec.transform, location).transpose() lweight = np.where(prev_present, sp.ndimage.map_coordinates(ball.llr[-1, f], image_location[::-1]), 0) # Points outside the image get likelihood=0.0 # FIXME: this should not be needed lweight[np.isnan(lweight)] = -np.inf maximum = np.max(lweight).astype(np.float64) normalized = lweight - maximum exp = np.exp(normalized) weight = n * (exp / np.sum(exp)) * 1.1 # Produce 10% more particles and then drop them, to avoid errors due to roundings for i in xrange(0, n, 10): if present[i]: point = tuple([int(x) for x in image_location[:, i]]) #color = (2.0/n - particle.weight, 2.0/n - particle.weight, 1.0) cv2.circle(img=ball.filter_debug[f], center=point, radius=0, color=np.array([1, 1, 1]) * weight[i]) # resampling cum_samples = np.ceil(np.cumsum(weight)).astype(np.int) samples = np.concatenate((cum_samples[0:1], np.diff(cum_samples))) ball.particle_location[f] = np.repeat(location, samples, axis=0)[:n] ball.particle_present[f] = np.repeat(present, samples, axis=0)[:n] ball.particle_parent[f] = np.repeat(np.mgrid[0:n], samples, axis=0)[:n] # we start from any particle at the end and get its path backward particle = 0 for f in xrange(block.frames - 1, 0, -1): ball.map_location[f] = ball.particle_location[f, particle] ball.map_present[f] = ball.particle_present[f, particle] particle = ball.particle_parent[f, particle] ball.path_debug[...] = 0 for f in xrange(block.frames): if not ball.map_present[f]: continue image_location = transformation.apply_projectivity(rec.transform, ball.map_location[f]).transpose() point = tuple([int(x) for x in image_location]) cv2.circle(ball.path_debug[f], center=point, radius=1, color=np.array([1, 1, 1]))