Esempio n. 1
0
 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
Esempio n. 2
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)
    
    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)'
Esempio n. 3
0
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]))