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
