# Iterate over all positions. out_file = file("icp_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. # 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 >> out_file, "F %f %f %f" % pose # 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()
# Iterate over all positions. out_file = file("icp_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. # You may play withe the number of iterations here to see # the effect on the trajectory! trafo = get_icp_transform(world_points, iterations=3) # Correct the initial position using trafo. pose = correct_pose(pose, trafo) # Write to file. # The pose. out_file.write("F %f %f %f\n" % pose) # 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()