def get_synced_log_data(logdir, goal, difficulty): log = robot_fingers.TriFingerPlatformLog(os.path.join(logdir, "robot_data.dat"), os.path.join(logdir, "camera_data.dat")) log_camera = tricamera.LogReader(os.path.join(logdir, "camera_data.dat")) stamps = log_camera.timestamps obs = {'robot': [], 'cube': [], 'images': [], 't': [], 'desired_action': [], 'stamp': [], 'acc_reward': []} ind = 0 acc_reward = 0.0 for t in range(log.get_first_timeindex(), log.get_last_timeindex()): camera_observation = log.get_camera_observation(t) acc_reward -= move_cube.evaluate_state( move_cube.Pose(**goal), camera_observation.filtered_object_pose, difficulty ) if 1000 * log.get_timestamp_ms(t) >= stamps[ind]: robot_observation = log.get_robot_observation(t) obs['robot'].append(robot_observation) obs['cube'].append(camera_observation.filtered_object_pose) obs['images'].append([convert_image(camera.image) for camera in camera_observation.cameras]) obs['desired_action'].append(log.get_desired_action(t)) obs['acc_reward'].append(acc_reward) obs['t'].append(t) obs['stamp'].append(log.get_timestamp_ms(t)) ind += 1 return obs
def main(): argparser = argparse.ArgumentParser(description=__doc__) argparser.add_argument( "logfile", type=str, help="Path to the log file.", ) argparser.add_argument( "outfile", type=str, help="Path to the output file.", ) argparser.add_argument( "--camera", "-c", choices=["camera60", "camera180", "camera300"], required=True, help="Name of the camera", ) args = argparser.parse_args() log_reader = tricamera.LogReader(args.logfile) if args.camera == "camera60": camera_idx = 0 elif args.camera == "camera180": camera_idx = 1 else: camera_idx = 2 first_obs = log_reader.data[0].cameras[camera_idx] last_obs = log_reader.data[-1].cameras[camera_idx] # determine rate based on time stamps start_time = first_obs.timestamp end_time = last_obs.timestamp interval = (end_time - start_time) / len(log_reader.data) fps = 1 / interval # convert to ms interval = int(interval * 1000) # Define the codec and create VideoWriter object first_img = utils.convert_image(first_obs.image) fourcc = cv2.VideoWriter_fourcc(*"XVID") writer = cv2.VideoWriter(args.outfile, fourcc, fps, first_img.shape[:2]) print( "Loaded {} frames at an average interval of {} ms ({:.1f} fps)".format( len(log_reader.data), interval, 1000 / interval)) for observation in log_reader.data: image = utils.convert_image(observation.cameras[camera_idx].image) writer.write(image)
def main(): parser = argparse.ArgumentParser(description=__doc__) parser.add_argument( "log_dir", type=pathlib.Path, help="Path to the log file.", ) args = parser.parse_args() camera_log_file = args.log_dir / "camera_data.dat" if not args.log_dir.is_dir(): print("{} does not exist.".format(args.log_dir)) sys.exit(1) calib_files = [] for name in CAMERA_NAMES: calib_file = args.log_dir / (name + ".yml") if calib_file.exists(): calib_files.append(str(calib_file)) else: print("{} does not exist.".format(calib_file)) sys.exit(1) cube_detector = object_tracker.CubeDetector(calib_files) log_reader = tricamera.LogReader(str(camera_log_file)) for observation in log_reader.data: images = [ utils.convert_image(camera.image) for camera in observation.cameras ] pose = cube_detector.detect_cube_single_thread(images) print(pose.position) debug_img = cube_detector.create_debug_image() cv2.imshow("Debug", debug_img) # stop if either "q" or ESC is pressed if cv2.waitKey(3) in [ord("q"), 27]: # 27 = ESC break
def main(): argparser = argparse.ArgumentParser(description=__doc__) argparser.add_argument( "filename", type=str, help="""Path to the log file.""", ) argparser.add_argument( "output_dir", type=str, help="Directory to which images are stored.", ) argparser.add_argument( "--step", "-s", type=int, metavar="n", help="Extract only every n-th frame.", ) args = argparser.parse_args() out_dir = pathlib.Path(args.output_dir) if not out_dir.is_dir(): print("{} does not exist or is not a directory".format(out_dir)) sys.exit(1) log_reader = tricamera.LogReader(args.filename) for i, observation in enumerate(log_reader.data[::args.step]): observation_dir = out_dir / ("%04d" % (i + 1)) observation_dir.mkdir() for camera_idx, name in enumerate( ["camera60", "camera180", "camera300"]): image = utils.convert_image(observation.cameras[camera_idx].image) img_path = observation_dir / "{}.png".format(name) cv2.imwrite(str(img_path), image)
def main(): argparser = argparse.ArgumentParser(description=__doc__) argparser.add_argument( "input", type=str, help="""Path to the log file.""", ) argparser.add_argument( "output", type=pathlib.Path, help="Directory to which images are stored.", ) args = argparser.parse_args() log_reader = tricamera.LogReader(args.input) object_log = pandas.DataFrame(columns=( "timestamp", "pos_x", "pos_y", "pos_z", "rot_x", "rot_y", "rot_z", "rot_q", "confidence", "filtered_pos_x", "filtered_pos_y", "filtered_pos_z", "filtered_rot_x", "filtered_rot_y", "filtered_rot_z", "filtered_rot_q", "filtered_confidence", )) pbar = progressbar.ProgressBar() for i, observation in enumerate(pbar(log_reader.data)): # use timestamp of camera 0 timestamp = observation.cameras[0].timestamp object_pose = observation.object_pose filtered_object_pose = observation.filtered_object_pose row = { "timestamp": timestamp, "pos_x": object_pose.position[0], "pos_y": object_pose.position[1], "pos_z": object_pose.position[2], "rot_x": object_pose.orientation[0], "rot_y": object_pose.orientation[1], "rot_z": object_pose.orientation[2], "rot_q": object_pose.orientation[3], "confidence": object_pose.confidence, "filtered_pos_x": filtered_object_pose.position[0], "filtered_pos_y": filtered_object_pose.position[1], "filtered_pos_z": filtered_object_pose.position[2], "filtered_rot_x": filtered_object_pose.orientation[0], "filtered_rot_y": filtered_object_pose.orientation[1], "filtered_rot_z": filtered_object_pose.orientation[2], "filtered_rot_q": filtered_object_pose.orientation[3], "filtered_confidence": filtered_object_pose.confidence, } object_log = object_log.append(row, ignore_index=True) object_log.to_csv(args.output)
def main(): parser = argparse.ArgumentParser(description=__doc__) parser.add_argument( "filename", type=str, help="Path to the log file.", ) parser.add_argument( "--visualize-object-pose", "-v", action="store_true", help="""Visualize detected object pose. This expects files camera{60,180,300}.yml with calibration parameters to exist in the same directory as the given camera log file. """, ) parser.add_argument( "--visualize-goal-pose", "-g", type=pathlib.Path, metavar="GOAL_FILE", help="Visualize goal from the specified JSON file.", ) parser.add_argument( "--show-confidence", action="store_true", help="Print the object pose confidence in the images.", ) parser.add_argument( "--unfiltered", action="store_true", help="Use the unfiltered object pose.", ) parser.add_argument( "--plot-cube-position", "-p", action="store_true", help="Plot cube position", ) parser.add_argument( "--compensate-cube-offset", action="store_true", help="Compensate cube position offset in old logfiles.", ) parser.add_argument( "--save-video", type=str, metavar="VIDEO_FILE", help="""Save the images of the camera selected by --camera to a AVI video file. Expects as argument the output path. """, ) parser.add_argument( "--camera", "-c", choices=CAMERA_NAMES, help="Name of the camera. Used by --save-video.", ) args = parser.parse_args() log_file_path = pathlib.Path(args.filename) if not log_file_path.exists(): print("{} does not exist.".format(log_file_path)) sys.exit(1) if args.visualize_goal_pose: if not args.visualize_goal_pose.exists(): print("{} does not exist.".format(args.visualize_goal_pose)) sys.exit(1) with open(args.visualize_goal_pose, "r") as fh: goal_dict = json.load(fh) goal_pose = trifinger_object_tracking.py_object_tracker.ObjectPose() goal_pose.position = goal_dict["goal"]["position"] goal_pose.orientation = goal_dict["goal"]["orientation"] calib_files = [] if args.visualize_object_pose or args.visualize_goal_pose: for name in CAMERA_NAMES: calib_file = log_file_path.parent / (name + ".yml") if calib_file.exists(): calib_files.append(str(calib_file)) else: print("{} does not exist.".format(calib_file)) sys.exit(1) cube_visualizer = tricamera.CubeVisualizer(calib_files) log_reader = tricamera.LogReader(args.filename) # determine rate based on time stamps start_time = log_reader.data[0].cameras[0].timestamp end_time = log_reader.data[-1].cameras[0].timestamp interval = (end_time - start_time) / len(log_reader.data) fps = 1 / interval # convert to ms interval = int(interval * 1000) if args.save_video: if not args.camera: print("--camera is required for saving video.") sys.exit(1) camera_index = CAMERA_NAMES.index(args.camera) first_img = utils.convert_image(log_reader.data[0].cameras[0].image) fourcc = cv2.VideoWriter_fourcc(*'XVID') video_writer = cv2.VideoWriter(args.save_video, fourcc, fps, first_img.shape[:2]) print( "Loaded {} frames at an average interval of {} ms ({:.1f} fps)".format( len(log_reader.data), interval, fps)) for observation in log_reader.data: images = [ utils.convert_image(camera.image) for camera in observation.cameras ] if args.unfiltered: object_pose = observation.object_pose else: object_pose = observation.filtered_object_pose if args.compensate_cube_offset: # object_pose is read-only and unfortunately copy.copy does not # work for this type object_pose_cpy = type(object_pose)() object_pose_cpy.position = np.array(object_pose.position) object_pose_cpy.orientation = np.array(object_pose.orientation) object_pose_cpy.confidence = object_pose.confidence object_pose = object_pose_cpy offset = np.array([0, 0, 0.0325]) cube_rot = Rotation.from_quat(object_pose.orientation) object_pose.position = object_pose.position + cube_rot.apply( offset) if args.visualize_goal_pose: cvmats = [trifinger_cameras.camera.cvMat(img) for img in images] images = cube_visualizer.draw_cube(cvmats, goal_pose, True) images = [np.array(img) for img in images] if args.visualize_object_pose: cvmats = [trifinger_cameras.camera.cvMat(img) for img in images] images = cube_visualizer.draw_cube(cvmats, object_pose, False) images = [np.array(img) for img in images] if args.show_confidence: images = [ cv2.putText(image, "confidence: %.2f" % object_pose.confidence, (0, image.shape[0] - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 0)) for image in images ] if args.save_video: video_writer.write(images[camera_index]) else: for i, name in enumerate(CAMERA_NAMES): cv2.imshow(name, images[i]) # stop if either "q" or ESC is pressed if cv2.waitKey(interval) in [ord("q"), 27]: # 27 = ESC break if args.plot_cube_position: plt.scatter(observation.cameras[0].timestamp, object_pose.position[0], color="red") plt.scatter(observation.cameras[0].timestamp, object_pose.position[1], color="green") plt.scatter(observation.cameras[0].timestamp, object_pose.position[2], color="blue") plt.title("Cube Position") legend_elements = [ Line2D([0], [0], marker='o', color='w', label='x', markerfacecolor='r'), Line2D([0], [0], marker='o', color='w', label='y', markerfacecolor='g'), Line2D([0], [0], marker='o', color='w', label='z', markerfacecolor='b'), ] plt.legend(handles=legend_elements, loc="upper right") plt.pause(0.01)
def main(): argparser = argparse.ArgumentParser(description=__doc__) argparser.add_argument( "filename", type=str, help="""Path to the log file.""", ) args = argparser.parse_args() log_reader = tricamera.LogReader(args.filename) # determine rate based on time stamps start_time = log_reader.data[0].cameras[0].timestamp end_time = log_reader.data[-1].cameras[0].timestamp duration = end_time - start_time interval = duration / len(log_reader.data) # convert to ms interval = int(interval * 1000) print( "Loaded {} frames at an average interval of {} ms ({:.1f} fps)".format( len(log_reader.data), interval, 1000 / interval)) print("Total duration: {:.1f} seconds".format(duration)) time_steps = [] prev_stamp = start_time for observation in log_reader.data[1:]: step = observation.cameras[0].timestamp - prev_stamp prev_stamp = observation.cameras[0].timestamp time_steps.append(step) time_steps = np.array(time_steps) print("Mean interval:", time_steps.mean()) print("Median interval:", np.median(time_steps)) stamps = [ np.array([ observation.cameras[c].timestamp for observation in log_reader.data ]) for c in range(3) ] fig, axes = plt.subplots(2, 3) for i in range(3): axes[0, i].plot(stamps[i]) axes[0, i].set_title("camera {}".format(i)) for i, (a, b) in enumerate(((0, 1), (0, 2), (1, 2))): # convert diffs to miliseconds diffs = (stamps[a] - stamps[b]) * 1000 axes[1, i].plot(diffs) axes[1, i].set_title("diff {} - {}".format(a, b)) axes[1, i].set_ylabel("Milliseconds") diffs = np.abs(diffs) print("Time differences {} - {}".format(a, b)) print("\tmean: {:.4f} ms".format(diffs.mean())) print("\tmin: {:.4f} ms".format(diffs.min())) print("\tmax: {:.4f} ms".format(diffs.max())) plt.show()