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 = '{}/learnedCorrespondence_interMatrix_noDepth_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, 10): 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) try: kp1, kp2 = kpNet.detect_learned_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] omegay, flag_stop = compute_angular_velocity_through_correspondences( kp1, kp2, 100) previous_pose = current_pose current_pose = update_current_pose(current_pose, 0.0, 0.1, 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: print('flag_stop = {}'.format(flag_stop)) 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' )
'/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)
import argparse import os import numpy as np import matplotlib.pyplot as plt config_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), '..', 'configs', 'test_control.yaml') print(config_file) if __name__ == '__main__': parser = argparse.ArgumentParser() parser.add_argument('--config', type=str, default=config_file) args = parser.parse_args() env = HuskyNavigateEnv(config=args.config) env.reset() base_action_omage = np.array([-0.001, 0.001, -0.001, 0.001]) base_action_v = np.array([0.001, 0.001, 0.001, 0.001]) control_signal = -0.5 control_signal_omega = 0.5 v = 0 omega = 0 kp = 100 ki = 0.1 kd = 25 ie = 0 de = 0 olde = 0 ie_omega = 0
def main(scene_idx=0, sigma=0.0, keep_prob=1.0): #scene_idx = 0 #keep_prob = 0.95 #sigma = 5.0 #print('keep_prob = {}'.format(keep_prob)) #print('sigma = {}'.format(sigma)) ## 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] ## create test folder test_folder = '/home/reza/Datasets/GibsonEnv/my_code/vs_controller/test_LBVS' f = open('{}/{}.txt'.format(test_folder, method_description), 'a') f.write('sigma = {}, keep_prob = {}\n'.format(sigma, keep_prob)) f.write('scene_name = {}\n'.format(scene_name)) list_count_correct = [] list_count_runs = [] 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_siamese_fusion_new(3).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) 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))) 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): 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() count_correct = 0 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, 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_depth = goal_depth.copy() current_depth = start_depth.copy() episode_reward = 0 flag_succ = False 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] ## add gaussian noise overlapArea = addGaussianNoise(overlapArea, sigma=sigma) #overlapArea = removeCorrespondenceRandomly(overlapArea, keep_prob=keep_prob) overlapArea = removeCorrespondenceRandomly_withSmoothing(overlapArea, keep_prob=keep_prob) 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) 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() if flag_succ: count_succ += 1 count_correct += 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('sigma = {}, keep_prob = {}, num_succ = {}, num_run = {}, count_short_runs_succ = {}, count_short_runs = {}'.format(sigma, keep_prob, count_succ, len(point_pose_npy_file)-1, count_short_runs_succ, count_short_runs)) f.write('point {} : {}/{}\n'.format(point_idx, count_correct, len(point_pose_npy_file)-1)) f.flush() list_count_correct.append(count_correct) list_count_runs.append(len(point_pose_npy_file)-1) f.write('In total : {}/{}\n'.format(sum(list_count_correct), sum(list_count_runs))) 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_old(left_pose, right_pose, lamb=0.5): x1, y1 = left_pose[0], left_pose[1] a1, b1 = cos(left_pose[2]), sin(left_pose[2]) x2, y2 = right_pose[0], right_pose[1] a2, b2 = cos(right_pose[2]), sin(right_pose[2]) x_y_dist = math.sqrt((x1-x2)**2 + (y1-y2)**2) theta_dist = math.sqrt((a1-a2)**2 + (b1-b2)**2) return x_y_dist + lamb * theta_dist 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_overlap_resnet(trained_model_path=None, num_actions=action_space, input_channels=4) agent = DQN_vs_overlap_resnet(trained_model_path=model_weights_save_path, num_actions=action_space, input_channels=4) 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() opticalFlow = genGtDenseCorrespondenseFlowMap(start_depth, goal_depth, start_pose, goal_pose)[:,:,:2] memory_opticalFlow = np.concatenate((opticalFlow, opticalFlow), axis=2) state = [memory_opticalFlow] 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_opticalFlow = genGtDenseCorrespondenseFlowMap(next_left_depth, goal_depth, current_pose, goal_pose)[:,:,:2] memory_new_opticalFlow = np.concatenate((state[0][:,:,2:4], new_opticalFlow), axis=2) ''' fig = plt.figure(figsize=(15, 5)) #cols, rows r, c = 1, 6 ax = fig.add_subplot(r, c, 1) ax.imshow(memory_new_opticalFlow[:, :, 0]) ax = fig.add_subplot(r, c, 2) ax.imshow(memory_new_opticalFlow[:, :, 1]) ax = fig.add_subplot(r, c, 3) ax.imshow(memory_new_opticalFlow[:, :, 2]) ax = fig.add_subplot(r, c, 4) ax.imshow(memory_new_opticalFlow[:, :, 3]) plt.show() ''' #print('memory_new_opticalFlow.shape = {}'.format(memory_new_opticalFlow.shape)) new_state = [memory_new_opticalFlow] ## 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))
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_file_addr = '/home/reza/Datasets/GibsonEnv/my_code/visual_servoing/sample_image_pairs_test_longer_dist/{}'.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 = 10 #4 elif i == 3: len_dist_list = 10 #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()