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