out_file = open("estimate_wall_transform.txt", "w")
for i in range(len(logfile.scan_data)):
# Compute the new pose.
pose = filter_step(pose, logfile.motor_ticks[i], ticks_to_mm,
robot_width, scanner_displacement)
# Subsample points.
subsampled_points = get_subsampled_points(logfile.scan_data[i])
world_points = [
RobotLogfile.scanner_to_world(pose, c) for c in subsampled_points
]
# Get the transformation
left, right = get_corresponding_points_on_wall(world_points)
trafo = estimate_transform(left, right, fix_scale=True)
# Correct the initial position using trafo. Also transform points.
if trafo:
pose = correct_pose(pose, trafo)
world_points = [apply_transform(trafo, p) for p in world_points]
else:
world_points = []
# Write to file.
# The pose.
print("F %f %f %f" % pose, file=out_file)
# Write the scanner points and corresponding points.
write_cylinders(out_file, "W C", world_points)
out_file.close()
RobotLogfile.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)
# Transform the cylinders using the estimated transform.
transformed_world_cylinders = []
if trafo:
transformed_world_cylinders =\
[apply_transform(trafo, c) for c in
[world_cylinders[pair[0]] for pair in cylinder_pairs]]
# Write to file.
# The pose.
print("F %f %f %f" % pose, file=out_file)
# The detected cylinders in the scanner's coordinate system.
write_cylinders(out_file, "D C", cartesian_cylinders)
# The detected cylinders, transformed using the estimated trafo.
write_cylinders(out_file, "W C", transformed_world_cylinders)
out_file.close()
# Iterate over all positions.
out_file = open("icp_wall_transform.txt", "w")
for i in range(len(logfile.scan_data)):
# Compute the new pose.
pose = filter_step(pose, logfile.motor_ticks[i],
ticks_to_mm, robot_width,
scanner_displacement)
# Subsample points.
subsampled_points = get_subsampled_points(logfile.scan_data[i])
world_points = [RobotLogfile.scanner_to_world(pose, c)
for c in subsampled_points]
# Get the transformation.
# You may play withe the number of iterations here to see
# the effect on the trajectory!
trafo = get_icp_transform(world_points, iterations = 40)
# Correct the initial position using trafo.
pose = correct_pose(pose, trafo)
# Write to file.
# The pose.
print ("F %f %f %f" % pose, file=out_file)
# Write the scanner points and corresponding points.
write_cylinders(out_file, "W C",
[apply_transform(trafo, p) for p in world_points])
out_file.close()
pose = (1850.0, 1897.0, 3.717551306747922)
# Read the logfile which contains all scans.
logfile = RobotLogfile()
logfile.read("robot4_motors.txt")
logfile.read("robot4_scan.txt")
# Iterate over all positions.
out_file = open("find_wall_pairs.txt", "w")
for i in range(len(logfile.scan_data)):
# Compute the new pose.
pose = filter_step(pose, logfile.motor_ticks[i], ticks_to_mm,
robot_width, scanner_displacement)
# Subsample points.
subsampled_points = get_subsampled_points(logfile.scan_data[i])
world_points = [
RobotLogfile.scanner_to_world(pose, c) for c in subsampled_points
]
# Get corresponding points on wall.
left, right = get_corresponding_points_on_wall(world_points)
# Write to file.
# The pose.
print("F %f %f %f" % pose, file=out_file)
# Write the scanner points and corresponding points.
write_cylinders(out_file, "W C", left + right)
out_file.close()
# Iterate over all positions.
out_file = open("find_cylinder_pairs.txt", "w")
for i in range(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 = [
RobotLogfile.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("F %f %f %f" % pose, file=out_file)
# 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",
[reference_cylinders[j[1]] for j in cylinder_pairs])
out_file.close()