def load_object_file_worker(self, object_file_name):
px_per_meter = 500
padding_meters = 0.5
erosion_iterations = 5
self.send_status_message("Loading object file")
verts, faces = rrt.load_obj(object_file_name)
self.send_status_message("Finding 2D cross-section")
cross_section = rrt.get_cross_section(verts, faces, z=self.z)
cross_section_2d = [c[:, 0:2] for c in cross_section]
self.send_status_message("Creating free mask")
_, free = rrt.make_free_space_image(
cross_section_2d,
px_per_meter,
padding_meters,
erosion_iterations=erosion_iterations)
min_x, max_x, min_y, max_y = rrt.cross_section_bounds(
cross_section_2d, padding_meters)
self.pf = rrt.PathFinder(
os.path.dirname(object_file_name),
free=free,
pc=rrt.PointConverter((min_x, max_x, min_y, max_y), px_per_meter,
padding_meters, free),
cross_section_2d=cross_section_2d,
)
self.object_file_name = object_file_name
self.object_file_loaded()
def main(directory, x0, y0, x1, y1):
path_finder = rrt.PathFinder(directory)
print("Loading...")
path_finder.load()
print("Finding...")
solution, lines = path_finder.find(x0, y0, x1, y1)
if solution:
print("Writing solution...")
floormap_file_name = os.path.join(directory, 'floormap.png')
floormap = cv2.imread(floormap_file_name)
floormap_with_path_file_name = os.path.join(directory,
'floormap_with_path.png')
for i in range(len(lines) - 1):
cv2.line(floormap, (lines[i][1], lines[i][0]),
(lines[i + 1][1], lines[i + 1][0]), (0, 0, 255), 10)
cv2.circle(floormap,
tuple([lines[0][1], lines[0][0]]),
50, (0, 255, 0),
thickness=30)
cv2.circle(floormap,
tuple([lines[-1][1], lines[-1][0]]),
50, (0, 0, 255),
thickness=30)
cv2.imwrite(floormap_with_path_file_name, floormap)
else:
print("No path found")
def main(directory, scale, x0, y0, x1, y1):
pf = rrt.PathFinder(directory)
pf.load()
bounds = pf.get_bounds()
solution, path_lines = pf.find(x0, y0, x1, y1)
width = pf.pc.x_to_pixel(bounds.max_x - bounds.min_x)*scale
height = pf.pc.x_to_pixel(bounds.max_y - bounds.min_y)*scale
win = Gtk.Window()
win.connect('destroy', Gtk.main_quit)
win.set_default_size(width, height)
drawing_area = Gtk.DrawingArea()
rrt_display = RrtDisplay(
drawing_area,
pf,
width,
height,
scale,
solution
)
win.add(drawing_area)
drawing_area.connect('draw', rrt_display.draw)
win.show_all()
Gtk.main()
def main(scene_idx=0):
#scene_idx = 0
mapper_scene2z = get_mapper()
mapper_scene2points = get_mapper_scene2points()
Train_Scenes, Test_Scenes = get_train_test_scenes()
scene_name = Test_Scenes[scene_idx]
num_startPoints = len(mapper_scene2points[scene_name])
num_steps = 35
## create test folder
test_folder = '/home/reza/Datasets/GibsonEnv/my_code/visual_servoing/test_IBVS'
'''
approach_folder = '{}/SIFT_interMatrix_gtDepth_Vz_OmegaY'.format(test_folder)
create_folder(approach_folder)
scene_folder = '{}/{}'.format(approach_folder, scene_name)
create_folder(scene_folder)
'''
f = open('{}/{}_{}.txt'.format(test_folder, perception_rep, depth_method), 'a')
f.write('scene_name = {}\n'.format(scene_name))
list_count_correct = []
list_count_runs = []
list_count_steps = []
## rrt functions
## first figure out how to sample points from rrt graph
rrt_directory = '/home/reza/Datasets/GibsonEnv/gibson/assets/dataset/{}_for_rrt'.format(scene_name)
path_finder = rrt.PathFinder(rrt_directory)
path_finder.load()
num_nodes = len(path_finder.nodes_x)
free = cv2.imread('/home/reza/Datasets/GibsonEnv/gibson/assets/dataset/{}_for_rrt/free.png'.format(scene_name), 0)
## GibsonEnv setup
## For Gibson Env
import gym, logging
from mpi4py import MPI
from gibson.envs.husky_env import HuskyNavigateEnv
from baselines import logger
import skimage.io
from transforms3d.euler import euler2quat
config_file = os.path.join('/home/reza/Datasets/GibsonEnv/my_code/CVPR_workshop', 'env_yamls', '{}_navigate.yaml'.format(scene_name))
env = HuskyNavigateEnv(config=config_file, gpu_count = 1)
obs = env.reset() ## this line is important otherwise there will be an error like 'AttributeError: 'HuskyNavigateEnv' object has no attribute 'potential''
def get_obs(current_pose):
pos, orn = func_pose2posAndorn(current_pose, mapper_scene2z[scene_name])
env.robot.reset_new_pose(pos, orn)
obs, _, _, _ = env.step(4)
obs_rgb = obs['rgb_filled']
obs_depth = obs['depth']
return obs_rgb.copy(), obs_depth.copy()
base_folder = '/home/reza/Datasets/GibsonEnv/my_code/visual_servoing/sample_image_pairs_{}'.format('test')
## go through each point folder
for point_idx in range(0, num_startPoints):
#for point_idx in range(0, 1):
print('point_idx = {}'.format(point_idx))
#point_folder = '{}/point_{}'.format(scene_folder, point_idx)
#create_folder(point_folder)
## read in start img and start pose
point_image_folder = '{}/{}/point_{}'.format(base_folder, scene_name, point_idx)
point_pose_npy_file = np.load('{}/{}/point_{}_poses.npy'.format(base_folder, scene_name, point_idx))
start_img = cv2.imread('{}/{}.png'.format(point_image_folder, point_pose_npy_file[0]['img_name']))[:, :, ::-1]
start_pose = point_pose_npy_file[0]['pose']
## index 0 is the left image, so right_img_idx starts from index 1
count_correct = 0
list_correct_img_names = []
list_whole_stat = []
list_steps = []
for right_img_idx in range(1, len(point_pose_npy_file)):
#for right_img_idx in range(1, 11):
flag_correct = False
print('right_img_idx = {}'.format(right_img_idx))
count_steps = 0
current_pose = start_pose
right_img_name = point_pose_npy_file[right_img_idx]['img_name']
goal_pose = point_pose_npy_file[right_img_idx]['pose']
goal_img, goal_depth = get_obs(goal_pose)
list_result_poses = [current_pose]
num_matches = 0
flag_broken = False
while count_steps < num_steps:
current_img, current_depth = get_obs(current_pose)
noise = np.random.normal(0.0, 0.5, (256, 256))
current_depth[:, :, 0] += noise
try:
kp1, kp2 = detect_correspondences(current_img, goal_img)
if count_steps == 0:
start_depth = current_depth.copy()
except:
print('run into error')
break
num_matches = kp1.shape[1]
vx, vz, omegay, flag_stop = compute_velocity_through_correspondences_and_depth(kp1, kp2, current_depth)
previous_pose = current_pose
current_pose, _, _, flag_stop_goToPose = goToPose_one_step(current_pose, vx, vz, omegay)
## check if there is collision during the action
left_pixel = path_finder.point_to_pixel((previous_pose[0], previous_pose[1]))
right_pixel = path_finder.point_to_pixel((current_pose[0], current_pose[1]))
# rrt.line_check returns True when there is no obstacle
if not rrt.line_check(left_pixel, right_pixel, free):
flag_broken = True
print('run into an obstacle ...')
break
## check if we should stop or not
if flag_stop or flag_stop_goToPose:
print('flag_stop = {}, flag_stop_goToPose = {}'.format(flag_stop, flag_stop_goToPose))
print('break')
break
count_steps += 1
list_result_poses.append(current_pose)
## sample current_img again to save in list_obs
current_img, current_depth = get_obs(current_pose)
#assert 1==2
## decide if this run is successful or not
flag_correct, dist, theta_change = similar_location_under_certainThreshold(goal_pose, list_result_poses[count_steps])
print('dist = {}, theta = {}'.format(dist, theta_change))
#print('start_pose = {}, final_pose = {}, goal_pose = {}'.format(start_pose, list_result_poses[-1], goal_pose))
if flag_correct:
count_correct += 1
list_correct_img_names.append(right_img_name[10:])
if flag_correct:
str_succ = 'Success'
print('str_succ = {}'.format(str_succ))
else:
str_succ = 'Failure'
print('str_succ = {}'.format(str_succ))
list_steps.append(len(list_result_poses))
## ===================================================================================================================
## plot the pose graph
'''
img_name = 'goTo_{}.jpg'.format(right_img_name[10:])
print('img_name = {}'.format(img_name))
## plot the poses
free2 = cv2.imread('/home/reza/Datasets/GibsonEnv/gibson/assets/dataset/{}_for_rrt/free.png'.format(scene_name), 1)
rows, cols, _ = free2.shape
plt.imshow(free2)
for m in range(len(list_result_poses)):
pose = list_result_poses[m]
x, y = path_finder.point_to_pixel((pose[0], pose[1]))
theta = pose[2]
plt.arrow(x, y, cos(theta), sin(theta), color='y', \
overhang=1, head_width=0.1, head_length=0.15, width=0.001)
## draw goal pose
x, y = path_finder.point_to_pixel((goal_pose[0], goal_pose[1]))
theta = goal_pose[2]
plt.arrow(x, y, cos(theta), sin(theta), color='r', \
overhang=1, head_width=0.1, head_length=0.15, width=0.001)
plt.axis([0, cols, 0, rows])
plt.xticks([])
plt.yticks([])
plt.title('{}, start point_{}, goal viewpoint {}, {}\n dist = {:.4f} meter, theta = {:.4f} degree\n'.format(scene_name, point_idx, right_img_name[10:], str_succ, dist, theta_change))
plt.savefig('{}/{}'.format(point_folder, img_name), bbox_inches='tight', dpi=(400))
plt.close()
## ======================================================================================================================
## save stats
current_test_dict = {}
current_test_dict['img_name'] = right_img_name
current_test_dict['success_flag'] = flag_correct
current_test_dict['dist'] = dist
current_test_dict['theta'] = theta_change
current_test_dict['steps'] = count_steps
current_test_dict['collision'] = flag_broken
list_whole_stat.append(current_test_dict)
np.save('{}/runs_statistics.npy'.format(point_folder), list_whole_stat)
success_rate = 1.0 * count_correct / (len(point_pose_npy_file)-1)
print('count_correct/num_right_images = {}/{} = {}'.format(count_correct, len(point_pose_npy_file)-1, success_rate))
## write correctly run target image names to file
f = open('{}/successful_runs.txt'.format(point_folder), 'w')
f.write('count_correct/num_right_images = {}/{} = {}\n'.format(count_correct, len(point_pose_npy_file)-1, success_rate))
for i in range(len(list_correct_img_names)):
f.write('{}\n'.format(list_correct_img_names[i]))
f.close()
print('writing correct run image names to txt ...')
'''
avg_steps = 1.0 * sum(list_steps) / len(list_steps)
f.write('point {} : {}/{}, {}\n'.format(point_idx, count_correct, len(point_pose_npy_file)-1, avg_steps))
list_count_correct.append(count_correct)
list_count_runs.append(len(point_pose_npy_file)-1)
list_count_steps.append(avg_steps)
f.flush()
avg_count_steps = 1.0 * sum(list_count_steps) / len(list_count_steps)
f.write('In total : {}/{}, {}\n'.format(sum(list_count_correct), sum(list_count_runs), avg_count_steps))
f.write('-------------------------------------------------------------------------------------\n')
def main(scene_idx=0, actual_episodes=1):
#scene_idx = 0
#actual_episodes=2
Train_Scenes, Test_Scenes = get_train_test_scenes()
scene_name = Train_Scenes[scene_idx]
num_startPoints = len(mapper_scene2points[scene_name])
model_weights_save_path = '{}'.format('/home/reza/Datasets/GibsonEnv/my_code/vs_controller/trained_dqn')
action_space = action_table.shape[0]
##=============================================================================================================
## rrt functions
## first figure out how to sample points from rrt graph
rrt_directory = '/home/reza/Datasets/GibsonEnv/gibson/assets/dataset/{}_for_rrt'.format(scene_name)
path_finder = rrt.PathFinder(rrt_directory)
path_finder.load()
num_nodes = len(path_finder.nodes_x)
free = cv2.imread('/home/reza/Datasets/GibsonEnv/gibson/assets/dataset/{}_for_rrt/free.png'.format(scene_name), 0)
##------------------------------------------------------------------------------------------------------------
## setup environment
import gym, logging
from mpi4py import MPI
from gibson.envs.husky_env import HuskyNavigateEnv
from baselines import logger
import skimage.io
from transforms3d.euler import euler2quat
config_file = os.path.join('/home/reza/Datasets/GibsonEnv/my_code/CVPR_workshop', 'env_yamls', '{}_navigate.yaml'.format(scene_name))
env = HuskyNavigateEnv(config=config_file, gpu_count = 1)
obs = env.reset() ## this line is important otherwise there will be an error like 'AttributeError: 'HuskyNavigateEnv' object has no attribute 'potential''
mapper_scene2z = get_mapper()
def get_obs(current_pose):
pos, orn = func_pose2posAndorn(current_pose, mapper_scene2z[scene_name])
env.robot.reset_new_pose(pos, orn)
obs, _, _, _ = env.step(4)
obs_rgb = obs['rgb_filled']
obs_depth = obs['depth']
#obs_normal = obs['normal']
return obs_rgb, obs_depth#, obs_normal
def close_to_goal(pose1, pose2, thresh=0.15):
L2_dist = math.sqrt((pose1[0] - pose2[0])**2 + (pose1[1] - pose2[1])**2)
thresh_L2_dist = thresh
theta_change = abs(pose1[2] - pose2[2])/math.pi * 180
return (L2_dist < thresh_L2_dist) and (theta_change <= 30)
def compute_distance(left_pose, right_pose, lamb_alpha=0.5, lamb_beta=0.2):
x1, y1 = left_pose[0], left_pose[1]
x2, y2 = right_pose[0], right_pose[1]
pho_dist = math.sqrt((x1-x2)**2 + (y1-y2)**2)
left_pose_heading = left_pose[2]
right_pose_heading = right_pose[2]
location_angle = atan2(y2-y1, x2-x1)
print('left_pose_heading = {}, right_pose_heading = {}, location_angle = {}'.format(left_pose_heading, right_pose_heading, location_angle))
if pho_dist >= 0.05:
## alpha angle in goToPose is the difference between location angle and left_pose_heading
a1, b1 = cos(location_angle), sin(location_angle)
a2, b2 = cos(left_pose_heading), sin(left_pose_heading)
alpha_dist = math.sqrt((a1-a2)**2 + (b1-b2)**2)
## beta angle in goToPose is the difference between right_pose_heading and location angle
a1, b1 = cos(right_pose_heading), sin(right_pose_heading)
a2, b2 = cos(location_angle), sin(location_angle)
beta_dist = math.sqrt((a1-a2)**2 + (b1-b2)**2)
else:
## when pho_dist is close to zero, alpha_dist is not important
alpha_dist = 0.0
## beta angle becomes the anlge between left and right poses
a1, b1 = cos(right_pose_heading), sin(right_pose_heading)
a2, b2 = cos(left_pose_heading), sin(left_pose_heading)
beta_dist = math.sqrt((a1-a2)**2 + (b1-b2)**2)
print('pho_dist = {:.2f}, alpha_dist = {:.2f}, beta_dist = {:.2f}'.format(pho_dist, alpha_dist, beta_dist))
return pho_dist + lamb_alpha * alpha_dist + lamb_beta * beta_dist
def decide_reward_and_done(previous_pose, current_pose, goal_pose, start_pose):
## check if the new step is on free space or not
reward = 0.0
done = 0
## check if current_pose is closer to goal_pose than previous_pose
'''
L2_dist_current = math.sqrt((current_pose[0] - goal_pose[0])**2 + (current_pose[1] - goal_pose[1])**2)
L2_dist_previous = math.sqrt((previous_pose[0] - goal_pose[0])**2 + (previous_pose[1] - goal_pose[1])**2)
if L2_dist_current < L2_dist_previous:
reward += 0.25
print('L2_dist_current = {:.2f}, L2_dist_previous = {:.2f}, reward = {}'.format(L2_dist_current, L2_dist_previous, reward))
'''
## following Fereshteh's DiVIs paper
dist_init = compute_distance(start_pose, goal_pose, lamb_alpha=0.2)
dist_current = compute_distance(current_pose, goal_pose, lamb_alpha=0.2)
reward = max(0, 1 - min(dist_init, dist_current)/(dist_init+0.0001))
print('dist_init = {:.2f}, dist_current = {:.2f}, reward = {:.2f}'.format(dist_init, dist_current, reward))
## check if current_pose is close to goal
## goal reward should be larger than all the previously accumulated reward
flag_close_to_goal = close_to_goal(current_pose, goal_pose)
if flag_close_to_goal:
reward = 50.0
done = 1
print('current_pose = {}, goal_pose = {}, flag_close_to_goal = {}, reward = {}'.format(current_pose, goal_pose, flag_close_to_goal, reward))
#collision_done = 0
## if there is a collision, reward is -1 and the episode is done
left_pixel = path_finder.point_to_pixel((previous_pose[0], previous_pose[1]))
right_pixel = path_finder.point_to_pixel((current_pose[0], current_pose[1]))
## rrt.line_check returns True when there is no obstacle
if not rrt.line_check(left_pixel, right_pixel, free):
print('bumped into obstacle ....')
reward = 0.0
#collision_done = 1
done=1
print('final reward = {}'.format(reward))
return reward, done, 0 #, collision_done
##============================================================================================================
base_folder = '/home/reza/Datasets/GibsonEnv/my_code/visual_servoing/sample_image_pairs_{}'.format('train')
#agent = DQN_vs_triplet(trained_model_path=None, num_actions=action_space, input_channels=5)
agent = DQN_vs_triplet(trained_model_path=model_weights_save_path, num_actions=action_space, input_channels=5)
rewards = []
avg_rewards = []
for i_epoch in range(actual_episodes):
## go through each point folder
for point_idx in range(0, num_startPoints):
#for point_idx in range(0, 1):
print('point_idx = {}'.format(point_idx))
## read in start img and start pose
point_image_folder = '{}/{}/point_{}'.format(base_folder, scene_name, point_idx)
point_pose_npy_file = np.load('{}/{}/point_{}_poses.npy'.format(base_folder, scene_name, point_idx))
#start_img = cv2.imread('{}/{}.png'.format(point_image_folder, point_pose_npy_file[0]['img_name']))[:, :, ::-1]
start_pose = point_pose_npy_file[0]['pose']
start_img, start_depth = get_obs(start_pose)
start_depth = start_depth.copy()
## index 0 is the left image, so right_img_idx starts from index 1
for right_img_idx in range(1, len(point_pose_npy_file)):
#for right_img_idx in range(3, 4):
#print('right_img_idx = {}'.format(right_img_idx))
current_pose = start_pose
right_img_name = point_pose_npy_file[right_img_idx]['img_name']
goal_pose = point_pose_npy_file[right_img_idx]['pose']
#goal_img = cv2.imread('{}/{}.png'.format(point_image_folder, right_img_name), 1)[:,:,::-1]
goal_img, goal_depth = get_obs(goal_pose)
goal_img, goal_depth = goal_img.copy(), goal_depth.copy()
overlapArea_currentView = genOverlapAreaOnCurrentView(start_depth, goal_depth, start_pose, goal_pose)[:,:,:2]
overlapArea_goalView = genOverlapAreaOnGoalView(start_depth, goal_depth, start_pose, goal_pose)[:,:,:2]
overlapArea = np.concatenate((overlapArea_currentView, overlapArea_goalView, start_depth), axis=2)
state = [overlapArea]
episode_reward = 0
for i_step in range(seq_len):
action = agent.select_action(state)
print('action = {}'.format(action))
## update current_pose
vz, omegay = action_table[action]
print('vz = {:.2f}, omegay = {:.2f}'.format(vz, omegay))
vx = 0.0
vx = vx * lambda_action
vz = vz * lambda_action
omegay = omegay * pi * lambda_action
print('actual velocity = {:.2f}, {:.2f}, {:.2f}'.format(vx, vz, omegay))
previous_pose = current_pose
current_pose = update_current_pose(current_pose, vx, vz, omegay)
## compute new_state
current_img, current_depth = get_obs(current_pose)
next_left_img, next_left_depth = current_img.copy(), current_depth.copy()
new_overlapArea_currentView = genOverlapAreaOnCurrentView(next_left_depth, goal_depth, current_pose, goal_pose)[:,:,:2]
new_overlapArea_goalView = genOverlapAreaOnGoalView(next_left_depth, goal_depth, current_pose, goal_pose)[:,:,:2]
new_overlapArea = np.concatenate((new_overlapArea_currentView, new_overlapArea_goalView, next_left_depth), axis=2)
new_state = [new_overlapArea]
## visualize the state
'''
fig = plt.figure(figsize=(15, 10))
r, c, = 2, 2
ax = fig.add_subplot(r, c, 1)
ax.imshow(next_left_img)
ax = fig.add_subplot(r, c, 2)
ax.imshow(goal_img)
ax = fig.add_subplot(r, c, 3)
start_mask = np.concatenate((new_overlapArea, np.zeros((256, 256, 1), dtype=np.uint8)), axis=2)
ax.imshow(start_mask)
plt.show()
'''
## collision done only stops continuing the sequence, but won't affect reward computing
reward, done, collision_done = decide_reward_and_done(previous_pose, current_pose, goal_pose, start_pose)
print('done = {}, collision_done = {}'.format(done, collision_done))
if i_step == seq_len-1:
print('used up all the steps ...')
done = 1
agent.memory.push(state, action, reward, new_state, done)
if len(agent.memory) > batch_size:
agent.update(batch_size)
state = new_state
episode_reward += reward
print('---------------- end of a action ------------------ ')
if done or collision_done:
break
print('---------------- end of a sequence ------------------ ')
rewards.append(episode_reward)
avg_rewards.append(np.mean(rewards[-10:]))
sys.stdout.write("------------------------------------epoch = {}, point = {}, traj = {}, reward: {}, average_reward: {} #_steps: {}\n".format(i_epoch, point_idx, right_img_idx, np.round(episode_reward, decimals=2), np.round(avg_rewards[-1], decimals=2), i_step))
if right_img_idx % 10 == 0:
agent.update_critic()
## plot the running_loss
plt.plot(rewards, label='reward')
plt.plot(avg_rewards, label='avg_reward')
plt.xlabel('Episode')
plt.ylabel('Reward')
plt.grid(True)
plt.legend(loc='upper right')
plt.yscale('linear')
plt.title('change of reward and avg_reward')
plt.savefig('{}/Reward_episode_{}_{}.jpg'.format(
model_weights_save_path, num_episodes, scene_name), bbox_inches='tight')
plt.close()
torch.save(agent.actor.state_dict(), '{}/dqn_epoch_200000_{}.pt'.format(model_weights_save_path, scene_name))
torch.save(agent.actor.state_dict(), '{}/dqn_epoch_200000.pt'.format(model_weights_save_path))
scene_name = Test_Scenes[scene_idx]
elif mode == 'Train':
scene_name = Train_Scenes[scene_idx]
num_startPoints = len(mapper_scene2points[scene_name])
model_weights_save_path = '{}/{}'.format(
'/home/reza/Datasets/GibsonEnv/my_code/vs_controller/trained_dqn',
approach)
action_space = action_table.shape[0]
##=============================================================================================================
## rrt functions
## first figure out how to sample points from rrt graph
rrt_directory = '/home/reza/Datasets/GibsonEnv/gibson/assets/dataset/{}_for_rrt'.format(
scene_name)
path_finder = rrt.PathFinder(rrt_directory)
path_finder.load()
num_nodes = len(path_finder.nodes_x)
free = cv2.imread(
'/home/reza/Datasets/GibsonEnv/gibson/assets/dataset/{}_for_rrt/free.png'.
format(scene_name), 0)
##------------------------------------------------------------------------------------------------------------
## setup environment
import gym, logging
from mpi4py import MPI
from gibson.envs.husky_env import HuskyNavigateEnv
from baselines import logger
import skimage.io
from transforms3d.euler import euler2quat
config_file = os.path.join(
def main(scene_idx=0, point_a=0, right_a=0):
#scene_idx = 0
Train_Scenes, Test_Scenes = get_train_test_scenes()
mapper_scene2points = get_mapper_scene2points()
scene_name = Test_Scenes[scene_idx]
num_startPoints = len(mapper_scene2points[scene_name])
## as the move forward distance = 0.1, assume velocity is 0.01, it needs 10 steps.
def pose_interpolation(start_pose,
end_pose,
num=10,
include_endpoint=False):
x0, y0, theta0 = start_pose
x1, y1, theta1 = end_pose
x = np.linspace(x0, x1, num=num, endpoint=include_endpoint)
y = np.linspace(y0, y1, num=num, endpoint=include_endpoint)
## convert to quaternion
q0 = Quaternion(axis=[0, -1, 0], angle=theta0)
q1 = Quaternion(axis=[0, -1, 0], angle=theta1)
pose_list = []
v = np.array([1, 0, 0])
for idx, q in enumerate(
Quaternion.intermediates(q0,
q1,
num - 1,
include_endpoints=True)):
if idx < num:
e, d, f = q.rotate(v)
theta = atan2(f, e)
current_pose = [x[idx], y[idx], theta]
pose_list.append(current_pose)
return pose_list
## rrt functions
## first figure out how to sample points from rrt graph
rrt_directory = '/home/reza/Datasets/GibsonEnv/gibson/assets/dataset/{}_for_rrt'.format(
scene_name)
path_finder = rrt.PathFinder(rrt_directory)
path_finder.load()
num_nodes = len(path_finder.nodes_x)
free = cv2.imread(
'/home/reza/Datasets/GibsonEnv/gibson/assets/dataset/{}_for_rrt/free.png'
.format(scene_name), 0)
## draw the observations
## setup environment
import gym, logging
from mpi4py import MPI
from gibson.envs.husky_env import HuskyNavigateEnv
from baselines import logger
import skimage.io
from transforms3d.euler import euler2quat
config_file = os.path.join(
'/home/reza/Datasets/GibsonEnv/my_code/CVPR_workshop', 'env_yamls',
'{}_navigate.yaml'.format(scene_name))
env = HuskyNavigateEnv(config=config_file, gpu_count=1)
obs = env.reset(
) ## this line is important otherwise there will be an error like 'AttributeError: 'HuskyNavigateEnv' object has no attribute 'potential''
mapper_scene2z = get_mapper()
def get_obs(current_pose):
pos, orn = func_pose2posAndorn(current_pose,
mapper_scene2z[scene_name])
env.robot.reset_new_pose(pos, orn)
obs, _, _, _ = env.step(4)
obs_rgb = obs['rgb_filled']
obs_depth = obs['depth']
#obs_normal = obs['normal']
return obs_rgb, obs_depth #, obs_normal
def close_to_goal(pose1, pose2, thresh=0.20):
L2_dist = math.sqrt((pose1[0] - pose2[0])**2 +
(pose1[1] - pose2[1])**2)
thresh_L2_dist = thresh
theta_change = abs(pose1[2] - pose2[2]) / math.pi * 180
return (L2_dist <= thresh_L2_dist) #and (theta_change <= 30)
base_folder = '/home/reza/Datasets/GibsonEnv/my_code/visual_servoing/sample_image_pairs_{}'.format(
'test')
#for point_idx in range(2, 3):
for point_idx in range(point_a, point_a + 1):
print('point_idx = {}'.format(point_idx))
## read in start img and start pose
point_image_folder = '{}/{}/point_{}'.format(base_folder, scene_name,
point_idx)
point_pose_npy_file = np.load('{}/{}/point_{}_poses.npy'.format(
base_folder, scene_name, point_idx))
save_folder = '{}/{}/point_{}'.format(
'/home/reza/Datasets/GibsonEnv/my_code/vs_controller/for_video',
scene_name, point_idx)
## index 0 is the left image, so right_img_idx starts from index 1
#for right_img_idx in range(1, len(point_pose_npy_file)):
#for right_img_idx in range(1, 101):
for right_img_idx in range(right_a, right_a + 1):
right_img_name = point_pose_npy_file[right_img_idx]['img_name']
## Read in pose npy file generated from DQN
dqn_pose_npy_file = np.load(
'{}/run_{}/{}_waypoint_pose_list.npy'.format(
save_folder, right_img_idx, right_img_name[10:]))
start_pose = dqn_pose_npy_file[0]
goal_pose = dqn_pose_npy_file[1]
dqn_pose_list = dqn_pose_npy_file[2]
goal_img, goal_depth = get_obs(goal_pose)
goal_img = goal_img[:, :, ::-1]
cv2.imwrite(
'{}/run_{}/goal_img.jpg'.format(save_folder, right_img_idx),
goal_img)
interpolated_pose_list = []
## build the subsequence
len_dqn_pose_list = len(dqn_pose_list)
for i in range(len_dqn_pose_list - 1):
first_pose = dqn_pose_list[i]
second_pose = dqn_pose_list[i + 1]
subseq_pose_list = pose_interpolation(first_pose, second_pose)
interpolated_pose_list += subseq_pose_list
interpolated_pose_list.append(dqn_pose_list[-1])
img_name = 'goTo_{}.jpg'.format('current')
print('img_name = {}'.format(img_name))
## plot the poses
free2 = cv2.imread(
'/home/reza/Datasets/GibsonEnv/gibson/assets/dataset/{}_for_rrt/free.png'
.format(scene_name), 1)
rows, cols, _ = free2.shape
plt.imshow(free2)
for m in range(len(interpolated_pose_list)):
#for m in range(0, 100, 5):
pose = interpolated_pose_list[m]
x, y = path_finder.point_to_pixel((pose[0], pose[1]))
theta = pose[2]
plt.arrow(x, y, cos(theta), sin(theta), color='y', \
overhang=1, head_width=0.1, head_length=0.15, width=0.001)
## draw goal pose
x, y = path_finder.point_to_pixel((goal_pose[0], goal_pose[1]))
theta = goal_pose[2]
plt.arrow(x, y, cos(theta), sin(theta), color='r', \
overhang=1, head_width=0.1, head_length=0.15, width=0.001)
## draw start pose
x, y = path_finder.point_to_pixel((start_pose[0], start_pose[1]))
theta = start_pose[2]
plt.arrow(x, y, cos(theta), sin(theta), color='b', \
overhang=1, head_width=0.1, head_length=0.15, width=0.001)
plt.axis([0, cols, 0, rows])
plt.xticks([])
plt.yticks([])
#plt.title('{}, start point_{}, goal viewpoint {}, {}'.format(scene_name, point_idx, right_img_name[10:], str_succ))
plt.savefig('{}/run_{}/{}'.format(save_folder, right_img_idx,
'overview.jpg'),
bbox_inches='tight',
dpi=(400))
plt.close()
#'''
for i in range(len(interpolated_pose_list)):
current_pose = interpolated_pose_list[i]
obs_rgb, obs_depth = get_obs(current_pose)
obs_rgb = obs_rgb[:, :, ::-1]
cv2.imwrite(
'{}/run_{}/step_{}.jpg'.format(save_folder, right_img_idx, i),
obs_rgb)
def main(scene_idx=0, point_a=0):
#scene_idx = 1
## necessary constants
mapper_scene2points = get_mapper_scene2points()
num_episodes = 200000
batch_size = 64
lambda_action = 0.25
action_table = buildActionMapper(flag_fewer_actions=True)
seq_len = 50
Train_Scenes, Test_Scenes = get_train_test_scenes()
if mode == 'Test':
scene_name = Test_Scenes[scene_idx]
elif mode == 'Train':
scene_name = Train_Scenes[scene_idx]
num_startPoints = len(mapper_scene2points[scene_name])
model_weights_save_path = '{}/{}'.format('/home/reza/Datasets/GibsonEnv/my_code/vs_controller/trained_dqn', approach)
action_space = action_table.shape[0]
##=============================================================================================================
## rrt functions
## first figure out how to sample points from rrt graph
rrt_directory = '/home/reza/Datasets/GibsonEnv/gibson/assets/dataset/{}_for_rrt'.format(scene_name)
path_finder = rrt.PathFinder(rrt_directory)
path_finder.load()
num_nodes = len(path_finder.nodes_x)
free = cv2.imread('/home/reza/Datasets/GibsonEnv/gibson/assets/dataset/{}_for_rrt/free.png'.format(scene_name), 0)
##------------------------------------------------------------------------------------------------------------
## setup environment
import gym, logging
from mpi4py import MPI
from gibson.envs.husky_env import HuskyNavigateEnv
from baselines import logger
import skimage.io
from transforms3d.euler import euler2quat
config_file = os.path.join('/home/reza/Datasets/GibsonEnv/my_code/CVPR_workshop', 'env_yamls', '{}_navigate.yaml'.format(scene_name))
env = HuskyNavigateEnv(config=config_file, gpu_count = 1)
obs = env.reset() ## this line is important otherwise there will be an error like 'AttributeError: 'HuskyNavigateEnv' object has no attribute 'potential''
mapper_scene2z = get_mapper()
def get_obs(current_pose):
pos, orn = func_pose2posAndorn(current_pose, mapper_scene2z[scene_name])
env.robot.reset_new_pose(pos, orn)
obs, _, _, _ = env.step(4)
obs_rgb = obs['rgb_filled']
obs_depth = obs['depth']
#obs_normal = obs['normal']
return obs_rgb, obs_depth#, obs_normal
def close_to_goal(pose1, pose2, thresh=0.20):
L2_dist = math.sqrt((pose1[0] - pose2[0])**2 + (pose1[1] - pose2[1])**2)
thresh_L2_dist = thresh
theta_change = abs(pose1[2] - pose2[2])/math.pi * 180
return (L2_dist <= thresh_L2_dist) #and (theta_change <= 30)
##============================================================================================================
if mode == 'Test':
base_folder = '/home/reza/Datasets/GibsonEnv/my_code/visual_servoing/sample_image_pairs_{}'.format('test')
elif mode == 'Train':
base_folder = '/home/reza/Datasets/GibsonEnv/my_code/visual_servoing/sample_image_pairs_{}'.format('train')
import torch
import torch.nn as nn
import torch.nn.functional as F
device = torch.device('cuda:0') ## Default CUDA device
num_epochs = 200000 ## same as # of trajs sampled
num_actions = action_table.shape[0]
if input_type == 'both':
perception = Perception_overlap(4).to(device)
elif input_type == 'siamese':
perception = Perception_siamese(4).to(device)
elif input_type == 'optical_flow':
perception = Perception_overlap(2).to(device)
elif input_type == 'optical_flow_depth':
perception = Perception_overlap(3).to(device)
elif input_type == 'optical_flow_depth_normalized':
perception = Perception_overlap(3).to(device)
elif input_type == 'optical_flow_depth_unnormalized_mask':
perception = Perception_overlap(3).to(device)
elif input_type == 'optical_flow_depth_siamese':
perception = Perception_siamese_fusion_new(3).to(device)
elif input_type == 'optical_flow_memory':
perception = Preception_overlap_resnet(4).to(device)
else:
perception = Perception_overlap(2).to(device)
if input_type == 'siamese':
model = DQN_OVERLAP_Controller(perception, num_actions, input_size=512).to(device)
elif input_type == 'optical_flow_memory':
model = DQN_OVERLAP_RESNET_Controller(perception, num_actions, input_size=512).to(device)
else:
model = DQN_OVERLAP_Controller(perception, num_actions, input_size=256).to(device)
model.load_state_dict(torch.load('{}/dqn_epoch_{}_Uvalda.pt'.format(model_weights_save_path, num_epochs)))
#model.eval()
list_succ = []
list_collision = []
## go through each point folder
if mode == 'Test':
a, b = 0, 1
elif mode == 'Train':
a, b = 7, 8
#a, b = 0, 1
#for point_idx in range(0, num_startPoints):
#for point_idx in range(a, b):
for point_idx in range(point_a, point_a+1):
print('point_idx = {}'.format(point_idx))
task_folder = '{}/{}/point_{}'.format('/home/reza/Datasets/GibsonEnv/my_code/vs_controller/for_video', scene_name, point_idx)
create_folder(task_folder)
## read in start img and start pose
point_image_folder = '{}/{}/point_{}'.format(base_folder, scene_name, point_idx)
point_pose_npy_file = np.load('{}/{}/point_{}_poses.npy'.format(base_folder, scene_name, point_idx))
#start_img = cv2.imread('{}/{}.png'.format(point_image_folder, point_pose_npy_file[0]['img_name']))[:, :, ::-1]
start_pose = point_pose_npy_file[0]['pose']
start_img, start_depth = get_obs(start_pose)
start_depth = start_depth.copy()
count_succ = 0
count_collision = 0
count_short_runs = 0
count_short_runs_collision = 0
count_short_runs_succ = 0
## index 0 is the left image, so right_img_idx starts from index 1
#for right_img_idx in range(1, len(point_pose_npy_file)):
for right_img_idx in range(1, 101):
print('right_img_idx = {}'.format(right_img_idx))
run_folder = '{}/run_{}'.format(task_folder, right_img_idx)
create_folder(run_folder)
current_pose = start_pose
right_img_name = point_pose_npy_file[right_img_idx]['img_name']
goal_pose = point_pose_npy_file[right_img_idx]['pose']
#goal_img = cv2.imread('{}/{}.png'.format(point_image_folder, right_img_name), 1)[:,:,::-1]
goal_img, goal_depth = get_obs(goal_pose)
goal_depth = goal_depth.copy()
current_depth = start_depth.copy()
episode_reward = 0
flag_succ = False
poses_list = []
poses_list.append(start_pose)
poses_list.append(goal_pose)
poses_list.append([current_pose])
for i_step in range(seq_len):
if input_type == 'both' or input_type == 'siamese':
overlapArea_currentView = genOverlapAreaOnCurrentView(current_depth, goal_depth, current_pose, goal_pose)[:,:,:2]
overlapArea_goalView = genOverlapAreaOnGoalView(current_depth, goal_depth, current_pose, goal_pose)[:,:,:2]
overlapArea = np.concatenate((overlapArea_currentView, overlapArea_goalView), axis=2)
elif input_type == 'optical_flow':
overlapArea = genGtDenseCorrespondenseFlowMap(current_depth, goal_depth, current_pose, goal_pose)[:,:,:2]
overlapArea = removeCorrespondenceRandomly(overlapArea, keep_prob=1.0)
elif input_type == 'optical_flow_depth':
opticalFlow = genGtDenseCorrespondenseFlowMap(current_depth, goal_depth, current_pose, goal_pose)[:,:,:2]
overlapArea = np.concatenate((opticalFlow, current_depth), axis=2)
elif input_type == 'optical_flow_depth_normalized':
opticalFlow = genGtDenseCorrespondenseFlowMap(current_depth, goal_depth, current_pose, goal_pose)[:,:,:2]
normalized_opticalFlow = normalize_opticalFlow(opticalFlow)
normalized_depth = normalize_depth(current_depth)
#normalized_depth = np.ones((256, 256, 1), np.float32)
overlapArea = np.concatenate((normalized_opticalFlow, normalized_depth), axis=2)
elif input_type == 'optical_flow_depth_unnormalized_mask':
opticalFlow, mask_flow = genGtDenseCorrespondenseFlowMapAndMask(current_depth, goal_depth, current_pose, goal_pose)
opticalFlow = opticalFlow[:, :, :2]
normalized_depth = current_depth * mask_flow
#normalized_opticalFlow = normalize_opticalFlow(opticalFlow)
normalized_depth = normalize_depth(normalized_depth)
overlapArea = np.concatenate((opticalFlow, normalized_depth), axis=2)
elif input_type == 'optical_flow_depth_siamese':
opticalFlow = genGtDenseCorrespondenseFlowMap(current_depth, goal_depth, current_pose, goal_pose)[:,:,:2]
normalized_depth = normalize_depth(current_depth)
#normalized_depth = np.ones((256, 256, 1), np.float32)
overlapArea = np.concatenate((opticalFlow, normalized_depth), axis=2)
#print('overlapArea.shape = {}'.format(overlapArea.shape))
elif input_type == 'optical_flow_memory':
opticalFlow = genGtDenseCorrespondenseFlowMap(current_depth, goal_depth, current_pose, goal_pose)[:,:,:2]
if i_step == 0:
overlapArea = np.concatenate((opticalFlow, opticalFlow), axis=2)
else:
overlapArea = np.concatenate((old_opticalFlow, opticalFlow), axis=2)
else:
overlapArea = genOverlapAreaOnCurrentView(current_depth, goal_depth, current_pose, goal_pose)[:,:,:2]
tensor_left = torch.tensor(overlapArea, dtype=torch.float32).to(device).unsqueeze(0).permute(0, 3, 1, 2)
Qvalue_table = model(tensor_left)
pred = Qvalue_table.max(1)[1].view(1, 1).detach().cpu().numpy().item() ## batch_size x 3
#print('Qvalue_table: {}'.format(Qvalue_table))
#print('pred = {}'.format(pred))
## update current_pose
vz, omegay = action_table[pred]
#print('vz = {:.2f}, omegay = {:.2f}'.format(vz, omegay))
vx = 0.0
vx = vx * lambda_action
vz = vz * lambda_action
omegay = omegay * pi * lambda_action
#print('actual velocity = {:.2f}, {:.2f}, {:.2f}'.format(vx, vz, omegay))
previous_pose = current_pose
current_pose = update_current_pose(current_pose, vx, vz, omegay)
poses_list[2].append(current_pose)
flag_broken = False
left_pixel = path_finder.point_to_pixel((previous_pose[0], previous_pose[1]))
right_pixel = path_finder.point_to_pixel((current_pose[0], current_pose[1]))
## rrt.line_check returns True when there is no obstacle
if not rrt.line_check(left_pixel, right_pixel, free):
print('run into something')
flag_broken = True
break
if close_to_goal(current_pose, goal_pose):
print('success run')
flag_succ = True
break
## compute new_state
current_img, current_depth = get_obs(current_pose)
current_depth = current_depth.copy()
#old_opticalFlow = opticalFlow.copy()
np.save('{}/{}_waypoint_pose_list.npy'.format(run_folder, right_img_name[10:]), poses_list)
#assert 1==2
if flag_succ:
count_succ += 1
list_succ.append(point_pose_npy_file[right_img_idx]['img_name'])
if findShortRangeImageName(right_img_name):
count_short_runs_succ += 1
if flag_broken:
count_collision += 1
list_collision.append(point_pose_npy_file[right_img_idx]['img_name'])
if findShortRangeImageName(right_img_name):
count_short_runs_collision += 1
if findShortRangeImageName(right_img_name):
count_short_runs += 1
print('count_succ = {}'.format(count_succ))
print('count_collision = {}'.format(count_collision))
print('count_short_runs_succ = {}'.format(count_short_runs_succ))
print('count_short_runs_collision = {}'.format(count_short_runs_collision))
print('num_succ = {}, num_run = {}, count_short_runs_succ = {}, count_short_runs = {}'.format(count_succ, len(point_pose_npy_file), count_short_runs_succ, count_short_runs))
# load the occupancy img
free = cv2.imread('free.png', 0)
#==============================build the rrt tree to cover the whole environment===================================
edges_from_px, edges_to_px, nodes_x, nodes_y, edges = rrt.make_rrt(free)
floor_map = cv2.imread('free.png', 1)
for i, edge_from_px in enumerate(edges_from_px):
cv2.line(floor_map, edge_from_px, edges_to_px[i], (255, 0, 0), thickness=5)
cv2.imwrite('floor_map.png', floor_map)
#plt.imshow(floor_map)
#plt.show()
path_finder = rrt.PathFinder(nodes_x, nodes_y, edges, free)
#=================================== find a path from pixel (1000, 1000) to pixel (4000, 4000)======================
start_pixel = (1000, 1000)
target_pixel = (4000, 4000)
_, lines = path_finder.find_path_between_pixels(start_pixel, target_pixel)
traj = cv2.imread('free.png', 1)
for i in range(len(lines) - 1):
y0, x0 = lines[i]
y1, x1 = lines[i + 1]
cv2.line(traj, (x0, y0), (x1, y1), (255, 0, 0), thickness=10)
cv2.imwrite('traj.png', traj)
#plt.imshow(traj)
#plt.show()
def refine_path_worker(self, ):
path_nodes = self.solution.path
node_map = [node for node in self.solution.path]
num_nodes = len(path_nodes)
nodes_x = np.array([self.pf.nodes_x[node] for node in path_nodes])
nodes_y = np.array([self.pf.nodes_y[node] for node in path_nodes])
points = [
self.pf.pc.point_to_pixel((nodes_x[i], nodes_y[i]))
for i in range(num_nodes)
]
last_cost = self.solution.cost
edges = defaultdict(dict)
for idx, node in enumerate(path_nodes):
if idx:
node0, node1 = idx - 1, idx
distance = math.sqrt((nodes_x[node0] - nodes_x[node1])**2 +
(nodes_y[node0] - nodes_y[node1])**2)
edges[node0][node1] = distance
edges[node1][node0] = distance
solution = None
last_solution = None
max_edge_tries = num_nodes
max_iters = 1000
tolerance = 0.0000001
max_zero_diffs = 100
n_zero_diffs = 0
iters = 0
mesg = ""
# pdb.set_trace()
while True:
iters += 1
node0 = None
node1 = None
found = False
for i in range(max_edge_tries):
# if not last_solution:
# nodes = list(range(num_nodes))
# else:
# nodes = last_solution.path
nodes = list(range(num_nodes))
node0 = nodes[int(random.random() * len(nodes))]
node1 = nodes[int(random.random() * len(nodes))]
if (node0 != node1 and node0 not in edges[node1]
and rrt.line_check(
points[node0], points[node1], self.pf.free,
skip=5)):
found = True
break
if found:
distance = math.sqrt((nodes_x[node0] - nodes_x[node1])**2 +
(nodes_y[node0] - nodes_y[node1])**2)
edges[node0][node1] = distance
edges[node1][node0] = distance
# self.pf.edges[node_map[node0]][node_map[node1]] = distance
# self.pf.edges[node_map[node1]][node_map[node0]] = distance
# n_edges = self.pf.edges_idx.shape[0]
# edges_idx = np.zeros((n_edges + 1, 2), dtype=np.int64)
# edges_idx[:n_edges,:] = self.pf.edges_idx
# edges_idx[n_edges] = (node_map[node0], node_map[node1])
# self.pf.edges_idx = edges_idx
# self.send_redraw()
pf = rrt.PathFinder(free=self.pf.free,
pc=self.pf.pc,
nodes_x=nodes_x,
nodes_y=nodes_y,
edges=edges)
solution, _ = pf.find(self.path_start_point[0],
self.path_start_point[1],
self.path_end_point[0],
self.path_end_point[1])
if not solution:
mesg = "Didn't get back a path solution"
solution = last_soluton or None
break
cost = solution.cost
delta = last_cost - cost
if delta < 0:
last_cost = cost
last_solution = solution
continue
if delta < tolerance:
mesg = "Got a diff {} < which is less than {}".format(
delta, tolerance)
n_zero_diffs += 1
if n_zero_diffs == max_zero_diffs:
break
else:
continue
n_zero_diffs = 0
last_cost = cost
last_solution = solution
if iters > max_iters:
mesg = "Ran out of iterations {}".format(max_iters)
break
else:
mesg = "Couldn't find a new edge after {} tries.".format(
max_edge_tries)
break
self.send_status_message(mesg)
if solution:
# Add new edges in path to RRT
edges_to_add = []
for i, node1 in enumerate(solution.path):
if i:
node0 = solution.path[i - 1]
if node_map[node1] not in self.pf.edges[node_map[node0]]:
edges_to_add.append((node0, node1))
for node0, node1 in edges_to_add:
self.pf.edges[node_map[node0]][
node_map[node1]] = edges[node0][node1]
self.pf.edges[node_map[node1]][
node_map[node0]] = edges[node1][node0]
self.pf.edges_idx = np.vstack((self.pf.edges_idx,
np.array([(node_map[i], node_map[j])
for i, j in edges_to_add],
dtype=np.int64)))
# Copy to path to main path
for i, node in enumerate(solution.path):
solution.path[i] = node_map[node]
self.solution = solution
self.send_redraw()
def main(scene_idx):
scene_name = Test_Scenes[scene_idx]
scene_file_addr = '/home/reza/Datasets/GibsonEnv/my_code/visual_servoing/sample_image_pairs_test/{}'.format(
scene_name)
#scene_name = Train_Scenes[scene_idx]
#scene_file_addr = '/home/reza/Datasets/GibsonEnv/my_code/visual_servoing/sample_image_pairs_train/{}'.format(scene_name)
create_folder(scene_file_addr)
## rrt functions
## first figure out how to sample points from rrt graph
rrt_directory = '/home/reza/Datasets/GibsonEnv/gibson/assets/dataset/{}_for_rrt'.format(
scene_name)
path_finder = rrt.PathFinder(rrt_directory)
path_finder.load()
num_nodes = len(path_finder.nodes_x)
free = cv2.imread(
'/home/reza/Datasets/GibsonEnv/gibson/assets/dataset/{}_for_rrt/free.png'
.format(scene_name), 0)
## GibsonEnv setup
config_file = os.path.join(
'/home/reza/Datasets/GibsonEnv/my_code/CVPR_workshop', 'env_yamls',
'{}_navigate.yaml'.format(scene_name))
env = HuskyNavigateEnv(config=config_file, gpu_count=1)
obs = env.reset(
) ## this line is important otherwise there will be an error like 'AttributeError: 'HuskyNavigateEnv' object has no attribute 'potential''
def get_obs(current_pose):
pos, orn = func_pose2posAndorn(current_pose,
mapper_scene2z[scene_name])
env.robot.reset_new_pose(pos, orn)
obs, _, _, _ = env.step(4)
obs_rgb = obs['rgb_filled']
obs_depth = obs['depth']
return obs_rgb.copy(), obs_depth.copy()
left_pose_list = mapper_scene2points[scene_name]
right_pose_list = []
for p_idx, p in enumerate(left_pose_list):
#for p_idx in range(0, 1):
#p = left_pose_list[p_idx]
list_whole = []
x0, y0, theta0 = p
left_pose = [x0, y0, theta0]
point_file_addr = '{}/point_{}'.format(scene_file_addr, p_idx)
create_folder(point_file_addr)
current_pose = left_pose
left_rgb, left_depth = get_obs(current_pose)
cv2.imwrite('{}/left_img.png'.format(point_file_addr),
left_rgb[:, :, ::-1])
np.save('{}/left_img_depth.npy'.format(point_file_addr), left_depth)
## add left_img to list_whole
current_dict = {}
current_dict['img_name'] = 'left_img'
current_dict['pose'] = left_pose
list_whole.append(current_dict)
for i in range(len(theta_list)):
if i == 0 or i == 6:
len_dist_list = 2
elif i == 1 or i == 5:
len_dist_list = 3
elif i == 2 or i == 4:
len_dist_list = 4
elif i == 3:
len_dist_list = 6
print('len_dist_list = {}'.format(len_dist_list))
for j in range(len_dist_list):
location_theta = plus_theta_fn(theta0, theta_list[i])
location_dist = dist_list[j]
x1 = x0 + location_dist * math.cos(location_theta)
y1 = y0 + location_dist * math.sin(location_theta)
left_pixel = path_finder.point_to_pixel(left_pose)
right_pixel = path_finder.point_to_pixel((x1, y1))
# check the line
flag = rrt.line_check(left_pixel, right_pixel, free)
if not flag:
print('j = {}, obstacle'.format(j))
else:
for diff_theta_idx in range(len(diff_theta_list)):
diff_theta = diff_theta_list[diff_theta_idx]
theta1 = plus_theta_fn(theta0, diff_theta)
right_pose = [x1, y1, theta1]
current_pose = right_pose
right_rgb, right_depth = get_obs(current_pose)
## check if there is common space between left img and right img
kp1, kp2 = sample_gt_dense_correspondences(
left_depth,
right_depth,
left_pose,
right_pose,
gap=32,
focal_length=128,
resolution=256,
start_pixel=31)
if kp1.shape[1] > 2:
cv2.imwrite(
'{}/right_img_dist_{}_theta_{}_heading_{}.png'.
format(point_file_addr, mapper_dist[j],
mapper_theta[i],
mapper_theta[diff_theta_idx]),
right_rgb[:, :, ::-1])
np.save(
'{}/right_img_dist_{}_theta_{}_heading_{}_depth.npy'
.format(point_file_addr, mapper_dist[j],
mapper_theta[i],
mapper_theta[diff_theta_idx]),
right_depth)
right_pose_list.append(right_pose)
## add right_img to list_whole
current_dict = {}
current_dict[
'img_name'] = 'right_img_dist_{}_theta_{}_heading_{}'.format(
mapper_dist[j], mapper_theta[i],
mapper_theta[diff_theta_idx])
current_dict['pose'] = right_pose
list_whole.append(current_dict)
else:
print('No common space')
## save list_whole
np.save('{}/point_{}_poses.npy'.format(scene_file_addr, p_idx),
list_whole)
# plot the pose graph
pose_file_addr = '{}'.format(scene_file_addr)
img_name = '{}_sampled_poses.jpg'.format(scene_name)
print('img_name = {}'.format(img_name))
## plot the poses
free = cv2.imread(
'/home/reza/Datasets/GibsonEnv/gibson/assets/dataset/{}_for_rrt/free.png'
.format(scene_name), 1)
rows, cols, _ = free.shape
plt.imshow(free)
for m in range(len(left_pose_list)):
pose = left_pose_list[m]
x, y = path_finder.point_to_pixel((pose[0], pose[1]))
theta = pose[2]
plt.arrow(x, y, cos(theta), sin(theta), color='r', \
overhang=1, head_width=0.1, head_length=0.15, width=0.001)
for m in range(len(right_pose_list)):
pose = right_pose_list[m]
x, y = path_finder.point_to_pixel((pose[0], pose[1]))
theta = pose[2]
plt.arrow(x, y, cos(theta), sin(theta), color='b', \
overhang=1, head_width=0.1, head_length=0.15, width=0.001)
plt.axis([0, cols, 0, rows])
plt.xticks([])
plt.yticks([])
plt.savefig('{}/{}'.format(pose_file_addr, img_name),
bbox_inches='tight',
dpi=(400))
plt.close()
