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_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 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_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 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
# 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 = [ LegoLogfile.scanner_to_world(pose, c) for c in cartesian_cylinders ] # Write results to file. # The pose. print >> out_file, "F %f %f %f" % pose # The detected cylinders in the scanner's coordinate system. write_cylinders(out_file, "D C", cartesian_cylinders) # The detected cylinders in the world coordinate system. write_cylinders(out_file, "W C", world_cylinders) out_file.close()
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. result.append(((distance, angle), (x, y), (xs, ys), best_index)) return result