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()