示例#1
0
def distHeadingToLine(line):
    '''
  Returns real distance reading to the line, where line is a camera line
  line = (x1,y1), (x2,y2)
  '''
    # Get two points on the (camera) line
    (x1, y1), (x2, y2) = line

    # Convert to points in real space
    x1, y1 = cameraPointToXY((x1, y1))
    x2, y2 = cameraPointToXY((x2, y2))

    return util.pointLineVector((0, 0), (x1, y1), (x2, y2))
示例#2
0
def distHeadingToLine(line):
  '''
  Returns real distance reading to the line, where line is a camera line
  line = (x1,y1), (x2,y2)
  '''
  # Get two points on the (camera) line
  (x1, y1), (x2, y2) = line

  # Convert to points in real space
  x1, y1 = cameraPointToXY((x1, y1))
  x2, y2 = cameraPointToXY((x2, y2))

  return util.pointLineVector((0,0), (x1,y1), (x2,y2))
示例#3
0
def lineProbabilityGivenParticleLocation(observation, particles):
    '''
  observation are the endpoints of the line segment
  '''
    obs_dist, obs_heading = distHeadingToLine(observation)
    print 'Line: %f cm\t %f deg' % (obs_dist, obs_heading * 180 / np.pi)

    pt1, pt2 = observation
    pt1 = cameraPointToXY(pt1)
    pt2 = cameraPointToXY(pt2)
    print 'Line segment: %s, %s' % (pt1, pt2)

    probs = np.zeros(len(particles))
    for line in lines:
        # Get the distance, absolute heading from particle to the line
        dists, headings = util.pointLineVector(particles[:, 0:2], line[0],
                                               line[1])
        # Adjust heading to account for the heading of the robot
        headings = util.normalizeRadians(headings - (particles[:, 2]))

        # Use new distance metric for line segments
        # Convert candidate line into robot coordinate frame
        lineA = transform(line[0], particles)
        lineB = transform(line[1], particles)

        # Take each endpoint of the observed line
        #  and calculate its distance to the candidate line
        #  (both in the robot's coordinate frame)
        dist_metric = (util.pointLineSegmentDistance(pt1, lineA, lineB) +
                       util.pointLineSegmentDistance(pt2, lineA, lineB))

        # Scale distance metric by heading of line
        heading_error = np.abs(util.normalizeRadians(obs_heading - headings))
        dist_metric = dist_metric * \
            np.exp(3*heading_error)

        probs = probs + np.exp(-0.005 * np.abs(dist_metric) -
                               7 * heading_error)

    return probs
示例#4
0
def lineProbabilityGivenParticleLocation(observation, particles):
  '''
  observation are the endpoints of the line segment
  '''
  obs_dist, obs_heading = distHeadingToLine(observation)
  print 'Line: %f cm\t %f deg' % (obs_dist, obs_heading*180/np.pi)

  pt1, pt2 = observation
  pt1 = cameraPointToXY(pt1)
  pt2 = cameraPointToXY(pt2)
  print 'Line segment: %s, %s' % (pt1, pt2)

  probs = np.zeros(len(particles))
  for line in lines:
    # Get the distance, absolute heading from particle to the line
    dists, headings = util.pointLineVector(particles[:,0:2], 
                                           line[0], line[1])
    # Adjust heading to account for the heading of the robot
    headings = util.normalizeRadians(headings - (particles[:,2]))

    # Use new distance metric for line segments
    # Convert candidate line into robot coordinate frame
    lineA = transform(line[0], particles)
    lineB = transform(line[1], particles)

    # Take each endpoint of the observed line
    #  and calculate its distance to the candidate line
    #  (both in the robot's coordinate frame)
    dist_metric = (util.pointLineSegmentDistance(pt1, lineA, lineB) + 
                   util.pointLineSegmentDistance(pt2, lineA, lineB))

    # Scale distance metric by heading of line
    heading_error = np.abs(util.normalizeRadians(obs_heading - headings))
    dist_metric = dist_metric * \
        np.exp(3*heading_error)

    probs = probs + np.exp(-0.005 * np.abs(dist_metric) -7 * heading_error)

  return probs