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
# 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)
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
for k in range(
1): #PROBABLY REMOVE THE FOR LOOP, ITERATIONS FOR ICP
print "iter ", k
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"
r = 0
best_r = 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)):
#numPoints += 1
corr_found = False
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
dist = 100.
r_count = 0
# Loop thru landmarks and find closest landmark point to current point
ref_type = None
while (r_count < self.nLandmarks):
ref = self.landmarks[r]
pot_dist, ref_pt = self.get_dist(
ref, (pt_x, pt_y),
self.max_correlation_dist)
if (pot_dist < dist and not (ref_pt == None)):
dist = pot_dist
best_r = r
if (not corr_found):
corr_found = True
left_list.append((pt_x, pt_y))
right_list.append(ref_pt)
ref_type = ref.ref_type
ref_type_list.append(ref_type)
nc += 1
else:
right_list[nc - 1] = ref_pt
ref_type = ref.ref_type
ref_type_list[nc - 1] = ref_type
#if(dist < self.FOUND_DIST_THRESH): # BAD BECAUSE THE EAST VS. WEST SHED WALL IS CLOSE
# break
r = (r + 1) % (self.nLandmarks)
r_count += 1
r = best_r
if (not ref_type == None):
if (not ref_type == 'horz'):
delta_x_count += 1
delta_x_sum += right_list[nc - 1][0] - pt_x
if (not ref_type == 'vert'):
delta_y_count += 1
delta_y_sum += right_list[nc - 1][1] - pt_y
print "numPoints: ", numPoints
print "delta_x_count, delta_x_sum: ", delta_x_count, delta_x_sum
print "delta_y_count, delta_y_sum: ", delta_y_count, delta_y_sum
print "lx, ly, rx, ry"
#~ for pk, ref_type in enumerate(ref_type_list):
#~ if(ref_type == 'horz' and delta_x_count > 0):
#~ right_list[pk] = (right_list[pk][0]+delta_x_sum/delta_x_count, right_list[pk][1])
#~ if(ref_type == 'vert' and delta_y_count > 0):
#~ right_list[pk] = (right_list[pk][0], right_list[pk][1]+delta_y_sum/delta_y_count)
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):
trafo = estimate_transform(left_list,
right_list,
fix_scale=True)
cur_dist_sum = 0.
new_dist_sum = 0.
if (not trafo == None):
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 ((not trafo == None)
and (new_dist_sum < cur_dist_sum + 1.0) 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 "lx, ly, rx, ry"
#for l,r in zip(left_list,right_list):
# print l[0], ',', l[1], ',', r[0], ',', r[1]
print "trafo:"
print trafo
la, c, s, dx, dy = trafo
else:
print "exit for loop, iter ", k
break
#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)
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 "\nNEW POINT CLOUD"
r = 0
best_r = 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.
line_segs = []
line_seg_pts = []
nSegPts = 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 k, (point_map, point_laser) in enumerate(
zip(pcl2.read_points(pc_map, skip_nans=True),
pcl2.read_points(msg, skip_nans=True))):
corr_found = False
range_sqd = point_laser[0]**2 + point_laser[
1]**2 # only if you loop thru local laser points
if (nSegPts == 0):
segPtA = point_map
segPt1 = point_map
line_seg_pts.append(segPtA)
ang_cur = 0.
nSegPts = 1
elif (range_sqd > 0.25 and range_sqd < 400.0):
segPtB = point_map
ang = self.angle(segPt1, segPtB)
sub_seg_dist_sqd = (segPtB[0] - segPt1[0])**2 + (
segPtB[1] - segPt1[1])**2
angA = self.angle(segPtA, segPtB)
#or (nSegPts > 2 and abs(angA-ang_cur) < self.angle_eps_rad) # too easy for corner transition to blend with prev seg
if (sub_seg_dist_sqd < self.short_gap**2):
angle_eps = self.angle_eps_short_rad
else:
angle_eps = self.angle_eps_long_rad
# temp debug
#~ print "DEBUG LINE SEGMENTS"
#~ print "pt1: ", segPt1
#~ print "ptB: ", segPtB
#~ print "ang: ", ang, ", sub_dist: ", math.sqrt(sub_seg_dist_sqd)
#~ print "ang diff: ", self.get_abs_angle_diff(ang,ang_cur)
#~ print "END DEBUG LINE SEGMENTS"
if (
(nSegPts == 1 or
self.get_abs_angle_diff(ang, ang_cur) < angle_eps)
and sub_seg_dist_sqd < 8.**2
): # and segPtB[3]-segPt1[3] <= 2 # not available with bag data from scanse
line_seg_pts.append(segPtB)
nSegPts += 1
#~ print "SEG Pt PASSED"
if (nSegPts == 2):
ang_cur = ang # ang_cur = ang_cur*(1-alpha) + ang*alpha #only if nSegPts > 2
else:
if (nSegPts >= self.min_nSegPts):
line_segs.append(
np.array([[segPtA[0], segPtA[1]],
[segPt1[0], segPt1[1]]]))
print "FOUND LINE SEGMENT, nSegPts: ", nSegPts, "angle: ", self.angle(
segPtA, segPt1)
print line_segs[-1]
for pk in range(nSegPts):
print line_seg_pts[pk]
segPtA = segPt1
ang_cur = ang
line_seg_pts = []
if (sub_seg_dist_sqd < 8.**2):
nSegPts = 2
line_seg_pts.append(segPtA)
line_seg_pts.append(segPtB)
else:
nSegPts = 1
segPtA = segPtB
line_seg_pts.append(segPtA)
segPt1 = segPtB
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.1):
#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
dist = 100.
r_count = 0
# Loop thru landmarks and find closest landmark point to current point
ref_type = None
while (r_count < self.nLandmarks):
ref = self.landmarks[r]
pot_dist, ref_pt = self.get_dist(
ref, (pt_x, pt_y), self.max_correlation_dist)
if (pot_dist < dist and not (ref_pt == None)):
dist = pot_dist
best_r = r
if (not corr_found):
corr_found = True
left_list.append((pt_x, pt_y))
right_list.append(ref_pt)
ref_type = ref.ref_type
ref_type_list.append(ref_type)
nc += 1
else:
right_list[nc - 1] = ref_pt
ref_type = ref.ref_type
ref_type_list[nc - 1] = ref_type
#if(dist < self.FOUND_DIST_THRESH): # BAD BECAUSE THE EAST VS. WEST SHED WALL IS CLOSE
# break
r = (r + 1) % (self.nLandmarks)
r_count += 1
r = best_r
if (not ref_type == None):
if (not ref_type == 'horz'):
delta_x_count += 1
delta_x_sum += right_list[nc - 1][0] - pt_x
if (not ref_type == 'vert'):
delta_y_count += 1
delta_y_sum += right_list[nc - 1][1] - pt_y
print "numPoints: ", numPoints
print "delta_x_count, delta_x_sum: ", delta_x_count, delta_x_sum
print "delta_y_count, delta_y_sum: ", delta_y_count, delta_y_sum
print "lx, ly, rx, ry"
#~ for pk, ref_type in enumerate(ref_type_list):
#~ if(ref_type == 'horz' and delta_x_count > 0):
#~ right_list[pk] = (right_list[pk][0]+delta_x_sum/delta_x_count, right_list[pk][1])
#~ if(ref_type == 'vert' and delta_y_count > 0):
#~ right_list[pk] = (right_list[pk][0], right_list[pk][1]+delta_y_sum/delta_y_count)
#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):
trafo = estimate_transform(left_list,
right_list,
fix_scale=True)
cur_dist_sum = 0.
new_dist_sum = 0.
if (not trafo == None):
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 ((not trafo == None)
and (new_dist_sum < cur_dist_sum + 1.0) 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 "lx, ly, rx, ry"
#for l,r in zip(left_list,right_list):
# print l[0], ',', l[1], ',', r[0], ',', r[1]
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)
self.update_pose()
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)
# 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)
out_file.close()
