def cornerProbabilityGivenParticleLocation(observation, particle):
'''
Calculates P(e|x_t), given that observation is the (scalar) distance
to the corner
TODO: r, theta = observation
'''
obs_dist, obs_heading = observation
prob = 0.0
for corner in corners:
# calculate distance to corner
distance = util.distance(particle[0:2], corner)
# calculate the relative heading w.r.t particle position and heading
heading = util.normalizeRadians(util.heading(particle[0:2], corner) - particle[2])
# TODO tune sigmas
# (assume P(e|x_t) ~ exp{-1/2 * |distance - obs_dist| / sigma_1^2}
# * exp{-1/2 * |heading - obs_heading| / sigma_2^2} )
prob += numpy.exp(-0.1 * numpy.abs(distance - obs_dist) +
-7 * numpy.abs(heading - obs_heading))
'''
if corner is corners[0]:
print particle, 'hdg:', heading
print numpy.exp(-0.1 * numpy.abs(distance - obs_dist) +
-10 * numpy.abs(heading - obs_heading))
print numpy.exp(-1 * numpy.abs(heading - obs_heading))
'''
return prob
def cornerProbabilityGivenParticleLocation(observation, particle):
'''
Calculates P(e|x_t), given that observation is the (scalar) distance
to the corner
TODO: r, theta = observation
'''
obs_dist, obs_heading = observation
prob = 0.0
for corner in corners:
# calculate distance to corner
distance = util.distance(particle[0:2], corner)
# calculate the relative heading w.r.t particle position and heading
heading = util.normalizeRadians(
util.heading(particle[0:2], corner) - particle[2])
# TODO tune sigmas
# (assume P(e|x_t) ~ exp{-1/2 * |distance - obs_dist| / sigma_1^2}
# * exp{-1/2 * |heading - obs_heading| / sigma_2^2} )
prob += numpy.exp(-0.1 * numpy.abs(distance - obs_dist) +
-7 * numpy.abs(heading - obs_heading))
'''
if corner is corners[0]:
print particle, 'hdg:', heading
print numpy.exp(-0.1 * numpy.abs(distance - obs_dist) +
-10 * numpy.abs(heading - obs_heading))
print numpy.exp(-1 * numpy.abs(heading - obs_heading))
'''
return prob
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
def cornerProbabilityGivenParticleLocation(observation, particles):
'''
Calculates P(e|x_t), given that observation is a point in the
robot coordinate frame
observation is a single observation
particles is a 2D numpy array of particles:
[[x0, y0, theta0],
[x1, y1, theta1],
etc...]
'''
obs_dist, obs_heading = distHeadingToPoint(observation)
print 'Corner: %f cm\t%f deg' % (obs_dist, obs_heading * 180 / np.pi)
probs = np.zeros(len(particles))
for corner in corners:
# calculate distance from each particle to corner
corner = np.array(corner)
distanceVectors = corner - particles[:, 0:2]
distances = np.array(map(np.linalg.norm, distanceVectors))
# calculate the relative heading w.r.t particle position and heading
headings = np.arctan2(distanceVectors[:, 1], distanceVectors[:, 0])
headings = util.normalizeRadians(headings - (particles[:, 2]))
# TODO tune sigmas
# (assume P(e|x_t) ~ exp{-1/2 * |distance - obs_dist| / sigma_1^2}
# * exp{-1/2 * |heading - obs_heading| / sigma_2^2} )
probs = probs + np.exp(-0.005 * np.abs(distances - obs_dist) +
-7 * np.abs(headings - obs_heading))
'''
if corner is corners[0]:
print particle, 'hdg:', heading
print np.exp(-0.1 * np.abs(distance - obs_dist) +
-10 * np.abs(heading - obs_heading))
print np.exp(-1 * np.abs(heading - obs_heading))
'''
return probs
def cornerProbabilityGivenParticleLocation(observation, particles):
'''
Calculates P(e|x_t), given that observation is a point in the
robot coordinate frame
observation is a single observation
particles is a 2D numpy array of particles:
[[x0, y0, theta0],
[x1, y1, theta1],
etc...]
'''
obs_dist, obs_heading = distHeadingToPoint(observation)
print 'Corner: %f cm\t%f deg' % (obs_dist, obs_heading*180/np.pi)
probs = np.zeros(len(particles))
for corner in corners:
# calculate distance from each particle to corner
corner = np.array(corner)
distanceVectors = corner - particles[:, 0:2]
distances = np.array(map(np.linalg.norm, distanceVectors))
# calculate the relative heading w.r.t particle position and heading
headings = np.arctan2(distanceVectors[:,1], distanceVectors[:,0])
headings = util.normalizeRadians(headings - (particles[:,2]))
# TODO tune sigmas
# (assume P(e|x_t) ~ exp{-1/2 * |distance - obs_dist| / sigma_1^2}
# * exp{-1/2 * |heading - obs_heading| / sigma_2^2} )
probs = probs + np.exp(-0.005 * np.abs(distances - obs_dist) +
-7 * np.abs(headings - obs_heading))
'''
if corner is corners[0]:
print particle, 'hdg:', heading
print np.exp(-0.1 * np.abs(distance - obs_dist) +
-10 * np.abs(heading - obs_heading))
print np.exp(-1 * np.abs(heading - obs_heading))
'''
return probs
def find_lines(frame):
# Resize to 640x480
frame_size = cv.GetSize(frame)
if frame_size[0] != 640:
frame_small = cv.CreateMat(480, 640, cv.CV_8UC3)
cv.Resize(frame, frame_small)
else:
frame_small = frame
# Threshold by distance: blank out all top pixels
cv.Rectangle(frame_small, (0, 0), (640, 80), (0, 0, 0, 0), cv.CV_FILLED)
# Convert to grayscale
frame_size = cv.GetSize(frame_small)
frame_gray = cv.CreateImage(frame_size, cv.IPL_DEPTH_8U, 1)
cv.CvtColor(frame_small, frame_gray, cv.CV_BGR2GRAY)
# Use color thresholding to get white lines
threshold = cv.CreateImage(frame_size, cv.IPL_DEPTH_8U, 1)
cv.Threshold(frame_gray, threshold, 190, 255, cv.CV_THRESH_BINARY)
# Morphological ops to reduce noise
# TODO try to reduce sizes to increase detection of faraway lines
openElement = cv.CreateStructuringElementEx(7, 7, 3, 3, cv.CV_SHAPE_RECT)
closeElement = cv.CreateStructuringElementEx(11, 11, 5, 5,
cv.CV_SHAPE_RECT)
cvOpen(threshold, threshold, openElement)
cvClose(threshold, threshold, closeElement)
# Use Canny edge detection to find edges
edges = cv.CreateImage(frame_size, cv.IPL_DEPTH_8U, 1)
cv.Canny(threshold, edges, 100, 200)
# Use Hough transform to find equations for lines
line_storage = cv.CreateMemStorage()
lines = cv.HoughLines2(edges, line_storage, cv.CV_HOUGH_STANDARD, 1,
cv.CV_PI / 180.0, 120)
lines = list(lines)
# Remove spurious line from the black rectangle up top
for line in lines[:]:
if (abs(180 - line[0]) < 10
and abs(util.normalizeRadians(cv.CV_PI / 2 - line[1])) < 0.01):
lines.remove(line)
# Group lines that are within r +/-12 and theta +/- 5 degrees
grouped_lines = []
r_threshold = 12 # in px
theta_threshold = cv.CV_PI * 5 / 180 # in radians
while len(lines) > 0:
line1 = normalizeLine(lines.pop())
avg_line = line1
matched_lines = [line1]
for j in range(len(lines) - 1, -1, -1):
line2 = normalizeLine(lines[j])
if verbose:
# Print which criteria were matched
if (abs(avg_line[0] - line2[0]) < r_threshold):
print 1,
if (abs(util.normalizeRadians(avg_line[1] - line2[1])) <
theta_threshold):
print 2,
print avg_line, line2
if (abs(avg_line[0] - line2[0]) < r_threshold and
abs(util.normalizeRadians(avg_line[1] - line2[1])) < \
theta_threshold):
matched_lines.append(line2)
avg_line = avgLines(matched_lines)
lines.pop(j)
if verbose: print matched_lines
grouped_lines.append(avg_line)
lines = grouped_lines
# Group possible pairs of lines by smallest angle difference
grouped_lines = []
while len(lines) > 0:
line1 = normalizeLine(lines.pop())
closest = None
for j in range(len(lines) - 1, -1, -1):
line2 = normalizeLine(lines[j])
# Find the closest match
if ((closest is None
or abs(util.normalizeRadians(line1[1] - line2[1])) < \
abs(util.normalizeRadians(line1[1] - closest[1])))
# Make sure difference < pi/4 to reduce errors
and abs(util.normalizeRadians(line1[1] - line2[1])) < \
cv.CV_PI / 4):
closest = line2
if closest is not None:
lines.remove(closest)
# Sort list by line[0] (radius)
if line1[0] > closest[0]:
line1, closest = closest, line1
# Make a tuple (line1, line2) or (line,) if no match found
grouped_lines.append((line1, closest))
else:
grouped_lines.append((line1, ))
# Print lines
if len(grouped_lines) > 0:
print 'Groups of lines:', grouped_lines
i = 0
for group in grouped_lines:
for j in range(len(group)):
rho, theta = group[j]
a, b = np.cos(theta), np.sin(theta)
x0, y0 = a * rho, b * rho
pt1 = (cv.Round(x0 + 1000 * (-b)), cv.Round(y0 + 1000 * (a)))
pt2 = (cv.Round(x0 - 1000 * (-b)), cv.Round(y0 - 1000 * (a)))
#cv.Line(frame_small, pt1, pt2,
# hv2rgb(360.0*i/len(grouped_lines), 1.0), 1, 8)
i += 1
# If 2+ groups of lines, find corners (intersection point of lines)
intersection_pts = []
if len(grouped_lines) > 1:
for i in range(len(grouped_lines)):
pair1 = grouped_lines[i]
for j in range(i + 1, len(grouped_lines)):
pair2 = grouped_lines[j]
# Make sure their angles differ by more than 10 deg to
# reduce errors
if (abs(util.normalizeRadians(pair1[0][1] - pair2[0][1])) <
cv.CV_PI * 10 / 180):
break
# Enumerate intersection points
pts = []
for line1 in pair1:
for line2 in pair2:
pts.append(lineIntersection(line1, line2))
# Find average of intersection points
x = sum(pt[0] for pt in pts) / len(pts)
y = sum(pt[1] for pt in pts) / len(pts)
pt = (x, y)
print 'Intersection:', pt,
intersection_pts.append(pt)
pt = (cv.Round(x), cv.Round(y))
cv.Circle(frame_small, pt, 4, (0, 255, 0, 0))
# Find direction of intersection by following each line
# (direction is defined as the point of the T)
angles = []
for pair in grouped_lines:
angles.append(pair[0][1] + cv.CV_PI / 2)
angles.append(pair[0][1] - cv.CV_PI / 2)
for angle in angles:
# Look 50px down the line for white pixels
# TODO look a variable amount
x1 = x + 50 * np.cos(angle)
y1 = y + 50 * np.sin(angle)
# Enforce limits
# TODO handle when intersection is off the bounds of the image
# Currently orientation of the intersection is not being used
# by the particle filter
x1 = min(max(0, x1), frame_size[0] - 1)
y1 = min(max(0, y1), frame_size[1] - 1)
srchPt = cv.Round(x1), cv.Round(y1)
if threshold[srchPt[1], srchPt[0]] == 0:
x1 = x + 50 * np.cos(angle + cv.CV_PI)
y1 = y + 50 * np.sin(angle + cv.CV_PI)
invSrchPt = cv.Round(x1), cv.Round(y1)
cv.Line(frame_small, pt, invSrchPt, (0, 255, 0, 0), 1,
8)
print 'Angle:', angle + cv.CV_PI
break
# Convert line equations into line segments
line_segments = []
for group in grouped_lines:
if len(group) == 2:
# Get the average of the lines in a pair
line1, line2 = group
line = ((line1[0] + line2[0]) / 2.0, (line1[1] + line2[1]) / 2.0)
else:
line = group[0]
# Look down the line for the endpoints of the white region
line_segment = lineToVisibleSegment(line, threshold)
if line_segment != None:
line_segments.append(line_segment)
print 'Line segments:', line_segments
i = 0
for pt1, pt2 in line_segments:
pt1 = cv.Round(pt1[0]), cv.Round(pt1[1])
pt2 = cv.Round(pt2[0]), cv.Round(pt2[1])
cv.Line(frame_small, pt1, pt2,
hv2rgb(360.0 * i / len(line_segments), 1.0), 2, 8)
i += 1
cv.ShowImage('frame', frame_small)
cv.ShowImage('edges', threshold)
return line_segments, intersection_pts
def find_lines(frame):
# Resize to 640x480
frame_size = cv.GetSize(frame)
if frame_size[0] != 640:
frame_small = cv.CreateMat(480, 640, cv.CV_8UC3)
cv.Resize(frame, frame_small)
else:
frame_small = frame
# Threshold by distance: blank out all top pixels
cv.Rectangle(frame_small, (0,0), (640, 80), (0,0,0,0), cv.CV_FILLED)
# Convert to grayscale
frame_size = cv.GetSize(frame_small)
frame_gray = cv.CreateImage(frame_size, cv.IPL_DEPTH_8U, 1)
cv.CvtColor(frame_small, frame_gray, cv.CV_BGR2GRAY)
# Use color thresholding to get white lines
threshold = cv.CreateImage(frame_size, cv.IPL_DEPTH_8U, 1)
cv.Threshold(frame_gray, threshold, 190, 255, cv.CV_THRESH_BINARY)
# Morphological ops to reduce noise
# TODO try to reduce sizes to increase detection of faraway lines
openElement = cv.CreateStructuringElementEx(7, 7, 3, 3,
cv.CV_SHAPE_RECT)
closeElement = cv.CreateStructuringElementEx(11, 11, 5, 5,
cv.CV_SHAPE_RECT)
cvOpen(threshold, threshold, openElement)
cvClose(threshold, threshold, closeElement)
# Use Canny edge detection to find edges
edges = cv.CreateImage(frame_size, cv.IPL_DEPTH_8U, 1)
cv.Canny(threshold, edges, 100, 200)
# Use Hough transform to find equations for lines
line_storage = cv.CreateMemStorage()
lines = cv.HoughLines2(edges, line_storage, cv.CV_HOUGH_STANDARD,
1, cv.CV_PI/180.0, 120)
lines = list(lines)
# Remove spurious line from the black rectangle up top
for line in lines[:]:
if (abs(180 - line[0]) < 10 and
abs(util.normalizeRadians(cv.CV_PI/2 - line[1])) < 0.01):
lines.remove(line)
# Group lines that are within r +/-12 and theta +/- 5 degrees
grouped_lines = []
r_threshold = 12 # in px
theta_threshold = cv.CV_PI * 5 / 180 # in radians
while len(lines) > 0:
line1 = normalizeLine(lines.pop())
avg_line = line1
matched_lines = [line1]
for j in range(len(lines)-1, -1, -1):
line2 = normalizeLine(lines[j])
if verbose:
# Print which criteria were matched
if (abs(avg_line[0] - line2[0]) < r_threshold):
print 1,
if (abs(util.normalizeRadians(avg_line[1] - line2[1])) <
theta_threshold):
print 2,
print avg_line, line2
if (abs(avg_line[0] - line2[0]) < r_threshold and
abs(util.normalizeRadians(avg_line[1] - line2[1])) < \
theta_threshold):
matched_lines.append(line2)
avg_line = avgLines(matched_lines)
lines.pop(j)
if verbose: print matched_lines
grouped_lines.append(avg_line)
lines = grouped_lines
# Group possible pairs of lines by smallest angle difference
grouped_lines = []
while len(lines) > 0:
line1 = normalizeLine(lines.pop())
closest = None
for j in range(len(lines)-1, -1, -1):
line2 = normalizeLine(lines[j])
# Find the closest match
if ((closest is None
or abs(util.normalizeRadians(line1[1] - line2[1])) < \
abs(util.normalizeRadians(line1[1] - closest[1])))
# Make sure difference < pi/4 to reduce errors
and abs(util.normalizeRadians(line1[1] - line2[1])) < \
cv.CV_PI / 4):
closest = line2
if closest is not None:
lines.remove(closest)
# Sort list by line[0] (radius)
if line1[0] > closest[0]:
line1, closest = closest, line1
# Make a tuple (line1, line2) or (line,) if no match found
grouped_lines.append((line1, closest))
else:
grouped_lines.append((line1,))
# Print lines
if len(grouped_lines) > 0:
print 'Groups of lines:', grouped_lines
i = 0
for group in grouped_lines:
for j in range(len(group)):
rho, theta = group[j]
a, b = np.cos(theta), np.sin(theta)
x0, y0 = a*rho, b*rho
pt1 = (cv.Round(x0 + 1000*(-b)),
cv.Round(y0 + 1000*(a)))
pt2 = (cv.Round(x0 - 1000*(-b)),
cv.Round(y0 - 1000*(a)));
#cv.Line(frame_small, pt1, pt2,
# hv2rgb(360.0*i/len(grouped_lines), 1.0), 1, 8)
i += 1
# If 2+ groups of lines, find corners (intersection point of lines)
intersection_pts = []
if len(grouped_lines) > 1:
for i in range(len(grouped_lines)):
pair1 = grouped_lines[i]
for j in range(i+1, len(grouped_lines)):
pair2 = grouped_lines[j]
# Make sure their angles differ by more than 10 deg to
# reduce errors
if (abs(util.normalizeRadians(pair1[0][1] - pair2[0][1])) <
cv.CV_PI*10/180):
break
# Enumerate intersection points
pts = []
for line1 in pair1:
for line2 in pair2:
pts.append(lineIntersection(line1, line2))
# Find average of intersection points
x = sum(pt[0] for pt in pts)/len(pts)
y = sum(pt[1] for pt in pts)/len(pts)
pt = (x, y)
print 'Intersection:', pt,
intersection_pts.append(pt)
pt = (cv.Round(x), cv.Round(y))
cv.Circle(frame_small, pt, 4, (0,255,0,0))
# Find direction of intersection by following each line
# (direction is defined as the point of the T)
angles = []
for pair in grouped_lines:
angles.append(pair[0][1] + cv.CV_PI/2)
angles.append(pair[0][1] - cv.CV_PI/2)
for angle in angles:
# Look 50px down the line for white pixels
# TODO look a variable amount
x1 = x + 50*np.cos(angle)
y1 = y + 50*np.sin(angle)
# Enforce limits
# TODO handle when intersection is off the bounds of the image
# Currently orientation of the intersection is not being used
# by the particle filter
x1 = min(max(0, x1), frame_size[0]-1)
y1 = min(max(0, y1), frame_size[1]-1)
srchPt = cv.Round(x1), cv.Round(y1)
if threshold[srchPt[1], srchPt[0]] == 0:
x1 = x + 50*np.cos(angle + cv.CV_PI)
y1 = y + 50*np.sin(angle + cv.CV_PI)
invSrchPt = cv.Round(x1), cv.Round(y1)
cv.Line(frame_small, pt, invSrchPt,
(0,255,0,0), 1, 8)
print 'Angle:', angle+cv.CV_PI
break
# Convert line equations into line segments
line_segments = []
for group in grouped_lines:
if len(group) == 2:
# Get the average of the lines in a pair
line1, line2 = group
line = ((line1[0] + line2[0]) / 2.0, (line1[1] + line2[1]) / 2.0)
else:
line = group[0]
# Look down the line for the endpoints of the white region
line_segment = lineToVisibleSegment(line, threshold)
if line_segment != None:
line_segments.append(line_segment)
print 'Line segments:', line_segments
i = 0
for pt1, pt2 in line_segments:
pt1 = cv.Round(pt1[0]), cv.Round(pt1[1])
pt2 = cv.Round(pt2[0]), cv.Round(pt2[1])
cv.Line(frame_small, pt1, pt2, hv2rgb(360.0*i/len(line_segments), 1.0),
2, 8)
i += 1
cv.ShowImage('frame', frame_small)
cv.ShowImage('edges', threshold)
return line_segments, intersection_pts
