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 trafo_eval(self, trafo, left_list, right_list):
cur_dist_sum = 0
new_dist_sum = 0
for l, r in zip(left_list, right_list):
cur_dist = math.sqrt((l[0] - r[0])**2 + (l[1] - r[1])**2)
cur_dist_sum += cur_dist
new_l = apply_transform(trafo, (l[0], l[1]))
new_dist = math.sqrt((new_l[0] - r[0])**2 + (new_l[1] - r[1])**2)
new_dist_sum += new_dist
print "cur_sum, new_sum: ", cur_dist_sum, ", ", new_dist_sum
return cur_dist_sum, new_dist_sum
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)
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 = 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()
def cloud_callback(self, msg):
#self.cloud_count += 1
#print "Cloud has ", len(msg.data), " points" //len(msg.data) does not give num points in cloud. It is encoded differently.
if (self.cloud_count <= 1
): #TEMPORARY JUST TO TESTS MULTIPLE ITERATIONS WITH ONE CLOUD
dx = 0.0
dy = 0.0
s = 0.
c = 1.
try:
# TRANSFORM THE LASER POINTCLOUD FROM THE LASER FRAME TO MAP FRAME...
# LOOKUP THE TRANSFORM AT THE TIME THAT CORRESPONDNS WITH THE POINTCLOUD DATA (msg.header.stamp)
# IF MUTLIPLE ITERATIONS ARE DONE FOR ONE POINTCLOUD MESSAGE, YOU WILL NEED TO USE A DIFFERENT TIME
# IF YOU WANT TO TRY TO USE update_pose() EACH ITERATION
# MAYBE YOU CAN JUST DEFINE self.transform properly using self.odom_x,y,theta
# ANOTHER OPTION MAY BE TO DO THE ITERATIONS AFTER left_list IS FILLED AND JUST TRANSFORM left_list
#if(self.transform == None):
# not working with bag file. Requires extrapolation into future ( < 50 msec)
# tf MessageFilter or waitForTransform?
#self.transform = self.tf_buffer.lookup_transform("map","laser", msg.header.stamp) # Works with nav_sim, not bag, not jeep live
self.transform = self.tf_buffer.lookup_transform(
"map", "laser",
rospy.Time()) # rospy.Time(0) requests latest
#self.transform = self.tf_buffer.waitForTransform("map","laser", msg.header.stamp)
pc_map = do_transform_cloud(msg, self.transform)
#self.cloud_pub.publish(pc_map) #Temporary for validating the transformed point cloud
print "NEW POINT CLOUD"
rk = 0
best_rk = 0
left_list = []
right_list = []
ref_type_list = []
nc = 0
numPoints = 0
prev_x, prev_y = 0., 0. # THESE DUPLICATE POINTS MOSTLY COME FROM READINGS AT LOCAL LASER COORDINATE (0,0)
delta_x_sum = 0.
delta_y_sum = 0.
delta_x_count = 0.
delta_y_count = 0.
#for point_map in pcl2.read_points(pc_map, skip_nans=True):
# for points in both (map frame) and (in laser frame directly from scan msg)
for point_map, point_laser in zip(
pcl2.read_points(pc_map, skip_nans=True),
pcl2.read_points(msg, skip_nans=True)):
range_sqd = point_laser[0]**2 + point_laser[
1]**2 # only if you loop thru local laser points
if ((point_map[2] > self.min_scan_elev)
and (range_sqd > 0.25 and range_sqd < 900.0)):
numPoints += 1
pt_x = point_map[0]
pt_y = point_map[1]
if (numPoints > 1):
dist_sqd = (pt_x - prev_x)**2 + (pt_y - prev_y)**2
if (dist_sqd < 0.3):
print "duplicate point ", pt_x, ", ", pt_y
continue
prev_x = pt_x
prev_y = pt_y
#pt_z = point_map[2]
#print pt_x, pt_y
rk, ref_pt, ref = self.find_match(rk, pt_x, pt_y)
if (not ref == None):
left_list.append((pt_x, pt_y))
right_list.append(ref_pt)
ref_type_list.append(ref.ref_type)
print "numPoints: ", numPoints
print "lx, ly, rx, ry"
for l, r in zip(left_list, right_list):
print l[0], ',', l[1], ',', r[0], ',', r[1]
if (len(left_list) >= self.min_point_pairs):
cum_trafo = (1., 1., 0., 0., 0.)
trafo = estimate_transform(left_list,
right_list,
fix_scale=True)
if (not trafo == None):
cum_trafo = concatenate_transform(trafo, cum_trafo)
for j in xrange(5):
rk = 0
new_left_list = [
apply_transform(cum_trafo, p) for p in left_list
]
left_list = []
right_list = []
ref_type_list = [] # currently not used
for pk, left in enumerate(new_left_list):
rk, ref_pt, ref = self.find_match(
rk, left[0], left[1])
if (not ref == None):
left_list.append((left[0], left[1]))
right_list.append(ref_pt)
ref_type_list.append(ref.ref_type)
trafo = estimate_transform(left_list,
right_list,
fix_scale=True)
if (not trafo == None):
cum_trafo = concatenate_transform(trafo, cum_trafo)
trafo = cum_trafo
print "AFTER ITERS: lx, ly, rx, ry"
for l, r in zip(left_list, right_list):
print l[0], ',', l[1], ',', r[0], ',', r[1]
cur_dist_sum, new_dist_sum = self.trafo_eval(
trafo, left_list, right_list)
if (self.first_scan):
self.first_scan = False
self.prev_eval_dist_sum = cur_dist_sum
if (
(new_dist_sum < cur_dist_sum)
): #and (abs(cur_dist_sum - self.prev_eval_dist_sum) < 30.) ):
self.prev_eval_dist_sum = cur_dist_sum
print "enough points, better trafo"
if (abs(trafo[2]) < self.MAX_DELTA_THETA_RAD
and abs(trafo[3]) < self.MAX_DELTA_X
and abs(trafo[4]) < self.MAX_DELTA_Y):
print "trafo:"
print trafo
la, c, s, dx, dy = trafo
#except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
except Exception as e:
print "tf issue"
print repr(e)
pass
print c, s, dx, dy
#Complementary, low pass, filter: filt = filt*(1-alpha) + raw*(alpha)
alpha = self.alpha
x = self.odom_x
y = self.odom_y
raw_x = c * x - s * y + dx
self.odom_x = x * (1. - alpha) + raw_x * (alpha)
raw_y = s * x + c * y + dy
self.odom_y = y * (1. - alpha) + raw_y * (alpha)
self.odom_theta = self.odom_theta * (1. - alpha) + math.atan2(
s, c) * (alpha)
