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