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]))
