out_file = file("estimate_wall_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)
# Subsample points.
subsampled_points = get_subsampled_points(logfile.scan_data[i])
world_points = [
LegoLogfile.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 >> out_file, "F %f %f %f" % pose
# Write the scanner points and corresponding points.
write_cylinders(out_file, "W C", world_points)
out_file.close()
out_file = file("estimate_wall_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)
# Subsample points.
subsampled_points = get_subsampled_points(logfile.scan_data[i])
world_points = [LegoLogfile.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 >> out_file, "F %f %f %f" % pose
# Write the scanner points and corresponding points.
write_cylinders(out_file, "W C", world_points)
out_file.close()
