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