def get_icp_transform(world_points, iterations): # Iterate assignment and estimation of trafo a few times. # --->>> Implement your code here. # You may use the following strategy: # Start with the identity transform: overall_trafo = (1.0, 1.0, 0.0, 0.0, 0.0) # Then loop for j in xrange(iterations): for j in xrange(iterations): # Transform the world_points using the curent overall_trafo # Call get_correspoinding_points_on_wall(...) # Get the transformation left, right = get_corresponding_points_on_wall(world_points) # Determine transformation which is needed "on top of" the current # overall_trafo: trafo = estimate_transform(...) trafo = estimate_transform(left, right, fix_scale = True) # Concatenate the found transformation with the current overall_trafo # to obtain a new, 'combined' transformation to concatenate two similarities. overall_trafo = concatenate_transform(trafo, overall_trafo) # Correct the initial position using trafo. Also transform points. if trafo: world_points = [apply_transform(trafo, p) for p in world_points] else: world_points = [] # Return the final transformation. return overall_trafo
def get_icp_transform(world_points, iterations): # Iterate assignment and estimation of trafo a few times. # --->>> Implement your code here. # You may use the following strategy: # Start with the identity transform: overall_trafo = (1.0, 1.0, 0.0, 0.0, 0.0) # Then loop for j in xrange(iterations): for i in range(0, iterations): # Transform the world_points using the curent overall_trafo # (see 05_b on how to do this) world_points = [apply_transform(overall_trafo, p) for p in world_points] # Call get_correspoinding_points_on_wall(...) left, right = get_corresponding_points_on_wall(world_points) # Determine transformation which is needed "on top of" the current # overall_trafo: trafo = estimate_transform(...) trafo = estimate_transform(left, right, fix_scale = True) # Concatenate the found transformation with the current overall_trafo # to obtain a new, 'combined' transformation. You may use the function # overall_trafo = concatenate_transform(trafo, overall_trafo) # to concatenate two similarities. # Note also that estimate_transform may return None. if(trafo): overall_trafo = concatenate_transform(trafo, overall_trafo) # # Return the final transformation. return overall_trafo
def get_icp_transform(world_points, iterations): # Iterate assignment and estimation of trafo a few times. # --->>> Implement your code here. # You may use the following strategy: # Start with the identity transform: overall_trafo = (1.0, 1.0, 0.0, 0.0, 0.0) # Then loop for j in xrange(iterations): # Transform the world_points using the curent overall_trafo # (see 05_b on how to do this) # Call get_correspoinding_points_on_wall(...) # Determine transformation which is needed "on top of" the current # overall_trafo: trafo = estimate_transform(...) # Concatenate the found transformation with the current overall_trafo # to obtain a new, 'combined' transformation. You may use the function # overall_trafo = concatenate_transform(trafo, overall_trafo) # to concatenate two similarities. # Note also that estimate_transform may return None. # for i in range(iterations): world_points_tr = [apply_transform(overall_trafo, p) for p in world_points] left, right = get_corresponding_points_on_wall(world_points_tr) trafo = estimate_transform(left, right, fix_scale = True) if trafo: overall_trafo = concatenate_transform(trafo, overall_trafo) # Return the final transformation. return overall_trafo
# Iterate over all positions. 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 = [ 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("F %f %f %f" % pose, file=out_file) # Write the scanner points and corresponding points. write_cylinders(out_file, "W C", world_points)
# Iterate over all positions. 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 = [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. out_file.write("F %f %f %f\n" % pose) # Write the scanner points and corresponding points. write_cylinders(out_file, "W C", world_points)