# Also read the reference cylinders (the map).
logfile.read("robot_arena_landmarks.txt")
reference_cylinders = [l[1:3] for l in logfile.landmarks]
# Iterate over all positions.
out_file = file("find_cylinder_pairs.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]
# For every cylinder, find the closest reference cylinder.
cylinder_pairs = find_cylinder_pairs(
world_cylinders, reference_cylinders, max_cylinder_distance)
# Write 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 reference cylinders which were part of a cylinder pair.
write_cylinders(out_file, "W C",
logfile.read("robot4_motors.txt")
logfile.read("robot4_scan.txt")
# Also read the reference cylinders (this is our map).
logfile.read("robot_arena_landmarks.txt")
reference_cylinders = [l[1:3] for l in logfile.landmarks]
out_file = file("apply_transform.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
]
# For every cylinder, find the closest reference cylinder.
cylinder_pairs = find_cylinder_pairs(world_cylinders,
reference_cylinders,
max_cylinder_distance)
# Estimate a transformation using the cylinder pairs.
trafo = estimate_transform(
[world_cylinders[pair[0]] for pair in cylinder_pairs],
[reference_cylinders[pair[1]] for pair in cylinder_pairs],
fix_scale=True)
