예제 #1
0
def draw_world(est_pose, particles, world):
    """Visualization.
    This functions draws robots position in the world."""

    offset = 100;

    world[:] = CWHITE # Clear background to white

    # Find largest weight
    max_weight = 0
    for particle in particles:
        max_weight = max(max_weight, particle.getWeight())

    # Draw particles
    for particle in particles:
        x = int(particle.getX()) + offset
        y = int(particle.getY()) + offset
        colour = jet(particle.getWeight() / max_weight)
        cv2.circle(world, (x,y), 2, colour, 2)
        b = (int(particle.getX() + 15.0*np.cos(particle.getTheta()))+offset,
                                     int(particle.getY() - 15.0*np.sin(particle.getTheta()))+offset)
        cv2.line(world, (x,y), b, colour, 2)

    # Draw landmarks
    lm0 = (landmarks[0][0]+offset, landmarks[0][1]+offset)
    lm1 = (landmarks[1][0]+offset, landmarks[1][1]+offset)
    cv2.circle(world, lm0, 5, CRED, 2)
    cv2.circle(world, lm1, 5, CGREEN, 2)

    # Draw estimated robot pose
    a = (int(est_pose.getX())+offset, int(est_pose.getY())+offset)
    b = (int(est_pose.getX() + 15.0*np.cos(est_pose.getTheta()))+offset,
                                 int(est_pose.getY() - 15.0*np.sin(est_pose.getTheta()))+offset)
    cv2.circle(world, a, 5, CMAGENTA, 2)
    cv2.line(world, a, b, CMAGENTA, 2)
예제 #2
0
def sample_resample(particles, resample_num, landmark_location, measured_angle, measured_distance):
    print(landmark_location)
    # Adding uncertainty to the particles 10
    # particle.add_uncertainty_von_mises(particles, 0, 0.02)
    particle.add_uncertainty(particles, 10, 0.08)

    # Calculating weights for angles
    particleAngles = [particle.getTheta() for particle in particles]

    e_l = [((landmark_location[0] - particle.getX()) , (landmark_location[1] - particle.getY())) / np.linalg.norm([(particle.getX() - landmark_location[0]), (particle.getY() - landmark_location[1])]) for particle in particles]

    e_theta = [np.array((np.cos(angle),np.sin(angle))) for angle in particleAngles]
    phi = [np.arccos(np.dot(x,y)) for x, y in zip(e_l, e_theta)]
    e_hat_theta = [(-np.sin(angle),np.cos(angle)) for angle in particleAngles]

    particle_angle_diff_temp = [np.dot(x,y) for x,y in zip(e_l,e_hat_theta)]
    particle_angle_diff = [np.sign(x) * y for x,y in zip(particle_angle_diff_temp, phi)]

    angleWeights = calcWeights(particle_angle_diff, measured_angle, 0.08)


    #print ("angle weights after: {}".format(angleWeights))

    # Weights for distance
    particleLengths = [np.linalg.norm([(particle.getX() - landmark_location[0]), (particle.getY() - landmark_location[1])]) for particle in particles]

    # Var should be roughly 50
    lengthWeights = calcWeights(particleLengths, measured_distance, 20)
    #print ("length weights after: {}".format(lengthWeights))

    # Combining the weight types
    '''
    normWeights = []
    for i in range(resample_num):
        normWeights.append(angleWeights[i] * lengthWeights[i])
    '''
    normWeights = [x * y for x, y in zip(angleWeights, lengthWeights)]

    normWeights = normWeights/np.sum(normWeights)

    # Setting the weight for each particle
    for i in range(resample_num):
        particles[i].setWeight(normWeights[i])

    # Resampling
    # Chooses resample_num random particles from the current particles based on the calculated weights.
    temp = np.random.choice(particles, resample_num, True, normWeights)
    retVal = []
    for elem in temp:
        retVal.append(copy.deepcopy(elem))

    return retVal
예제 #3
0
def draw_world(est_pose, particles, world):
    """Visualization.
    This functions draws robots position in the world coordinate system."""

    # Fix the origin of the coordinate system
    offsetX = 100
    offsetY = 250

    # Constant needed for transforming from world coordinates to screen coordinates (flip the y-axis)
    ymax = world.shape[0]

    world[:] = CWHITE  # Clear background to white

    # Find largest weight
    max_weight = 0
    for particle in particles:
        max_weight = max(max_weight, particle.getWeight())

    # Draw particles
    for particle in particles:
        x = int(particle.getX() + offsetX)
        y = ymax - (int(particle.getY() + offsetY))
        colour = jet(particle.getWeight() / max_weight)
        cv2.circle(world, (x, y), 2, colour, 2)
        b = (int(particle.getX() + 15.0 * np.cos(particle.getTheta())) +
             offsetX, ymax -
             (int(particle.getY() + 15.0 * np.sin(particle.getTheta())) +
              offsetY))
        cv2.line(world, (x, y), b, colour, 2)

    # Draw landmarks
    lm1 = (int(landmarks[0][0]), int(ymax - (landmarks[0][1])))
    lm2 = (int(landmarks[1][0]), int(ymax - (landmarks[1][1])))
    lm3 = (int(landmarks[2][0]), int(ymax - (landmarks[2][1])))
    lm4 = (int(landmarks[3][0]), int(ymax - (landmarks[3][1])))
    cv2.circle(world, lm1, 5, CRED, 2)
    cv2.circle(world, lm2, 5, CGREEN, 2)
    cv2.circle(world, lm3, 5, CGREEN, 2)
    cv2.circle(world, lm4, 5, CRED, 2)

    # Draw estimated robot pose
    a = (int(est_pose.getX()) + offsetX,
         ymax - (int(est_pose.getY()) + offsetY))
    b = (int(est_pose.getX() + 15.0 * np.cos(est_pose.getTheta())) + offsetX,
         ymax -
         (int(est_pose.getY() + 15.0 * np.sin(est_pose.getTheta())) + offsetY))
    cv2.circle(world, a, 5, CMAGENTA, 2)
    cv2.line(world, a, b, CMAGENTA, 2)
예제 #4
0
def draw_world(est_pose, particles, world):
    """Visualization.
    This functions draws robots position in the world."""
    
    offset = 100;
    
    world[:] = CWHITE # Clear background to white
    
    # Find largest weight
    max_weight = 0
    for particle in particles:
        max_weight = max(max_weight, particle.getWeight())

    # Draw particles
    for particle in particles:
        x = int(particle.getX()) + offset
        y = int(particle.getY()) + offset
        colour = jet(particle.getWeight() / max_weight)
        cv2.circle(world, (x,y), 2, colour, 2)
    
    # Draw landmarks
    lm0 = (landmarks[0][0]+offset, landmarks[0][1]+offset)
    lm1 = (landmarks[1][0]+offset, landmarks[1][1]+offset)
    cv2.circle(world, lm0, 5, CRED, 2)
    cv2.circle(world, lm1, 5, CGREEN, 2)
    
    # Draw estimated robot pose
    a = (int(est_pose.getX())+offset, int(est_pose.getY())+offset)
    b = (int(est_pose.getX() + 15.0*np.cos(est_pose.getTheta()))+offset, 
                                 int(est_pose.getY() + 15.0*np.sin(est_pose.getTheta()))+offset)
    cv2.circle(world, a, 5, CMAGENTA, 2)
    cv2.line(world, a, b, CMAGENTA, 2)
예제 #5
0
def where_to_go(particle, goal):
    """ Takes a particle (which must be our best bet for our actual position)
    and the place we want to be (goal = [x, y])

    returns a list of the length the robot needs to drive, the degrees it
    needs to turn and the direction it needs to turn"""
    ang = vector_angle([particle.getX(), particle.getY()], goal)
    if ang <= 180:
        turn_dir = 'right'
        turn_deg = ang
    else:
        turn_dir = 'left'
        turn_deg = 180 - ang
    length = np.sqrt(
        np.power((particle.getX() - goal[0]), 2) +
        np.power((particle.getY() - goal[1]), 2))
    return [length, turn_dir, turn_deg]
예제 #6
0
def where_to_go(particle, goal):
    """ Takes a particle (which must be our best bet for our actual position)
    and the place we want to be (goal = [x, y])

    returns a list of the length the robot needs to drive, the degrees it
    needs to turn and the direction it needs to turn"""
    ang = vector_angle([particle.getX(), particle.getY()], goal)
    if ang <= 180:
        turn_dir = 'right'
        turn_deg = ang
    else:
        turn_dir = 'left'
        turn_deg = 180 - ang
    length = np.sqrt(
        np.power((particle.getX() - goal[0]), 2) + np.power(
            (particle.getY() - goal[1]), 2))
    print 'REPORT FROM: where_to_go'
    print 'est_particle: ' + str([particle.getX(), particle.getY()])
    print 'goal: ' + str(goal)
    print 'estimated course: dist=' + str(length) + 'dir=' + turn_dir +\
          'turn degree=' + str(turn_deg)
    return [length, turn_dir, turn_deg]
예제 #7
0
def draw_world(est_pose, particles, world):
    """Visualization.
    This functions draws robots position in the world."""

    offset = 100

    world[:] = CWHITE  # Clear background to white

    # Draw particles
    for particle in particles:
        x = int(particle.getX()) + offset
        y = int(particle.getY()) + offset
        cv2.circle(world, (x, y), 2, CBLUE, 2)
        b = (int(particle.getX() + 15.0 * np.cos(particle.getTheta())) +
             offset,
             int(particle.getY() - 15.0 * np.sin(particle.getTheta())) +
             offset)
        cv2.line(world, (x, y), b, CBLUE, 2)

    # Draw landmarks
    lm0 = (landmarks[0][0] + offset, landmarks[0][1] + offset)
    lm1 = (landmarks[1][0] + offset, landmarks[1][1] + offset)
    lm2 = (landmarks[2][0] + offset, landmarks[2][1] + offset)
    lm3 = (landmarks[3][0] + offset, landmarks[3][1] + offset)

    cv2.circle(world, lm0, 5, CRED, 2)
    cv2.circle(world, lm1, 5, CGREEN, 2)
    cv2.circle(world, lm2, 5, CGREEN, 2)
    cv2.circle(world, lm3, 5, CRED, 2)

    # Draw estimated robot pose
    a = (int(est_pose.getX()) + offset, int(est_pose.getY()) + offset)
    b = (int(est_pose.getX() + 15.0 * np.cos(est_pose.getTheta())) + offset,
         int(est_pose.getY() - 15.0 * np.sin(est_pose.getTheta())) + offset)
    cv2.circle(world, a, 5, CMAGENTA, 2)
    cv2.line(world, a, b, CMAGENTA, 2)
예제 #8
0
                angleWeight = measured_angle/particle.getTheta()
            else:
                posWeight = calculated_distance/measured_distance
                angleWeight = particle.getTheta()/measured_angle
            particle.setWeight() = posWeight*angleWeight


"""



    for particle in particles:
        posWeight = 0
        angleWeight = 0
        if(objectType == 'horizontal'):
            calculated_distance = (math.sqrt(2**particle.getX() + 2**particle.getY()))
            PosWeight = (1/math.sqrt(2*math.pi*sigma_d**2))*math.exp((-measured_distance-calculated_distance)/(2*sigma_distance**2)))
            angleWeight = (1/math.sqrt(2*math.pi*sigma_d**2))*math.exp((-measured_angle-particle.getTheta()/(2*sigma_theta**2)))
            elif(objectType == 'vertical'):
            calculated_distance = (math.sqrt(2**(particle.getX-300) + 2**particle.getY))
            PosWeight = (1/math.sqrt(2*math.pi*sigma_d**2))*math.exp((-measured_distance-calculated_distance)/(2*sigma_distance**2)))
            angleWeight = (1/math.sqrt(2*math.pi*sigma_d**2))*math.exp((-measured_angle-particle.getTheta()/(2*sigma_theta**2)))

        # Resampling
        # XXX: You do this

        # Draw detected pattern
        cam.draw_object(colour)
    else:
        # No observation - reset weights to uniform distribution
        for p in particles:
예제 #9
0
def particle_landmark_vector(mark, particle):
    (mark_x, mark_y) = LANDMARK_COORDINATES[mark]
    x = mark_x - particle.getX()
    y = -1.0 * (mark_y - particle.getY())
    return [x, y]
예제 #10
0
def particle_landmark_vector(mark, particle):
    (mark_x, mark_y) = landmarks[mark]
    x = mark_x - particle.getX()
    y = -1.0 * (mark_y - particle.getY())
    return [x, y]
예제 #11
0
        if delta.getDistance() < 75: # this may have to be less
            print "We've visited ", target.getIdentifier()
            goals.pop()
            target = None
        else:
            # TODO: turn delta.getTheta()
            # TODO: drive forward delta.getDistance()

    # Read odometry, see how far we have moved, and update particles.
    # Or use motor controls to update particles
    for particle in particles:
        s = sin(delta.getTheta())
        c = cos(delta.getTheta())
        
        x = particle.getX()
        y = particle.getY()

        particle.setX((x * c - y * s) - delta.getX())
        particle.setY((x * s + y * c) - delta.getY())

    # Fetch next frame
    colour, distorted = cam.get_colour()    
    
    if waittime < time.time():
        if nextmove == "S":
            stop()
        else:
            # Detect objects
            objectType, measured_distance, measured_angle, colourProb = cam.get_object(colour)
            if objectType != 'none':
                print "Object type = ", objectType