Text

function PF = generate_initial_PF(fov_hor, fov_ver, min_depth, max_depth, number_particles)
    particles_top = floor(number_particles * 0.75);
    particles_bottom = number_particles - particles_top;
    depth_slice = (max_depth - min_depth) / 2 + min_depth;

    depth_bottom = min_depth + (depth_slice - min_depth) * rand(particles_bottom, 1);
    depth_top = depth_slice + (max_depth - depth_slice) * rand(particles_top, 1);

    depth = [depth_bottom; depth_top];

    PF = zeros(number_particles, 3);

    for i = 1:number_particles
        PF(i, 1) = - tand(fov_hor / 2) * depth(i, 1) + ( 2 * tand(fov_hor / 2) * depth(i, 1)) * rand(1,1);
        PF(i, 2) = - tand(fov_ver / 2) * depth(i, 1) + ( 2 * tand(fov_ver / 2) * depth(i, 1)) * rand(1,1);
        PF(i, 3) = depth(i, 1);
    end

    scatter3(PF(:,3), PF(:,2), PF(:,1),'.');

    %{
    scatter(PF(:,1), PF(:,3), '.');
    hold on;
    x = min_depth:max_depth;
    y = tand(fov_hor / 2)*x;
    plot(y, x, 'k');
    y2 = -tand(fov_hor / 2)*x;
    plot(y2, x, 'k');

    figure;

    scatter(PF(:,2), PF(:,3), '.');
    hold on;
    x = min_depth:max_depth;
    y3 = tand(fov_ver / 2)*x;
    plot(y3, x, 'k');
    y4 = -tand(fov_ver / 2)*x;
    plot(y4, x, 'k');

    figure;

    x = 1:number_particles;
    scatter(x, PF(:,3), '.');
    %}
end

camera_x = (ball_u - camera_info.K[2]) * camera_z / camera_info.K[0]
 		camera_y = (ball_v - camera_info.K[5]) * camera_z / camera_info.K[4]

 		camera_x = -camera_x
 		camera_y = -camera_y

 		# From camera to robot
 		camera_trans_x = -0.125
 		camera_trans_y = 0.0
 		camera_trans_z = 0.25

 		robot_x = camera_z + camera_trans_x
 		robot_y = camera_x + camera_trans_y
 		robot_z = camera_y + camera_trans_z

 		# From robot to world
 		theta = 2*math.atan2(odom.pose.pose.orientation.z, odom.pose.pose.orientation.w)
 		world_x = math.cos(-theta)*robot_x - math.sin(-theta)*robot_y
 		world_y = math.sin(-theta)*robot_x + math.cos(-theta)*robot_y
 		world_z = robot_z

 		world_x = world_x + odom.pose.pose.position.x
 		world_y = world_y + odom.pose.pose.position.y


def create_uniform_particles(num_particles, fov_vertical=62, fov_horizontal=48.6, max_depth=3.0, min_depth=0.9):
    particles = np.empty((num_particles, 3))
    particles[:, 2] = uniform(min_depth, max_depth, size=num_particles)
    particles[:, 0] = -tan(fov_vertical / 2) * particles[:, 2] + (2 * tan(fov_vertical / 2) * particles[:, 2]) * uniform(0, 1)
    particles[:, 1] = -tan(fov_horizontal / 2) * particles[:, 2] + (2 * tan (fov_vertical / 2) * particles[:, 2]) * uniform(0, 1)

    return particles

# TODO: Como prever o próximo estado da bola?
def predict(particles, u, std, dt=1.0):
    return particles

def update(particles, weights, z, R, landmarks):
    for i, landmark in enumerate(landmarks):
        distance = np.linalg.norm(particles - landmark, axis=1)
        weights *= scipy.stats.norm(distance, R).pdf(z[i])

    weights += 1.e-300
    weights /= sum(weights)

def estimate(particles, weights):
    pos = particles
    mean = np.average(pos, weights=weights, axis=0)
    var = np.average((pos-mean)**2, weights=weights, axis=0)

    return mean, var


num_particles = 500
landmarks = np.array([[1.4, 2.2, 2.8], [1.5, 1.8, 2], [0.4, -1, 1.5]])
nl = len(landmarks)
sensor_std_err = .1

# Cálculo do Aamir
robot_number = 0
zs = []
for r in range(0, num_robots):
    if r != robot_number:
        zs.append(confidence)

particles = create_uniform_particles(num_particles)
weights = np.ones(num_particles)

predict(particles, u=(0.0, 0.0), std=(0.1, 0.1))

update(particles, weights, z=zs, R=sensor_std_err, landmarks=landmarks)

mean, var = estimate(particles, weights)
print(mean)
print(var)