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)