# Also read the reference cylinders (the map). logfile.read("robot_arena_landmarks.txt") reference_cylinders = [l[1:3] for l in logfile.landmarks] # Iterate over all positions. out_file = file("find_cylinder_pairs.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) # Extract cylinders, also convert them to world coordinates. cartesian_cylinders = compute_scanner_cylinders( logfile.scan_data[i], depth_jump, minimum_valid_distance, cylinder_offset) world_cylinders = [LegoLogfile.scanner_to_world(pose, c) for c in cartesian_cylinders] # For every cylinder, find the closest reference cylinder. cylinder_pairs = find_cylinder_pairs( world_cylinders, reference_cylinders, max_cylinder_distance) # Write to file. # The pose. print >> out_file, "F %f %f %f" % pose # The detected cylinders in the scanner's coordinate system. write_cylinders(out_file, "D C", cartesian_cylinders) # The reference cylinders which were part of a cylinder pair. write_cylinders(out_file, "W C",
logfile.read("robot4_motors.txt") logfile.read("robot4_scan.txt") # Also read the reference cylinders (this is our map). logfile.read("robot_arena_landmarks.txt") reference_cylinders = [l[1:3] for l in logfile.landmarks] out_file = file("apply_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) # Extract cylinders, also convert them to world coordinates. cartesian_cylinders = compute_scanner_cylinders( logfile.scan_data[i], depth_jump, minimum_valid_distance, cylinder_offset) world_cylinders = [ LegoLogfile.scanner_to_world(pose, c) for c in cartesian_cylinders ] # For every cylinder, find the closest reference cylinder. cylinder_pairs = find_cylinder_pairs(world_cylinders, reference_cylinders, max_cylinder_distance) # Estimate a transformation using the cylinder pairs. trafo = estimate_transform( [world_cylinders[pair[0]] for pair in cylinder_pairs], [reference_cylinders[pair[1]] for pair in cylinder_pairs], fix_scale=True)