def get_observations(scan, jump, min_dist, cylinder_offset, robot_pose, scanner_displacement, reference_cylinders, max_reference_distance): der = compute_derivative(scan, min_dist) cylinders = find_cylinders(scan, der, jump, min_dist) # Compute scanner pose from robot pose. scanner_pose = (robot_pose[0] + cos(robot_pose[2]) * scanner_displacement, robot_pose[1] + sin(robot_pose[2]) * scanner_displacement, robot_pose[2]) # For every detected cylinder which has a closest matching pole in the # reference cylinders set, put the measurement (distance, angle) and the # corresponding reference cylinder into the result list. result = [] for c in cylinders: # Compute the angle and distance measurements. angle = LegoLogfile.beam_index_to_angle(c[0]) distance = c[1] + cylinder_offset # Compute x, y of cylinder in world coordinates. x, y = distance*cos(angle), distance*sin(angle) x, y = LegoLogfile.scanner_to_world(scanner_pose, (x, y)) # Find closest cylinder in reference cylinder set. best_dist_2 = max_reference_distance * max_reference_distance best_ref = None for ref in reference_cylinders: dx, dy = ref[0] - x, ref[1] - y dist_2 = dx * dx + dy * dy if dist_2 < best_dist_2: best_dist_2 = dist_2 best_ref = ref # If found, add to both lists. if best_ref: result.append(((distance, angle), best_ref)) return result
def get_observations(scan, jump, min_dist, cylinder_offset, robot_pose, scanner_displacement, reference_cylinders, max_reference_distance): der = compute_derivative(scan, min_dist) cylinders = find_cylinders(scan, der, jump, min_dist) # Compute scanner pose from robot pose. scanner_pose = (robot_pose[0] + cos(robot_pose[2]) * scanner_displacement, robot_pose[1] + sin(robot_pose[2]) * scanner_displacement, robot_pose[2]) # For every detected cylinder which has a closest matching pole in the # reference cylinders set, put the measurement (distance, angle) and the # corresponding reference cylinder into the result list. result = [] for c in cylinders: # Compute the angle and distance measurements. angle = LegoLogfile.beam_index_to_angle(c[0]) distance = c[1] + cylinder_offset # Compute x, y of cylinder in world coordinates. x, y = distance * cos(angle), distance * sin(angle) x, y = LegoLogfile.scanner_to_world(scanner_pose, (x, y)) # Find closest cylinder in reference cylinder set. best_dist_2 = max_reference_distance * max_reference_distance best_ref = None for ref in reference_cylinders: dx, dy = ref[0] - x, ref[1] - y dist_2 = dx * dx + dy * dy if dist_2 < best_dist_2: best_dist_2 = dist_2 best_ref = ref # If found, add to both lists. if best_ref: result.append(((distance, angle), best_ref)) return result
def get_observations(scan, jump, min_dist, cylinder_offset, robot, max_cylinder_distance): # scan = filter1(scan) scan_f = filter2(scan) # der = compute_derivative(scan, min_dist) # cylinders = find_cylinders(scan, der, jump, min_dist) der = compute_derivative(scan_f, min_dist) # der111 = compute_derivative111(scan_f, min_dist) der2 = compute_derivative111(der, 0) mul_der = [] for i in xrange(len(der2)): mul_der.append(der[i] * abs(der2[i])) mul_der = filter2(mul_der) start_stop = [] start_stop = convert_to_start_stop(mul_der, jump) cylinders = find_cylinders(scan_f, start_stop, jump, min_dist) # Compute scanner pose from robot pose. scanner_pose = (robot.state[0] + cos(robot.state[2]) * robot.scanner_displacement, robot.state[1] + sin(robot.state[2]) * robot.scanner_displacement, robot.state[2]) # For every detected cylinder which has a closest matching pole in the # cylinders that are part of the current state, put the measurement # (distance, angle) and the corresponding cylinder index into the result list. result = [] for c in cylinders: # Compute the angle and distance measurements. angle = LegoLogfile.beam_index_to_angle(c[0]) distance = c[1] + cylinder_offset # Compute x, y of cylinder in world coordinates. xs, ys = distance * cos(angle), distance * sin(angle) x, y = LegoLogfile.scanner_to_world(scanner_pose, (xs, ys)) # Find closest cylinder in the state. best_dist_2 = max_cylinder_distance * max_cylinder_distance best_index = -1 for index in xrange(robot.number_of_landmarks): pole_x, pole_y = robot.state[3 + 2 * index:3 + 2 * index + 2] dx, dy = pole_x - x, pole_y - y dist_2 = dx * dx + dy * dy if dist_2 < best_dist_2: best_dist_2 = dist_2 best_index = index best_index_2 = robot.find_cylinder((distance, angle), float(0.1)) # Always add result to list. Note best_index may be -1. # print(">>> best_index %d"%best_index) if (best_index != best_index_2): print("best_index %d best_index_2 %d" % (best_index, best_index_2)) result.append(((distance, angle), (x, y), (xs, ys), best_index)) return result
def __init__(self, scan, cylinders, points_per_sector=66, min_distance=300, inline_threshold=45, attempts=10, valid_threshold=0.8): self.points_per_sector = points_per_sector self.scan = [] self.sector_rays = [] self.min_distance = min_distance self.n_valid_sectors = 0 self.valid_sectors = [] self.best_lines = [] self.inline_threshold = inline_threshold self.landmarks = [] self.walls = [] offset = 0 self.sectors = [] while offset < len(scan): bearing = LegoLogfile.beam_index_to_angle(offset) x, y = 3000 * cos(bearing), 3000 * sin(bearing) self.sector_rays.append(np.array([y / x, -1, 0, 0, 0, x, y])) ray_r = np.array([y / x, -1, 0]) bearing = LegoLogfile.beam_index_to_angle(offset + self.points_per_sector) x, y = 3000 * cos(bearing), 3000 * sin(bearing) ray_l = np.array([y / x, -1, 0]) sec_points = self.get_sector_scans_without_landmarks( scan, cylinders, offset) self.scan += sec_points # for point in sec_points: # self.scan.append(point) offset += self.points_per_sector sector = Sector(sec_points, (ray_l, ray_r), inline_threshold=self.inline_threshold, attempts=attempts, valid_threshold=valid_threshold) if sector.valid: self.n_valid_sectors += 1 self.sectors.append(sector) self.scan_len = len(self.scan) # print(self.n_valid_sectors) bearing = LegoLogfile.beam_index_to_angle(offset) x, y = 3000 * cos(bearing), 3000 * sin(bearing) self.sector_rays.append(np.array([y / x, -1, 0, 0, 0, x, y]))
def get_cylinders_from_scan(scan, jump, min_dist, cylinder_offset, points_per_scan, max_cylinder_d): scan_f = filter2(scan) # der = compute_derivative(scan, min_dist) # cylinders = find_cylinders(scan, der, jump, min_dist) der = compute_derivative(scan_f, min_dist) # der111 = compute_derivative111(scan_f, min_dist) der2 = compute_derivative111(der, 0) mul_der=[] for i in xrange(len(der2)): mul_der.append(der[i]*abs(der2[i])) mul_der = filter2(mul_der) start_stop = [] start_stop = convert_to_start_stop(mul_der, jump) cylinders = find_cylinders(scan_f, start_stop, jump, min_dist, points_per_scan, max_cylinder_d) # der = compute_derivative(scan, min_dist) # cylinders = find_cylinders(scan, der, jump, min_dist) result = [] for c in cylinders: # Compute the angle and distance measurements. bearing = LegoLogfile.beam_index_to_angle(c[0][0]) distance = c[0][1] + cylinder_offset # Compute x, y of cylinder in the scanner system. x, y = distance*cos(bearing), distance*sin(bearing) result.append( (np.array([distance, bearing]), np.array([x, y]), c[1])) return result
def assign_cylinders(cylinders, robot_pose, scanner_displacement, reference_cylinders): # Compute scanner pose from robot pose. scanner_pose = (robot_pose[0] + cos(robot_pose[2]) * scanner_displacement, robot_pose[1] + sin(robot_pose[2]) * scanner_displacement, robot_pose[2]) # Find closest cylinders. result = [] for c in cylinders: # Get world coordinate of cylinder. x, y = LegoLogfile.scanner_to_world(scanner_pose, c[2:4]) # Find closest cylinder in reference cylinder set. best_dist_2 = 1e300 best_ref = None for ref in reference_cylinders: dx, dy = ref[0] - x, ref[1] - y dist_2 = dx * dx + dy * dy if dist_2 < best_dist_2: best_dist_2 = dist_2 best_ref = ref # If found, add to both lists. if best_ref: result.append((c[0:2], best_ref)) return result
def get_cylinders_from_scan(scan, jump, min_dist, cylinder_offset): der = compute_derivative(scan, min_dist) cylinders = find_cylinders(scan, der, jump, min_dist) result = [] for c in cylinders: # Compute the angle and distance measurements. bearing = LegoLogfile.beam_index_to_angle(c[0]) distance = c[1] + cylinder_offset # Compute x, y of cylinder in the scanner system. x, y = distance*cos(bearing), distance*sin(bearing) result.append( (distance, bearing, x, y) ) return result
def get_cylinders_from_scan(scan, jump, min_dist, cylinder_offset): der = compute_derivative(scan, min_dist) cylinders = find_cylinders(scan, der, jump, min_dist) result = [] for c in cylinders: # Compute the angle and distance measurements. bearing = LegoLogfile.beam_index_to_angle(c[0]) distance = c[1] + cylinder_offset # Compute x, y of cylinder in the scanner system. x, y = distance * cos(bearing), distance * sin(bearing) result.append((distance, bearing, x, y)) return result
def compute_cartesian_coordinates(cylinders, cylinder_offset): result = [] for c in cylinders: # --->>> Insert here the conversion from polar to Cartesian coordinates. # c is a tuple (beam_index, range). # For converting the beam index to an angle, use # LegoLogfile.beam_index_to_angle(beam_index) radius = c[1] + cylinder_offset angle = LegoLogfile.beam_index_to_angle(c[0]) x = radius * cos(angle) y = radius * sin(angle) result.append((x, y)) # Replace this by your (x,y) return result
def get_observations(scan, jump, min_dist, cylinder_offset, robot, max_cylinder_distance): der = compute_derivative(scan, min_dist) cylinders = find_cylinders(scan, der, jump, min_dist) # Compute scanner pose from robot pose. scanner_pose = (robot.specific_state[0] + cos(robot.specific_state[2]) * robot.scanner_displacement, robot.specific_state[1] + sin(robot.specific_state[2]) * robot.scanner_displacement, robot.specific_state[2]) # For every detected cylinder which has a closest matching pole in the # cylinders that are part of the current state, put the measurement # (distance, angle) and the corresponding cylinder index into the result list. result = [] for c in cylinders: # Compute the angle and distance measurements. angle = LegoLogfile.beam_index_to_angle(c[0]) distance = c[1] + cylinder_offset # Compute x, y of cylinder in world coordinates. xs, ys = distance * cos(angle), distance * sin(angle) x, y = LegoLogfile.scanner_to_world(scanner_pose, (xs, ys)) # Find closest cylinder in the state. best_dist_2 = max_cylinder_distance * max_cylinder_distance best_index = -1 for index in range(robot.number_of_landmarks): pole_x, pole_y = robot.specific_state[3 + 2 * index:3 + 2 * index + 2] dx, dy = pole_x - x, pole_y - y dist_2 = dx * dx + dy * dy if dist_2 < best_dist_2: best_dist_2 = dist_2 best_index = index # Always add result to list. Note best_index may be -1. result.append(((distance, angle), (x, y), (xs, ys), best_index)) return result
def get_observations(scan, jump, min_dist, cylinder_offset, robot, max_cylinder_distance): der = compute_derivative(scan, min_dist) cylinders = find_cylinders(scan, der, jump, min_dist) # Compute scanner pose from robot pose. scanner_pose = ( robot.state[0] + cos(robot.state[2]) * robot.scanner_displacement, robot.state[1] + sin(robot.state[2]) * robot.scanner_displacement, robot.state[2]) # For every detected cylinder which has a closest matching pole in the # cylinders that are part of the current state, put the measurement # (distance, angle) and the corresponding cylinder index into the result list. result = [] for c in cylinders: # Compute the angle and distance measurements. angle = LegoLogfile.beam_index_to_angle(c[0]) distance = c[1] + cylinder_offset # Compute x, y of cylinder in world coordinates. xs, ys = distance*cos(angle), distance*sin(angle) x, y = LegoLogfile.scanner_to_world(scanner_pose, (xs, ys)) # Find closest cylinder in the state. best_dist_2 = max_cylinder_distance * max_cylinder_distance best_index = -1 for index in xrange(robot.number_of_landmarks): pole_x, pole_y = robot.state[3+2*index : 3+2*index+2] dx, dy = pole_x - x, pole_y - y dist_2 = dx * dx + dy * dy if dist_2 < best_dist_2: best_dist_2 = dist_2 best_index = index # Always add result to list. Note best_index may be -1. result.append(((distance, angle), (x, y), (xs, ys), best_index)) return result
def get_sector_scans_without_landmarks(self, scan, cylinders, offset): result = [] for i in xrange(self.points_per_sector): j = i + offset distance = scan[j] save_scan = True if (distance < self.min_distance): continue for measurement, measurement_in_scanner_system, indxs in cylinders: # print("indxs[0] %d indxs[1] %d" %(indxs[0], indxs[1])) if (j >= indxs[0] and j <= indxs[1]): save_scan = False break if save_scan: bearing = LegoLogfile.beam_index_to_angle(j) x, y = distance * cos(bearing), distance * sin(bearing) result.append(np.array([x, y])) return result
max_cylinder_distance): der = compute_derivative(scan, min_dist) cylinders = find_cylinders(scan, der, jump, min_dist) # Compute scanner pose from robot pose. scanner_pose = ( robot.state[0] + cos(robot.state[2]) * robot.scanner_displacement, robot.state[1] + sin(robot.state[2]) * robot.scanner_displacement, robot.state[2]) # For every detected cylinder which has a closest matching pole in the # cylinders that are part of the current state, put the measurement # (distance, angle) and the corresponding cylinder index into the result list. result = [] for c in cylinders: # Compute the angle and distance measurements. angle = LegoLogfile.beam_index_to_angle(c[0]) distance = c[1] + cylinder_offset # Compute x, y of cylinder in world coordinates. xs, ys = distance*cos(angle), distance*sin(angle) x, y = LegoLogfile.scanner_to_world(scanner_pose, (xs, ys)) # Find closest cylinder in the state. best_dist_2 = max_cylinder_distance * max_cylinder_distance best_index = -1 for index in range(robot.number_of_landmarks): pole_x, pole_y = robot.state[3+2*index : 3+2*index+2] dx, dy = pole_x - x, pole_y - y dist_2 = dx * dx + dy * dy if dist_2 < best_dist_2: best_dist_2 = dist_2 best_index = index # Always add result to list. Note best_index may be -1.
# Plot a scan of the robot using matplotlib. # 03_a_plot_scan # XU Shang, 2016-10-13 from pylab import plot,show from lego_robot import LegoLogfile # Read the logfile which contains all scans. logfile = LegoLogfile() logfile.read("robot4_scan.txt") # Plot one scan. plot(logfile.scan_data[10]) show()
else: g1 = x + l * cos(theta) g2 = y + l * sin(theta) g3 = theta return array([g1, g2, g3]) if __name__ == '__main__': scanner_displacement = 30.0 # scanner's position related to robot's coordinate ticks_to_mm = 0.349 # convert ticks from motor to distance robot_width = 155.0 # initial state state = array([1850.0, 1897.0, 213.0 / 180 * pi]) # read data from motor's tick sensor logfile = LegoLogfile() logfile.read("robot4_motors.txt") states = [] # loop over all motor ticks to generate a states list for ticks in logfile.motor_ticks: control = array(ticks) * ticks_to_mm state = ExtendedKalmanFilter.g(state, control, robot_width) states.append(state) f = open("states_from_ticks.txt", "w") # output the state of scanner instead of the robot for s in states: print >> f, "F %f %f %f" % \ tuple(s + [scanner_displacement*cos(s[2]), scanner_displacement*sin(s[2]),0.0])
# Plot the increments of the left and right motor. # 01_c_plot_motor_increments.py # Claus Brenner, 07 NOV 2012 from pylab import * from lego_robot import LegoLogfile if __name__ == '__main__': logfile = LegoLogfile() logfile.read("robot4_motors.txt") plot(logfile.motor_ticks) show()
if __name__ == '__main__': # The constants we used for the filter_step. scanner_displacement = 30.0 ticks_to_mm = 0.349 robot_width = 173.0 # The constants we used for the cylinder detection in our scan. minimum_valid_distance = 20.0 depth_jump = 100.0 cylinder_offset = 90.0 # The start pose we obtained miraculously. pose = (1850.0, 1897.0, 3.717551306747922) # Read the logfile which contains all scans. logfile = LegoLogfile() logfile.read("robot4_motors.txt") logfile.read("robot4_scan.txt") # Iterate over all positions. out_file = file("project_landmarks.txt", "w") for i in xrange(len(logfile.scan_data)): # Compute the new pose. pose = filter_step(pose, logfile.motor_ticks[i], ticks_to_mm, robot_width, scanner_displacement) # Extract cylinders, also convert them to world coordinates. cartesian_cylinders = compute_scanner_cylinders( logfile.scan_data[i], depth_jump, minimum_valid_distance, cylinder_offset) world_cylinders = [