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