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): # 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
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. # # Return the final transformation. # Init the trafo overall_trafo = (1.0, 1.0, 0.0, 0.0, 0.0) # Iterative Closest Points for j in xrange(iterations): # transform the points from LiDAR on the wall to the new estimation world_points_new = [ apply_transform(overall_trafo, p) for p in world_points ] # find the corresponding points on the wall left, right = get_corresponding_points_on_wall(world_points_new) # find the trafo for the next iteration trafo = estimate_transform(left, right, fix_scale=True) # concatenate the trafo ,like trafo chain if trafo: overall_trafo = concatenate_transform(trafo, overall_trafo) else: break 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. overall_trafo = (1.0, 1.0, 0.0, 0.0, 0.0) for j in xrange(iterations): if overall_trafo: new_world_points = [ apply_transform(overall_trafo, p) for p in world_points ] #hier the name of parameter should be changed, because if not the get_corresponding_points_on_wall method will still ues the #world_points, when the overall_trafo == False. #Tipps: check for the parameter name when wrong data occur and you just do the copy-paste work else: new_world_points = [] left, right = get_corresponding_points_on_wall(new_world_points) trafo = estimate_transform(left, right) 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. overall_trafo = (1.0,1.0,0.0,0.0,0.0) #pose = (1850.0, 1897.0, 3.717551306747922) world_points = [apply_transform(overall_trafo, p) for p in world_points] for j in xrange(iterations): l_list, r_list = get_corresponding_points_on_wall(world_points) new_trafo = estimate_transform(l_list, r_list, fix_scale = False) if new_trafo: overall_trafo = concatenate_transform(new_trafo,overall_trafo) if new_trafo: world_points = [apply_transform(new_trafo, p) for p in world_points] else: world_points =[] # Return the final transformation. return overall_trafo