def create_test_set(current_state, pose_client, airsim_client, potential_states_fetcher, file_manager, my_rng):
date_time_name = time.strftime("%Y-%m-%d-%H-%M")
print("experiment began at:", date_time_name)
#synth_dataset_client = Synth_Dataset_Client(airsim_client.length_of_simulation, file_manager.test_sets_loc)
files = get_synth_dataset_filenames(file_manager.test_sets_loc, file_manager.anim_num)
gt_poses_file = open( files["f_groundtruth_poses"], "w")
gt_poses_file.write("linecount\tanim_time\n")
camera_pos_file = open(files["f_camera_pos"], "w")
file_manager.init_photo_loc_dir(files["image_main_dir"])
for viewpoint in range(18):
if not os.path.exists(files["image_main_dir"]+"/camera_"+str(viewpoint)):
os.makedirs(files["image_main_dir"]+"/camera_"+str(viewpoint))
file_manager.save_intrinsics(airsim_client.intrinsics, files["f_intrinsics"])
airsim_retrieve_gt(airsim_client, pose_client, current_state, file_manager)
pose_client.immediate_future_pose = current_state.bone_pos_gt
pose_client.current_pose = current_state.bone_pos_gt
potential_states_fetcher.reset(pose_client, airsim_client, current_state)
potential_states_fetcher.get_potential_positions(airsim_client.linecount)
anim_time = airsim_client.getAnimationTime()
prev_drone_pose = torch.zeros([3,1])
num_of_loc = len(potential_states_fetcher.potential_trajectory_list)
while airsim_client.linecount < airsim_client.length_of_simulation:
file_manager.save_gt_values_dataset(airsim_client.linecount, anim_time, current_state.bone_pos_gt, gt_poses_file)
for trajectory_ind in range(num_of_loc):
potential_states_fetcher.restart_trajectory()
goal_traj = potential_states_fetcher.choose_trajectory_using_trajind(trajectory_ind)
set_position(goal_traj, airsim_client, current_state, pose_client, potential_states_fetcher, loop_mode=potential_states_fetcher.loop_mode)
take_photo(airsim_client, pose_client, current_state, file_manager, trajectory_ind)
new_drone_pose = current_state.C_drone_gt.clone()
assert not torch.allclose(prev_drone_pose, new_drone_pose)
prev_drone_pose = new_drone_pose.clone()
file_manager.prepare_test_set_gt(current_state, airsim_client.linecount, trajectory_ind, camera_pos_file)
airsim_retrieve_gt(airsim_client, pose_client, current_state, file_manager)
pose_client.immediate_future_pose = current_state.bone_pos_gt
pose_client.current_pose = current_state.bone_pos_gt
potential_states_fetcher.reset(pose_client, airsim_client, current_state)
potential_states_fetcher.get_potential_positions(airsim_client.linecount)
airsim_client.increment_linecount(pose_client.is_calibrating_energy)
update_animation(airsim_client, pose_client, current_state)
anim_time = airsim_client.getAnimationTime()
def save_gt_poses_loop(current_state, pose_client, airsim_client, file_manager):
prev_pose_3d_gt = np.zeros([3, pose_client.num_of_joints])
while airsim_client.linecount < 280:
#update state values read from AirSim and take picture
airsim_retrieve_gt(airsim_client, pose_client, current_state, file_manager)
take_photo(airsim_client, pose_client, current_state, file_manager)
assert not np.allclose(prev_pose_3d_gt, current_state.bone_pos_gt)
anim_time = airsim_client.getAnimationTime()
file_manager.write_gt_pose_values(anim_time, current_state.bone_pos_gt)
print(current_state.bone_pos_gt[:,0])
prev_pose_3d_gt = current_state.bone_pos_gt.copy()
start1=time.time()
update_animation(airsim_client, pose_client, current_state, delta_t=0.1)
print("updating animation took", time.time()-start1)
airsim_client.increment_linecount(pose_client.is_calibrating_energy)
def openpose_liftnet(current_state, pose_client, airsim_client, potential_states_fetcher, file_manager, parameters, rng_object):
airsim_client.framecount = 940
airsim_client.chosen_cam_view = 8
image_dir = "/cvlabsrc1/home/kicirogl/ActiveDrone/test_sets/mpi_inf_3dhp/camera_"+str(airsim_client.chosen_cam_view) +"/img_" +str(airsim_client.framecount) +".jpg"
print(file_manager.photo_loc)
take_photo(airsim_client, pose_client, current_state, file_manager)
pose_client.modes["mode_2d"] = "gt_with_noise" #set this back
noisy_gt, pose_2d_gt_cropped, heatmap_2d, cropped_image = determine_2d_positions(pose_client=pose_client,
current_state=current_state, my_rng=rng_object,
file_manager=file_manager, linecount=airsim_client.linecount)
pose_client.modes["mode_2d"] = "openpose" #set this back
openpose_res, pose_2d_gt_cropped, heatmap_2d, cropped_image = determine_2d_positions(pose_client=pose_client,
current_state=current_state, my_rng=rng_object,
file_manager=file_manager, linecount=airsim_client.linecount)
#find relative 3d pose using liftnet
pose_client.modes["mode_lift"] = "lift" #set this back
pose3d_lift_directions = determine_relative_3d_pose(pose_client=pose_client, current_state=current_state,
my_rng=rng_object, pose_2d=openpose_res, cropped_image=cropped_image,
heatmap_2d=heatmap_2d, file_manager=file_manager)
pose_client.modes["mode_lift"] = "gt"
#find relative 3d pose using gt
pose3d_lift_directions_gt = determine_relative_3d_pose(pose_client=pose_client, current_state=current_state,
my_rng=rng_object, pose_2d=noisy_gt, cropped_image=cropped_image,
heatmap_2d=heatmap_2d, file_manager=file_manager)
pose_client.modes["mode_lift"] = "gt_with_noise"
pose3d_lift_directions_gt_with_noise = determine_relative_3d_pose(pose_client=pose_client, current_state=current_state,
my_rng=rng_object, pose_2d=noisy_gt, cropped_image=cropped_image,
heatmap_2d=heatmap_2d, file_manager=file_manager)
plot_single_human(pose3d_lift_directions_gt.numpy(), file_manager.plot_loc, "lift_gt", pose_client.bone_connections)
plot_single_human(pose3d_lift_directions_gt_with_noise.numpy(), file_manager.plot_loc, "lift_gt_with_noise", pose_client.bone_connections)
plot_single_human(pose3d_lift_directions.numpy(), file_manager.plot_loc, "liftnet", pose_client.bone_connections)
plot_2d_projection(noisy_gt, file_manager.plot_loc, 0, pose_client.bone_connections, custom_name="2d_noisy_gt")
plot_2d_projection(pose_2d_gt_cropped, file_manager.plot_loc, 0, pose_client.bone_connections, custom_name="2d_gt")
plot_2d_projection(openpose_res, file_manager.plot_loc, 0, pose_client.bone_connections, custom_name="openpose")
def openpose_liftnet_other(current_state, pose_client, airsim_client, potential_states_fetcher, file_manager, parameters):
potential_states_fetcher.reset(pose_client, airsim_client, current_state)
potential_states_fetcher.get_potential_positions(airsim_client.linecount)
num_of_loc = len(potential_states_fetcher.potential_trajectory_list)
len_of_sim = 80
validation_anims = ["28_19", "06_13", "13_06"]
openpose_array = np.zeros([len(validation_anims)*len_of_sim*num_of_loc, 2, 15])
gt_2d_array = np.zeros([len(validation_anims)*len_of_sim*num_of_loc, 2, 15])
lift_array = np.zeros([len(validation_anims)*len_of_sim*num_of_loc, 3, 15])
gt_lift_array = np.zeros([len(validation_anims)*len_of_sim*num_of_loc, 3, 15])
for anim_num in range(len(validation_anims)):
parameters["ANIMATION_NUM"] = validation_anims[anim_num]
file_manager = FileManager(parameters, get_bone_len_file_name(pose_client.modes))
current_state.update_animation_gt_array(file_manager.f_anim_gt_array)
airsim_client.changeAnimation(ANIM_TO_UNREAL[file_manager.anim_num])
current_state.init_anim_time(airsim_client.default_initial_anim_time, file_manager.anim_num)
set_animation_to_frame(airsim_client, pose_client, current_state, airsim_client.default_initial_anim_time)
prev_drone_pose = torch.zeros([3,1])
airsim_client.linecount = 0
while airsim_client.linecount < len_of_sim and anim_num != 0:
airsim_retrieve_gt(airsim_client, pose_client, current_state, file_manager)
pose_client.immediate_future_pose = current_state.bone_pos_gt
pose_client.current_pose = current_state.bone_pos_gt
potential_states_fetcher.reset(pose_client, airsim_client, current_state)
potential_states_fetcher.get_potential_positions(airsim_client.linecount)
anim_time = airsim_client.getAnimationTime()
for trajectory_ind in range(num_of_loc):
# print(" ** traj ind", trajectory_ind)
index = anim_num*len_of_sim*num_of_loc+airsim_client.linecount*num_of_loc+trajectory_ind
#move
potential_states_fetcher.restart_trajectory()
goal_traj = potential_states_fetcher.choose_trajectory_using_trajind(trajectory_ind)
set_position(goal_traj, airsim_client, current_state, pose_client, potential_states_fetcher, loop_mode=potential_states_fetcher.loop_mode)
#update state values read from AirSim and take picture
take_photo(airsim_client, pose_client, current_state, file_manager)
new_drone_pose = current_state.C_drone_gt.clone()
assert not torch.allclose(prev_drone_pose, new_drone_pose)
prev_drone_pose = new_drone_pose.clone()
#find 2d pose (using openpose and gt)
pose_2d_cropped, pose_2d_gt_cropped, heatmap_2d, cropped_image = determine_2d_positions(pose_client=pose_client,
current_state=current_state, my_rng=None,
file_manager=file_manager, linecount=airsim_client.linecount)
openpose_array[index, :, :] = pose_2d_cropped.numpy().copy()
gt_2d_array[index, :, :] = pose_2d_gt_cropped.numpy().copy()
#find relative 3d pose using liftnet
pose_client.modes["mode_lift"] = "lift" #set this back
pose3d_lift_directions = determine_relative_3d_pose(pose_client=pose_client, current_state=current_state,
my_rng=None, pose_2d=pose_2d_cropped, cropped_image=cropped_image,
heatmap_2d=heatmap_2d, file_manager=file_manager)
lift_array[index, :, :] = pose3d_lift_directions.numpy().copy()
pose_client.modes["mode_lift"] = "gt"
#find relative 3d pose using gt
pose3d_lift_directions_gt = determine_relative_3d_pose(pose_client=pose_client, current_state=current_state,
my_rng=None, pose_2d=pose_2d_cropped, cropped_image=cropped_image,
heatmap_2d=heatmap_2d, file_manager=file_manager)
gt_lift_array[index, :, :] = pose3d_lift_directions_gt.numpy().copy()
file_manager.save_openpose_and_gt2d(openpose_array, gt_2d_array)
file_manager.save_lift_and_gtlift(lift_array, gt_lift_array)
update_animation(airsim_client, pose_client, current_state, delta_t=0.2)
airsim_client.increment_linecount(pose_client.is_calibrating_energy)
def general_simulation_loop(current_state, pose_client, airsim_client, potential_states_fetcher, file_manager, parameters, my_rng):
"""
Description:
General simulation loop
Inputs:
current_state: an object of type State
pose_client: an object of type PoseEstimationClient
airsim_client: an object of type VehicleClient or DroneFlightClient
potential_states_fetcher: an object of type PotentialStatesFetcher
file_manager: object of type FileManager
"""
num_of_noise_trials = parameters["NUM_OF_NOISE_TRIALS"]
if pose_client.modes["mode_2d"] == "openpose":
num_of_noise_trials = 1
find_best_traj = parameters["FIND_BEST_TRAJ"]
take_photo(airsim_client, pose_client, current_state, file_manager)
initialize_empty_frames(airsim_client.linecount, pose_client, current_state, file_manager, my_rng)
set_animation_to_frame(airsim_client, pose_client, current_state, airsim_client.default_initial_anim_time)
potential_states_fetcher.reset(pose_client, airsim_client, current_state)
potential_states_fetcher.get_potential_positions(airsim_client.linecount)
while airsim_client.linecount < airsim_client.length_of_simulation:
## if we have find_best_traj set to True, we also need to evaluate the
## error at each trajectory. This may take a long time!
if find_best_traj and not pose_client.is_calibrating_energy:
start_best_sim = time.time()
my_rng.freeze_all_rng_states()
current_anim_time = airsim_client.getAnimationTime()
state_copy = current_state.deepcopy_state()
update_animation(airsim_client, pose_client.deepcopy_PEC(0), state_copy)
future_anim_time = state_copy.anim_time
## find goal location
for trajectory_ind in range(0, len(potential_states_fetcher.potential_trajectory_list)):
my_rng.reload_all_rng_states()
goal_traj = potential_states_fetcher.choose_trajectory_using_trajind(trajectory_ind)
my_rng_oracle = my_rng
## We run this for several different noises and average the results
## This makes sure our oracle is noise-independent.
for trial_ind in range(num_of_noise_trials):
potential_states_fetcher.restart_trajectory()
pose_client_copy = pose_client.deepcopy_PEC(trial_ind)
state_copy = current_state.deepcopy_state()
state_copy.update_anim_time(future_anim_time)
goal_trajectory = potential_states_fetcher.goal_trajectory
set_position(goal_trajectory, airsim_client, state_copy, pose_client_copy, potential_states_fetcher, loop_mode=potential_states_fetcher.loop_mode)
take_photo(airsim_client, pose_client_copy, state_copy, file_manager)
determine_positions(airsim_client.linecount, pose_client_copy, state_copy, file_manager, my_rng_oracle)
goal_traj.record_error_for_trial(pose_client_copy.FUTURE_WINDOW_SIZE-1, pose_client_copy.average_errors[pose_client_copy.MIDDLE_POSE_INDEX], pose_client_copy.overall_error)
goal_traj.find_overall_error()
file_manager.record_oracle_errors(airsim_client.linecount, potential_states_fetcher.potential_trajectory_list)
potential_states_fetcher.restart_trajectory()
my_rng.reload_all_rng_states()
set_animation_to_frame(airsim_client, pose_client, current_state, current_anim_time)
end_best_sim = time.time()
print("Simulating errors for all locations took", end_best_sim-start_best_sim, "seconds")
if potential_states_fetcher.trajectory == "oracle":
min_error = np.inf
for trajectory_ind in range(0, len(potential_states_fetcher.potential_trajectory_list)):
traj = potential_states_fetcher.potential_trajectory_list[trajectory_ind]
if traj.error_middle < min_error:
potential_states_fetcher.oracle_traj_ind=trajectory_ind
min_error = traj.error_middle
print("traj ind", trajectory_ind, "error", traj.error_middle)
print("chosen traj", potential_states_fetcher.oracle_traj_ind, "with error", min_error)
#find goal location
if airsim_client.linecount != 0:
goal_trajectory = potential_states_fetcher.choose_trajectory(pose_client, airsim_client.linecount, airsim_client.online_linecount, file_manager, my_rng)
print("chose trajectory", goal_trajectory.trajectory_index)
#move there
update_animation(airsim_client, pose_client, current_state)
set_position(goal_trajectory, airsim_client, current_state, pose_client, potential_states_fetcher, loop_mode=potential_states_fetcher.loop_mode)
file_manager.record_chosen_trajectory(airsim_client.linecount, goal_trajectory.trajectory_index)
#update state values read from AirSim and take picture
take_photo(airsim_client, pose_client, current_state, file_manager)
#find human pose
determine_positions(airsim_client.linecount, pose_client, current_state, file_manager, my_rng)
#plotting
if not pose_client.quiet and airsim_client.linecount > 0:
plot_drone_traj(pose_client, file_manager.plot_loc, airsim_client.linecount, pose_client.animation)
file_manager.write_error_values(pose_client.average_errors, airsim_client.linecount)
file_manager.write_distance_values(current_state.distances_travelled, current_state.total_distance_travelled, airsim_client.linecount)
potential_states_fetcher.reset(pose_client, airsim_client, current_state)
potential_states_fetcher.get_potential_positions(airsim_client.linecount)
airsim_client.increment_linecount(pose_client.is_calibrating_energy)
print('Progress:', airsim_client.linecount, "/", airsim_client.length_of_simulation)
def general_simulation_loop(current_state, pose_client, airsim_client,
potential_states_fetcher, file_manager, parameters,
my_rng):
"""
Description:
General simulation loop
Inputs:
current_state: an object of type State
pose_client: an object of type PoseEstimationClient
airsim_client: an object of type VehicleClient or DroneFlightClient
potential_states_fetcher: an object of type PotentialStatesFetcher
file_manager: object of type FileManager
"""
num_of_noise_trials = parameters["NUM_OF_NOISE_TRIALS"]
if pose_client.modes["mode_2d"] == "openpose":
num_of_noise_trials = 1
find_best_traj = parameters["FIND_BEST_TRAJ"]
take_photo(airsim_client, pose_client, current_state, file_manager)
initialize_empty_frames(airsim_client.linecount, pose_client,
current_state, file_manager, my_rng)
set_animation_to_frame(airsim_client, pose_client, current_state,
airsim_client.default_initial_anim_time)
potential_states_fetcher.reset(pose_client, airsim_client, current_state)
potential_states_fetcher.get_potential_positions(airsim_client.linecount)
while airsim_client.linecount < airsim_client.length_of_simulation:
#### if we have the best traj finder
if find_best_traj and not pose_client.is_calibrating_energy:
start_best_sim = time.time()
my_rng.freeze_all_rng_states()
current_anim_time = airsim_client.getAnimationTime()
state_copy = current_state.deepcopy_state()
update_animation(airsim_client, pose_client.deepcopy_PEC(0),
state_copy)
future_anim_time = state_copy.anim_time
#find goal location
for trajectory_ind in range(
0,
len(potential_states_fetcher.potential_trajectory_list)):
#print("* trajectory_ind", trajectory_ind)
my_rng.reload_all_rng_states()
goal_traj = potential_states_fetcher.choose_trajectory_using_trajind(
trajectory_ind)
#update_animation(airsim_client, pose_client_copy, state_copy)
#my_rng_oracle = rng_object(101, file_manager.saved_vals_loc, potential_states_fetcher.DRONE_POS_JITTER_NOISE_STD, pose_client.NOISE_3D_INIT_STD)
my_rng_oracle = my_rng
for trial_ind in range(num_of_noise_trials):
#print("** trial ind", trial_ind)
potential_states_fetcher.restart_trajectory()
pose_client_copy = pose_client.deepcopy_PEC(trial_ind)
state_copy = current_state.deepcopy_state()
state_copy.update_anim_time(future_anim_time)
#set_animation_to_frame(airsim_client, pose_client, state_copy, current_anim_time)
#print("*** future_ind", future_ind)
goal_trajectory = potential_states_fetcher.goal_trajectory
#goal_state = potential_states_fetcher.move_along_trajectory()
#update_animation(airsim_client, pose_client_copy, state_copy)
set_position(goal_trajectory,
airsim_client,
state_copy,
pose_client_copy,
potential_states_fetcher,
loop_mode=potential_states_fetcher.loop_mode)
take_photo(airsim_client, pose_client_copy, state_copy,
file_manager)
determine_positions(airsim_client.linecount,
pose_client_copy, state_copy,
file_manager, my_rng_oracle)
#print("error", pose_client_copy.average_errors[pose_client_copy.MIDDLE_POSE_INDEX])
goal_traj.record_error_for_trial(
pose_client_copy.FUTURE_WINDOW_SIZE - 1,
pose_client_copy.average_errors[
pose_client_copy.MIDDLE_POSE_INDEX],
pose_client_copy.overall_error)
goal_traj.find_overall_error()
file_manager.record_oracle_errors(
airsim_client.linecount,
potential_states_fetcher.potential_trajectory_list)
potential_states_fetcher.restart_trajectory()
my_rng.reload_all_rng_states()
set_animation_to_frame(airsim_client, pose_client, current_state,
current_anim_time)
end_best_sim = time.time()
print("Simulating errors for all locations took",
end_best_sim - start_best_sim, "seconds")
if potential_states_fetcher.trajectory == "oracle":
min_error = np.inf
for trajectory_ind in range(
0,
len(potential_states_fetcher.potential_trajectory_list)
):
traj = potential_states_fetcher.potential_trajectory_list[
trajectory_ind]
if traj.error_middle < min_error:
potential_states_fetcher.oracle_traj_ind = trajectory_ind
min_error = traj.error_middle
print("traj ind", trajectory_ind, "error",
traj.error_middle)
print("chosen traj", potential_states_fetcher.oracle_traj_ind,
"with error", min_error)
#find goal location
if airsim_client.linecount != 0:
start2 = time.time()
goal_trajectory = potential_states_fetcher.choose_trajectory(
pose_client, airsim_client.linecount,
airsim_client.online_linecount, file_manager, my_rng)
print("chose trajectory", goal_trajectory.trajectory_index)
#move there
update_animation(airsim_client, pose_client, current_state)
set_position(goal_trajectory,
airsim_client,
current_state,
pose_client,
potential_states_fetcher,
loop_mode=potential_states_fetcher.loop_mode)
end2 = time.time()
file_manager.record_chosen_trajectory(
airsim_client.linecount, goal_trajectory.trajectory_index)
#print("Choosing a trajectory took", end2-start2, "seconds")
#update state values read from AirSim and take picture
take_photo(airsim_client, pose_client, current_state, file_manager)
#find human pose
start3 = time.time()
determine_positions(airsim_client.linecount, pose_client,
current_state, file_manager, my_rng)
end3 = time.time()
print("finding human pose took", end3 - start3, "seconds")
#plotting
start4 = time.time()
if not pose_client.quiet and airsim_client.linecount > 0:
plot_drone_traj(pose_client, file_manager.plot_loc,
airsim_client.linecount, pose_client.animation)
file_manager.write_error_values(pose_client.average_errors,
airsim_client.linecount)
file_manager.write_distance_values(
current_state.distances_travelled,
current_state.total_distance_travelled, airsim_client.linecount)
end4 = time.time()
#print("plotting and recording error took ", end4-start4, "seconds")
# if not pose_client.is_calibrating_energy and not pose_client.quiet and file_manager.loop_mode == "toy_example":
# plot_potential_errors_and_uncertainties_matrix(airsim_client.linecount, potential_states_fetcher.potential_trajectory_list,
# potential_states_fetcher.goal_trajectory, find_best_traj, file_manager.plot_loc)
potential_states_fetcher.reset(pose_client, airsim_client,
current_state)
potential_states_fetcher.get_potential_positions(
airsim_client.linecount)
### Debugging
if not pose_client.quiet and (pose_client.animation == "mpi_inf_3dhp"
or pose_client.animation
== "cmu_panoptic_pose_1"):
_, frame = find_pose_and_frame_at_time(
current_state.anim_time + current_state.DELTA_T,
current_state.anim_gt_array, current_state.num_of_joints)
photo_locs = file_manager.get_photo_locs_for_all_viewpoints(
frame, potential_states_fetcher.thrown_view_list)
plot_thrown_views(potential_states_fetcher.thrown_view_list,
file_manager.plot_loc, photo_locs,
airsim_client.linecount,
pose_client.bone_connections)
airsim_client.increment_linecount(pose_client.is_calibrating_energy)