def __init__(self, train_mode=True, area_reward_scale=0.0005, collision_penalty=0.01, step_penalty=0.0, max_depth=2.0, render_door=False, start_indoor=True, ignore_collision=False, ob_dilation_kernel=5, depth_signal=True, max_steps=500): self.seed() self.configs = get_configs() self.env = None self.train_mode = train_mode self.render_door = render_door self.ignore_collision = ignore_collision self.start_indoor = start_indoor self.render_height = self.configs['render_height'] self.render_width = self.configs['render_width'] self.ob_dilation_kernel = ob_dilation_kernel self.config = load_config(self.configs['path'], prefix=self.configs['par_path']) self.move_sensitivity = self.configs['move_sensitivity'] self.rot_sensitivity = self.configs['rot_sensitivity'] self.train_houses = self.configs['train_houses'] self.test_houses = self.configs['test_houses'] if train_mode: self.houses_id = self.train_houses else: self.houses_id = self.test_houses self.depth_threshold = (0, max_depth) self.area_reward_scale = area_reward_scale self.collision_penalty = collision_penalty self.step_penalty = step_penalty self.max_step = max_steps n_channel = 3 if depth_signal: n_channel += 1 self.observation_shape = (self.render_width, self.render_height, n_channel) self.observation_space = spaces.Box(0, 255, shape=self.observation_shape, dtype=np.uint8) #self.action_space = spaces.Discrete(n_discrete_actions) self.action_space = spaces.Box(low=np.array([0.0, -1.0, -1.0]), high=np.array([1.0, 1.0, 1.0]), dtype=np.float32) self.tracker = [] self.num_rotate = 0 self.right_rotate = 0
def run_sim(rank, params, shared_model, shared_optimizer, count, lock): if not os.path.exists('./' + params.weight_dir): os.mkdir('./' + params.weight_dir) if not os.path.exists('./log'): os.mkdir('./log') logging.basicConfig(filename='./log/' + params.log_file + '.log', level=logging.INFO) ptitle('Training Agent: {}'.format(rank)) gpu_id = params.gpu_ids_train[rank % len(params.gpu_ids_train)] api = objrender.RenderAPI(w=params.width, h=params.height, device=gpu_id) cfg = load_config('config.json') torch.manual_seed(random.randint(0, 1000) + rank) if gpu_id >= 0: torch.cuda.manual_seed(random.randint(0, 1000) + rank) model = A3C_LSTM_GA() with torch.cuda.device(gpu_id): model = model.cuda() Agent = run_agent(model, gpu_id) house_id = params.house_id if house_id == -1: house_id = rank if house_id > 50: house_id = house_id % 50 env = Environment(api, get_house_id(house_id, params.difficulty), cfg) task = RoomNavTask(env, hardness=params.hardness, segment_input=params.semantic_mode, max_steps=params.max_steps, discrete_action=True) n_train = 0 best_rate = 0.0 save_model_index = 0 while True: n_train += 1 training(task, gpu_id, shared_model, Agent, shared_optimizer, params, lock, count) if n_train % 1000 == 0: with lock: n_update = count.value with torch.cuda.device(gpu_id): Agent.model.load_state_dict(shared_model.state_dict()) start_time = time.time() best_rate, save_model_index = testing(lock, n_update, gpu_id, Agent, task, best_rate, params, save_model_index, start_time, logging, house_id)
def create_env(self, config_path, house_id, render_width=args.render_width, render_height=args.render_height): api = objrender.RenderAPIThread(w=render_width, h=render_height, device=0) cfg = load_config(config_path) self.env = Environment(api, house_id, cfg)
def atari_env(env_id, args, type): api = objrender.RenderAPI(w=OBSERVATION_SPACE_SHAPE[2], h=OBSERVATION_SPACE_SHAPE[1], device=args.gpu_ids[0]) cfg = load_config('config_namazu.json') if type == 'train': pre_env = Environment(api, HOUSE_train[env_id], cfg, seed=args.seed) # TODO: temporarily use HOUSE # pre_env = Environment(api, HOUSE[env_id], cfg) elif type == 'test': pre_env = Environment(api, HOUSE_test[env_id], cfg, seed=args.seed) # TODO: temporarily use HOUSE # pre_env = Environment(api, HOUSE[env_id], cfg) env = RoomNavTask(pre_env, hardness=args.hardness, depth_signal=False, discrete_action=True, max_steps=args.max_episode_length) return env
def gen_cache_files(ids, skip_file): configs = get_configs() config = load_config(configs['path'], prefix=configs['par_path']) render_height = configs['render_height'] render_width = configs['render_width'] with open(skip_file, 'r') as f: skip_houses = json.load(f) for idx in tqdm(ids): if idx in skip_houses: continue print(idx) api = objrender.RenderAPI(render_width, render_height, device=0) try: env = Environment(api, idx, config, GridDet=configs['GridDet']) except AssertionError: skip_houses.append(idx) return skip_houses
def test_render(self): api = objrender.RenderAPI(w=SIDE, h=SIDE, device=0) cfg = load_config('config.json') houseID, house = find_first_good_house(cfg) env = Environment(api, house, cfg) location = house.getRandomLocation(ROOM_TYPE) env.reset(*location) # Check RGB env.set_render_mode(RenderMode.RGB) rgb = env.render_cube_map() self.assertEqual(rgb.shape, (SIDE, SIDE * 6, 3)) # Check SEMANTIC env.set_render_mode(RenderMode.RGB) semantic = env.render_cube_map() self.assertEqual(semantic.shape, (SIDE, SIDE * 6, 3)) # Check INSTANCE env.set_render_mode(RenderMode.INSTANCE) instance = env.render_cube_map() self.assertEqual(instance.shape, (SIDE, SIDE * 6, 3)) # Check DEPTH env.set_render_mode(RenderMode.DEPTH) depth = env.render_cube_map() self.assertEqual(depth.dtype, np.uint8) self.assertEqual(depth.shape, (SIDE, SIDE * 6, 2)) depth_value = depth[0, 0, 0] * 20.0 / 255.0 # Check INVDEPTH env.set_render_mode(RenderMode.INVDEPTH) invdepth = env.render_cube_map() self.assertEqual(invdepth.dtype, np.uint8) self.assertEqual(invdepth.shape, (SIDE, SIDE * 6, 3)) # Convert to 16 bit and then to depth id16 = 256 * invdepth[:, :, 0] + invdepth[:, :, 1] depth2 = depth_of_inverse_depth(id16) self.assertEqual(depth2.dtype, np.float) self.assertEqual(depth2.shape, (SIDE, SIDE * 6)) # Check that depth value is within 5% of depth from RenderMode.DEPTH self.assertAlmostEqual( depth2[0, 0], depth_value, delta=depth_value * 0.05)
def __init__(self, train_mode=True, area_reward_scale=1, collision_penalty=0.1, step_penalty=0.0005, max_depth=3.0, render_door=False, start_indoor=False, ignore_collision=False, ob_dilation_kernel=5, large_map_size=80): self.seed() self.configs = get_configs() self.configs['large_map_size'] = large_map_size self.env = None self.train_mode = train_mode self.render_door = render_door self.ignore_collision = ignore_collision self.start_indoor = start_indoor self.render_height = self.configs['render_height'] self.render_width = self.configs['render_width'] self.img_height = self.configs['output_height'] self.img_width = self.configs['output_width'] self.ob_dilation_kernel = ob_dilation_kernel self.config = load_config(self.configs['path'], prefix=self.configs['par_path']) self.move_sensitivity = self.configs['move_sensitivity'] self.rot_sensitivity = self.configs['rot_sensitivity'] self.train_houses = self.configs['train_houses'] self.test_houses = self.configs['test_houses'] if train_mode: self.houses_id = self.train_houses # print("Number of traning houses:", len(self.houses_id)) else: self.houses_id = None # self.test_houses self.depth_threshold = (0, max_depth) self.area_reward_scale = area_reward_scale self.collision_penalty = collision_penalty self.step_penalty = step_penalty self.observation_space = [self.img_width, self.img_height, 3] self.action_space = [6]
def main(): parser = argparse.ArgumentParser( description='Remove components in House3D') parser.add_argument('--eqa_file', type=str, default='../../path_data/eqa_v1.json') args = parser.parse_args() with open(args.eqa_file, 'r') as f: eqa_dataset = json.load(f) train_ids = eqa_dataset['splits']['train'] val_ids = eqa_dataset['splits']['val'] test_ids = eqa_dataset['splits']['test'] total_ids = train_ids + val_ids + test_ids configs = get_configs() config = load_config(configs['path'], prefix=configs['par_path']) door_ids = get_door_ids(config['modelCategoryFile']) for idx in tqdm(total_ids): src_folder = os.path.join(config['prefix'], idx) input_json_file = os.path.join(src_folder, 'house.json') output_json_file = os.path.join(src_folder, 'house-no-doors.json') write_json(input_json_file, output_json_file, door_ids)
def main(): parser = argparse.ArgumentParser() parser.add_argument( '-questions_json', default='/private/home/akadian/eqa-data/suncg-data/eqa_v1.json') parser.add_argument( '-graph_dir', default='/private/home/akadian/eqa-data/suncg-data/a-star') parser.add_argument( '-target_obj_conn_map_dir', default= '/private/home/akadian/eqa-data/suncg-data/a-star/target_obj_conn_map_dir' ) parser.add_argument('-shortest_path_dir') parser.add_argument( '-invalids_dir', default='/private/home/akadian/eqa-data/suncg-data/invalids/') parser.add_argument('-env_id', default=None) parser.add_argument('-debug', action='store_true') parser.add_argument('-map_resolution', default=1000, type=int) parser.add_argument('-seed', type=int, required=True) parser.add_argument('-check_validity', action="store_true") parser.add_argument('-log_path', default=None) parser.add_argument('-source_candidate_fraction', type=float, default=0.05) args = parser.parse_args() if args.log_path is None: args.log_path = 'seed_{}_resolution_{}.{}.log'.format( args.seed, args.map_resolution, str(datetime.now()).replace(' ', '_')) logging.basicConfig(filename=args.log_path, level=logging.INFO, format='%(asctime)-15s %(message)s') random.seed(args.seed) np.random.seed(args.seed) if not os.path.exists(args.shortest_path_dir): os.makedirs(args.shortest_path_dir) args.gpus = os.environ['CUDA_VISIBLE_DEVICES'].split(',') args.gpus = [int(x) for x in args.gpus] # create specific directories corresponding to the resolution args.graph_dir = os.path.join(args.graph_dir, str(args.map_resolution)) args.target_obj_conn_map_dir = os.path.join(args.target_obj_conn_map_dir, str(args.map_resolution)) # load house3d renderer cfg = load_config('../../House3D/tests/config.json') api_thread = objrender.RenderAPIThread(w=224, h=224, device=args.gpus[0]) # load envs list from questions json data = json.load(open(args.questions_json, 'r')) qns = data['questions'] if args.env_id is None: envs = sorted(list(set(qns.keys()))) else: envs = [args.env_id] random.shuffle(envs) invalid = [] count_path_found = 0 count_valid = 0 count_path_not_found = 0 count_no_source_cands = 0 shortest_path_lengths = [] for h in tqdm(envs): # `scn2scn` from suncg-toolbox segfaults for this env :/ if h == '436d655f24d385512e1e782b5ba88c6b': continue for q in qns[h]: logging.info("count_path_found: {}".format(count_path_found)) logging.info("count_valid: {}".format(count_valid)) logging.info( "count_path_not_found: {}".format(count_path_not_found)) logging.info( "count_no_source_cands: {}".format(count_no_source_cands)) if len(shortest_path_lengths) > 0: logging.info( "shortest path length mean: {}, median: {}, min: {}, max: {}" .format(np.mean(shortest_path_lengths), np.median(shortest_path_lengths), np.min(shortest_path_lengths), np.max(shortest_path_lengths))) logging.info("env, question pair: {}_{}".format(h, q['id'])) logging.info("{} {} {}".format(h, q['question'], q['answer'])) env = Environment(api_thread, h, cfg, ColideRes=args.map_resolution) h3d = House3DUtils( env, graph_dir=args.graph_dir, target_obj_conn_map_dir=args.target_obj_conn_map_dir, build_graph=False) if os.path.exists( os.path.join(args.shortest_path_dir, "{}_{}.pkl".format(h, q['id']))): logging.info("Shortest path exists") continue # set target object bbox_obj = [ x for x in q['bbox'] if x['type'] == 'object' and x['target'] is True ][0] obj_id = [] for x in h3d.objects: if all([h3d.objects[x]['bbox']['min'][i] == bbox_obj['box']['min'][i] for i in range(3)]) and \ all([h3d.objects[x]['bbox']['max'][i] == bbox_obj['box']['max'][i] for i in range(3)]): obj_id.append(x) if h3d.objects[x]['fine_class'] != bbox_obj['name']: logging.info('Name not matched {} {}'.format( h3d.objects[x]['fine_class'], bbox_obj['name'])) assert len(obj_id) == 1 bbox_room = [ x for x in q['bbox'] if x['type'] == 'room' and x['target'] is False ][0] target_room = False for room in h3d.env.house.all_rooms: if all([room['bbox']['min'][i] == bbox_room['box']['min'][i] for i in range(3)]) and \ all([room['bbox']['max'][i] == bbox_room['box']['max'][i] for i in range(3)]): target_room = room break target_obj = obj_id[0] h3d.set_target_object(h3d.objects[target_obj], target_room) # sample a close enough target point target_point_cands = np.argwhere((env.house.connMap >= 0) & (env.house.connMap <= 5)) target_point_idx = np.random.choice(target_point_cands.shape[0]) target_yaw, best_coverage = h3d._get_best_yaw_obj_from_pos( target_obj, [ target_point_cands[target_point_idx][0], target_point_cands[target_point_idx][1] ], height=1.0) target_point = (target_point_cands[target_point_idx][0], target_point_cands[target_point_idx][1], target_yaw) # graph creation used for selecting a source point t1 = time() if os.path.exists( os.path.join( h3d.graph_dir, h3d.env.house.house['id'] + '_' + target_obj + '.pkl')): print('loading graph') h3d.load_graph( os.path.join( h3d.graph_dir, h3d.env.house.house['id'] + '_' + target_obj + '.pkl')) else: print('building graph') h3d.build_graph(save_path=os.path.join( h3d.graph_dir, h3d.env.house.house['id'] + '_' + target_obj + '.pkl')) connmap_values = env.house.connMap.flatten() connmap_values.sort() # threshold for --source_candidate_fraction number of points thresh = connmap_values[int( (1.0 - args.source_candidate_fraction) * connmap_values.shape[0])] source_point_cands = np.argwhere((env.house.connMap != -1) & (env.house.connMap >= thresh)) if thresh < 50: # sanity check to prevent scenario when agent is spawned close to target location logging.info("No source candidates") invalid.append(h) count_no_source_cands += 1 continue t2 = time() logging.info("Time spent for graph creation {:.6f}s".format(t2 - t1)) for it in range(10): logging.info("Try: {}".format(it)) try: source_point_idx = np.random.choice( source_point_cands.shape[0]) source_point = (source_point_cands[source_point_idx][0], source_point_cands[source_point_idx][1], np.random.choice(h3d.angles)) # A* for shortest path t3 = time() target_x, target_y, target_yaw = target_point source_continous = h3d.env.house.to_coor(source_point[0], source_point[1], shft=True) target_continous = h3d.env.house.to_coor(target_x, target_y, shft=True) points_queue = [] distances_source = dict() prev_pos = dict() distances_source[source_point] = 0 prev_pos[source_point] = (-1.0, -1.0, -1.0, -1.0, -1.0) # schema for point in points_queue: # (x-grid-location, y-grid-location, yaw, x-continous-coordinate, y-continous-coordinate) source_point = (source_point[0], source_point[1], source_point[2], source_continous[0], source_continous[1]) heappush(points_queue, (heuristic_estimate( source_continous, target_continous), source_point)) while True: if len(points_queue) == 0: count_path_not_found += 1 logging.info("A* not able to find path to target") raise ValueError( "Path not found to target {} {}".format( source_point[:3], target_point)) f_dist, point = heappop(points_queue) add_neighbors(h3d, points_queue, point, distances_source, prev_pos, target_continous) if point[0] == target_x and point[ 1] == target_y and point[2] == target_yaw: # store path shortest_path_nodes = [] while True: shortest_path_nodes.append(point) point = prev_pos[point[:3]] if point[0] == -1: break shortest_path_nodes.reverse() break t4 = time() logging.info( "Time spent for coupled graph generation and A*: {:.6f}s" .format(t4 - t3)) # bookkeeping act_q, pos_q, coord_q, actual_q = [], [], [], [] episode_images = [] movemap = None for i in range(len(shortest_path_nodes) - 1): u = shortest_path_nodes[i] v = shortest_path_nodes[i + 1] pos_q.append( (float(u[3]), 1.0, float(u[4]), float(u[2]))) coord_q.append(h3d.env.house.to_grid(u[3], u[4])) curr_x, curr_y, curr_yaw = u[3], u[4], u[2] next_x, next_y, next_yaw = v[3], v[4], v[2] if curr_yaw != next_yaw: if next_yaw == 171 and curr_yaw == -180: act_q.append(1) elif next_yaw == -180 and curr_yaw == 171: act_q.append(2) elif next_yaw < curr_yaw: act_q.append(1) else: act_q.append(2) else: act_q.append(0) pos_q.append((shortest_path_nodes[-1][3], 1.0, shortest_path_nodes[-1][4], shortest_path_nodes[-1][2])) act_q.append(3) if args.check_validity: h3d.env.reset(x=pos_q[0][0], y=pos_q[0][2], yaw=pos_q[0][3]) h3d_yaw = pos_q[0][ 3] # dummy yaw limited to [-180, 180) actual_q.append( (float(h3d.env.cam.pos.x), 1.0, float(h3d.env.cam.pos.z), float(h3d.env.cam.yaw))) for i, action in enumerate(act_q[:-1]): pre_pos = [ h3d.env.cam.pos.x, h3d.env.cam.pos.z, h3d.env.cam.yaw ] img, _, episode_done = h3d.step(action) episode_images.append(img) post_pos = [ h3d.env.cam.pos.x, h3d.env.cam.pos.z, h3d.env.cam.yaw ] actual_q.append((float(h3d.env.cam.pos.x), 1.0, float(h3d.env.cam.pos.z), float(h3d.env.cam.yaw))) if all([ np.abs(pre_pos[x] - post_pos[x]) < 1e-9 for x in range(3) ]): raise ValueError("Invalid action") angle_delta = post_pos[2] - pre_pos[2] h3d_yaw = (h3d_yaw + 180 + angle_delta) % 360 - 180 assert np.abs(h3d.env.cam.pos.x - pos_q[i + 1][0]) < 1e-3 assert np.abs(h3d.env.cam.pos.z - pos_q[i + 1][2]) < 1e-3 assert h3d_yaw == pos_q[i + 1][3] count_valid += 1 movemap = h3d.env.house._showMoveMap(visualize=False) logging.info("Valid") result = { "actions": act_q, "actual_q": actual_q, "answer": q['answer'], "coordinates": coord_q, "images": episode_images, "movemap": movemap, "positions": pos_q, "question": q['question'], } with open( os.path.join(args.shortest_path_dir, "{}_{}.pkl".format(h, q['id'])), "wb") as f: pickle.dump(result, f) logging.info("Saved {}_{}.pkl".format(h, q['id'])) logging.info("Length of shortest path: {}".format( len(shortest_path_nodes))) shortest_path_lengths.append(len(shortest_path_nodes)) count_path_found += 1 break except KeyboardInterrupt: raise except: invalid.append("env, question pair: {}_{}".format( h, q['id'])) traceback.print_exc()
try: os.remove(path) except Exception as e: print(path, e) if __name__ == '__main__': # Get house ids to generate data for house_ids = open(args.house_list, 'r').readlines()[args.hid_lo:args.hid_hi] # Initialize renderer API and config api = objrender.RenderAPI(w=args.render_width, h=args.render_height, device=0) cfg = load_config(args.render_config_path) for h in house_ids: house_id = h[:-1] check_path = os.path.join(args.question_dir, house_id + '.json') print(check_path) if os.path.exists(check_path): print('Already have questions for this house --- SKIPPING') continue # Create house .obj and .mtl files in SUNCG scn2scn_path = os.path.join(args.suncgtoolbox_dir, 'gaps/bin/x86_64/scn2scn') house_file_dir = os.path.join(args.suncg_dir, 'house', house_id) house_file_prefix = os.path.join(house_file_dir, 'house')
def _load_envs(self, start_idx=-1, in_order=False): #self._clear_memory() if start_idx == -1: start_idx = self.env_set.index(self.pruned_env_set[-1]) + 1 # Pick envs self.pruned_env_set = self._pick_envs_to_load( split=self.split, max_envs=self.max_threads_per_gpu, start_idx=start_idx, in_order=in_order) if len(self.pruned_env_set) == 0: return # Load api threads start = time.time() if len(self.api_threads) == 0: for i in range(self.max_threads_per_gpu): self.api_threads.append( objrender.RenderAPIThread(w=224, h=224, device=self.gpu_id)) self.cfg = load_config('../../House3D/tests/config.json') print('[%.02f] Loaded %d api threads' % (time.time() - start, len(self.api_threads))) start = time.time() # Load houses from multiprocessing import Pool _args = ([h, self.cfg, self.map_resolution] for h in self.pruned_env_set) with Pool(len(self.pruned_env_set)) as pool: self.all_houses = pool.starmap(local_create_house, _args) print('[%.02f] Loaded %d houses' % (time.time() - start, len(self.all_houses))) start = time.time() # Load envs self.env_loaded = {} for i in range(len(self.all_houses)): print('[%02d/%d][split:%s][gpu:%d][house:%s]' % (i + 1, len(self.all_houses), self.split, self.gpu_id, self.all_houses[i].house['id'])) environment = Environment(self.api_threads[i], self.all_houses[i], self.cfg) self.env_loaded[self.all_houses[i].house['id']] = House3DUtils( environment, target_obj_conn_map_dir=self.target_obj_conn_map_dir, build_graph=False) # [TODO] Unused till now self.env_ptr = -1 print('[%.02f] Loaded %d house3d envs' % (time.time() - start, len(self.env_loaded))) # Mark available data indices self.available_idx = [ i for i, v in enumerate(self.env_list) if v in self.env_loaded ] # [TODO] only keeping legit sequences # needed for things to play well with old data temp_available_idx = self.available_idx.copy() for i in range(len(temp_available_idx)): if self.action_lengths[temp_available_idx[i]] < 5: self.available_idx.remove(temp_available_idx[i]) print('Available inds: %d' % len(self.available_idx)) # Flag to check if loaded envs have been cycled through or not # [TODO] Unused till now self.all_envs_loaded = False
def show(self, img=None): return self.env.show( img=img, renderMapLoc=(None if img is not None else self.info['grid']), renderSegment=True, display=(img is not None)) def debug_show(self): return self.env.debug_render() if __name__ == '__main__': from House3D import load_config api = objrender.RenderAPI(w=400, h=300, device=0) cfg = load_config('config_thinkPad.json') houses = [ '00065ecbdd7300d35ef4328ffe871505', 'cf57359cd8603c3d9149445fb4040d90', '31966fdc9f9c87862989fae8ae906295', 'ff32675f2527275171555259b4a1b3c3', '7995c2a93311717a3a9c48d789563590', '8b8c1994f3286bfc444a7527ffacde86', '775941abe94306edc1b5820e3a992d75', '32e53679b33adfcc5a5660b8c758cc96', '4383029c98c14177640267bd34ad2f3c', '0884337c703e7c25949d3a237101f060', '492c5839f8a534a673c92912aedc7b63', 'a7e248efcdb6040c92ac0cdc3b2351a6', '2364b7dcc432c6d6dcc59dba617b5f4b', 'e3ae3f7b32cf99b29d3c8681ec3be321', 'f10ce4008da194626f38f937fb9c1a03', 'e6f24af5f87558d31db17b86fe269cf2', '1dba3a1039c6ec1a3c141a1cb0ad0757', 'b814705bc93d428507a516b866efda28', '26e33980e4b4345587d6278460746ec4', '5f3f959c7b3e6f091898caa8e828f110', 'b5bd72478fce2a2dbd1beb1baca48abd', '9be4c7bee6c0ba81936ab0e757ab3d61' ] #env = MultiHouseEnv(api, houses[:3], cfg) # use 3 houses
def run_test(rank, params, loaded_model, lock, seen_succ, seen_length, unseen_succ, unseen_length): logging.basicConfig(filename='./log/' + params.log_file + '.log', level=logging.INFO) ptitle('Test Agent: {}'.format(rank)) gpu_id = params.gpu_ids_test[rank % len(params.gpu_ids_test)] api = objrender.RenderAPI(w=params.width, h=params.height, device=gpu_id) cfg = load_config('config.json') torch.manual_seed(params.seed + rank) if gpu_id >= 0: with torch.cuda.device(gpu_id): torch.cuda.manual_seed(params.seed + rank) model = A3C_LSTM_GA() with torch.cuda.device(gpu_id): model = model.cuda() load_model = torch.load( loaded_model, map_location=lambda storage, loc: storage.cuda(gpu_id)) model.load_state_dict(load_model) model.eval() Agent = run_agent(model, gpu_id) n_test = 0 start_time = time.time() while True: house_id = rank + (n_test * params.n_process) if house_id >= 70: break else: if house_id < 20: seen = True house = get_house_id(house_id) else: seen = False house = get_eval_house_id(house_id - (n_test * params.n_process)) env = Environment(api, house, cfg) task = RoomNavTask(env, hardness=params.hardness, segment_input=params.semantic_mode, max_steps=params.max_steps, discrete_action=True) #reward_type='indicator' eval = [] for i in range(params.n_test): next_observation = task.reset() target = task.info['target_room'] target = get_instruction_idx(target) with torch.cuda.device(gpu_id): target = Variable(torch.LongTensor(target)).cuda() Agent.cx = Variable(torch.zeros(1, 256).cuda()) Agent.hx = Variable(torch.zeros(1, 256).cuda()) Agent.target = target step, total_rew, good = 0, 0, 0 done = False while not done: observation = next_observation act = Agent.action_test(observation, target) next_observation, rew, done, info = task.step(actions[act[0]]) total_rew += rew if rew == 10: # success good = 1 step += 1 if done: break eval.append((step, total_rew, good)) if len(eval) > 0: succ = [e for e in eval if e[2] > 0] succ_rate = (len(succ) / len(eval)) * 100 avg_reward = sum([e[1] for e in eval]) / len(eval) avg_length = sum([e[0] for e in eval]) / len(eval) if seen: msg_seen = "Seen" msg_house = house_id else: msg_seen = "Unseen" msg_house = house_id - 20 msg = " ".join([ "++++++++++ Task Stats +++++++++++\n", "Time {}\n".format( time.strftime("%dd %Hh %Mm %Ss", time.gmtime(time.time() - start_time))), "Episode Played: {:d}\n".format(len(eval)), "{:s} House id: {:d}\n".format(msg_seen, msg_house), "Avg Reward = {:5.3f}\n".format(avg_reward), "Avg Length = {:.3f}\n".format(avg_length), "Success rate {:3.2f}%".format(succ_rate) ]) print(msg) logging.info(msg) with lock: if seen: seen_succ.value += len(succ) seen_length.value += sum([e[0] for e in eval]) else: unseen_succ.value += len(succ) unseen_length.value += sum([e[0] for e in eval]) n_test += 1
def run_sim(rank, params, shared_model, shared_optimizer, count, lock): ptitle('Training Agent: {}'.format(rank)) gpu_id = params.gpu_ids_train[rank % len(params.gpu_ids_train)] api = objrender.RenderAPI(w=params.width, h=params.height, device=gpu_id) cfg = load_config('config.json') if shared_optimizer is None: optimizer = optim.Adam(shared_model.parameters(), lr=params.lr, amsgrad=params.amsgrad, weight_decay=params.weight_decay) #optimizer.share_memory() else: optimizer = shared_optimizer torch.manual_seed(params.seed + rank) if gpu_id >= 0: torch.cuda.manual_seed(params.seed + rank) model = A3C_LSTM_GA() with torch.cuda.device(gpu_id): model = model.cuda() Agent = run_agent(model, gpu_id) house_id = params.house_id if house_id == -1: house_id = rank if house_id >= 20: house_id = house_id % 20 env = Environment(api, get_house_id(house_id), cfg) task = RoomNavTask(env, hardness=params.hardness, segment_input=params.semantic_mode, max_steps=params.max_steps, discrete_action=True) for episode in range(params.max_episode): next_observation = task.reset() target = task.info['target_room'] target = get_instruction_idx(target) with torch.cuda.device(gpu_id): target = Variable(torch.LongTensor(target)).cuda() Agent.model.load_state_dict(shared_model.state_dict()) Agent.cx = Variable(torch.zeros(1, 256).cuda()) Agent.hx = Variable(torch.zeros(1, 256).cuda()) Agent.target = target total_reward, num_steps, good = 0, 0, 0 Agent.done = False done = False Agent.eps_len = 0 while not done: num_steps += 1 observation = next_observation act, entropy, value, log_prob = Agent.action_train( observation, target) next_observation, reward, done, info = task.step(actions[act[0]]) rew = np.clip(reward, -1.0, 1.0) Agent.put_reward(rew, entropy, value, log_prob) if num_steps % params.num_steps == 0 or done: if done: Agent.done = done with lock: count.value += 1 Agent.training(next_observation, shared_model, optimizer, params) if done: break
def run_sim(rank, params, state_Queue, action_done, actions, reward_Queue, lock): ptitle('Training Agent: {}'.format(rank)) gpu_id = params.gpu_ids_train[rank % len(params.gpu_ids_train)] api = objrender.RenderAPI(w=params.width, h=params.height, device=gpu_id) cfg = load_config('config.json') house_id = params.house_id if house_id == -1: house_id = rank if house_id > 50: house_id = house_id % 50 env = Environment(api, get_house_id(house_id, params.difficulty), cfg) task = RoomNavTask(env, hardness=params.hardness, segment_input=params.semantic_mode, max_steps=params.max_steps, discrete_action=True) while True: next_observation = task.reset() target = task.info['target_room'] target = get_instruction_idx(target) # with torch.cuda.device(gpu_id): # target = Variable(torch.LongTensor(target)).cuda() total_reward, num_steps, good = 0, 0, 0 done = False test = False while not done: num_steps += 1 observation = next_observation state = rank, [observation, target] state_Queue.put(state) state_Queue.join() # action_done.get() # action done action = actions[rank] if action == 99: test = True break # call for test next_observation, reward, done, info = task.step(action) reward = np.clip(reward, -1.0, 10.0) if reward != -1.0 and reward != 10.0: # make sparse reward reward = 0.0 total_reward += reward rew = [rank, done, reward] # print("send - rank: {:d}, reward: {:3.2f}".format(rank, reward)) reward_Queue.put(rew) reward_Queue.join() if done: break
def __init__(self, questions_h5, vocab, num_frames=1, data_json=False, split='train', gpu_id=0, input_type='ques', max_threads_per_gpu=10, to_cache=False, target_obj_conn_map_dir=False, map_resolution=1000, overfit=False, max_controller_actions=5, max_actions=None): self.questions_h5 = questions_h5 self.vocab = load_vocab(vocab) self.num_frames = num_frames self.max_controller_actions = max_controller_actions np.random.seed() self.data_json = data_json self.split = split self.device = torch.device( 'cuda' if torch.cuda.is_available() else 'cpu') self.gpu_id = gpu_id self.input_type = input_type self.max_threads_per_gpu = max_threads_per_gpu self.target_obj_conn_map_dir = target_obj_conn_map_dir self.map_resolution = map_resolution self.overfit = overfit self.to_cache = to_cache self.img_data_cache = {} print('Reading question data into memory') # self.idx -> Object ID self.idx = _dataset_to_tensor(questions_h5['idx']) self.questions = _dataset_to_tensor(questions_h5['questions']) self.answers = _dataset_to_tensor(questions_h5['answers']) self.actions = _dataset_to_tensor(questions_h5['action_labels']) self.action_lengths = _dataset_to_tensor( questions_h5['action_lengths']) self.cfg = load_config('../../House3D/tests/config.json') # Saty: max_actions is None! if max_actions: # max actions will allow us to create arrays of a certain length. Helpful if you only want to train with 10 actions. assert isinstance(max_actions, int) num_data_items = self.actions.shape[0] new_actions = np.zeros( (num_data_items, max_actions + 2), dtype=np.int64) #Saty: WHY +2? -> for <start> and <end> new_lengths = np.ones( (num_data_items, ), dtype=np.int64) * max_actions for i in range(num_data_items): action_length = int(self.action_lengths[i]) new_actions[i, 0] = 1 new_actions[i, 1:max_actions + 1] = self.actions[ i, action_length - max_actions:action_length].numpy() self.actions = torch.LongTensor(new_actions) self.action_lengths = torch.LongTensor(new_lengths) if self.data_json != False: data = json.load(open(self.data_json, 'r')) self.envs = data['envs'] #Satyen: Gold mine! self.env_idx = data[self.split + '_env_idx'] self.env_list = [self.envs[x] for x in self.env_idx] self.env_set = list(set(self.env_list)) self.env_set.sort() if self.overfit == True: self.env_idx = self.env_idx[:1] self.env_set = self.env_list = [ self.envs[x] for x in self.env_idx ] print('Trying to overfit to [house %s]' % self.env_set[0]) logging.info('Trying to overfit to [house {}]'.format( self.env_set[0])) print('Total envs: %d' % len(list(set(self.envs)))) print('Envs in %s: %d' % (self.split, len(list(set(self.env_idx))))) if input_type != 'ques': '''' If training, randomly sample and load a subset of environments, train on those, and then cycle through to load the rest. On the validation and test set, load in order, and cycle through. For both, add optional caching so that if all environments have been cycled through once, then no need to re-load and instead, just the cache can be used. ''' self.api_threads = [] self._load_envs(start_idx=0, in_order=True) cnn_kwargs = {'num_classes': 191, 'pretrained': True} self.cnn = MultitaskCNN(**cnn_kwargs) self.cnn.eval() self.cnn.to(self.device) self.pos_queue = data[self.split + '_pos_queue'] self.boxes = data[self.split + '_boxes'] if max_actions: for i in range(len(self.pos_queue)): self.pos_queue[i] = self.pos_queue[i][-1 * max_actions:] if input_type == 'pacman': self.planner_actions = self.actions.clone().fill_(0) self.controller_actions = self.actions.clone().fill_(-1) self.planner_action_lengths = self.action_lengths.clone().fill_(0) self.controller_action_lengths = self.action_lengths.clone().fill_( 0) self.planner_hidden_idx = self.actions.clone().fill_(0) self.planner_pos_queue_idx, self.controller_pos_queue_idx = [], [] # parsing flat actions to planner-controller hierarchy print(" Parsing flat actions to planner-controller hierarchy") for i in tqdm(range(len(self.actions))): pa, ca, pq_idx, cq_idx, ph_idx = flat_to_hierarchical_actions( actions=self.actions[i] [:self.action_lengths[i] + 1], # Saty: Take all actions essentially ; This doesn't have <start> and <end> controller_action_lim=max_controller_actions ) # saty: This is 5 self.planner_actions[i][:len(pa)] = torch.Tensor(pa) self.controller_actions[i][:len(ca)] = torch.Tensor(ca) self.planner_action_lengths[i] = len(pa) - 1 self.controller_action_lengths[i] = len(ca) self.planner_pos_queue_idx.append(pq_idx) self.controller_pos_queue_idx.append( cq_idx) # Saty: This is just [1,2,3,4/.. , len(actions)] self.planner_hidden_idx[i][:len(ca)] = torch.Tensor(ph_idx)
def main(): parser = argparse.ArgumentParser() parser.add_argument('--data_file', default='../path_data/eqa_humans.json', type=str) parser.add_argument('--demo_id', default=0, type=int) parser.add_argument('--width', type=int, default=600) parser.add_argument('--height', type=int, default=450) parser.add_argument('--no_display', action='store_true', help='disable display on screen') parser.add_argument('--play_mode', default='auto', type=str, choices=['key', 'auto'], help='play the human demonstration in ' 'keyboard-control mode or auto-play mode') args = parser.parse_args() with open(args.data_file, 'r') as f: data = json.load(f) demo = data[args.demo_id] print('Total time steps:', len(demo['loc'])) locs = np.array(demo['loc'][1:]) ques = demo['question'] answer = demo['answer'] text = 'Q: {0:s} A: {1:s}'.format(ques, answer) text_height = 60 auto_freq = 15 # 15 hz cfg = load_config('config.json') api = objrender.RenderAPI(w=args.width, h=args.height, device=0) env = Environment(api, demo['house_id'], cfg, GridDet=0.05) save_folder = 'demo_{0:d}'.format(args.demo_id) os.makedirs(save_folder, exist_ok=True) if not args.no_display: print_keyboard_help(args.play_mode) t = 0 while True: # print(t) if t < len(locs): env.reset(x=locs[t][0], y=locs[t][2], yaw=locs[t][3]) mat = get_mat(env, text_height, text) cv2.imshow("human_demo", mat) if args.play_mode == 'key': key = cv2.waitKey(0) if key == 27 or key == ord('q'): # esc break elif key == ord('n'): if t < len(locs) - 1: t += 1 elif key == ord('p'): if t > 0: t -= 1 elif key == ord('r'): t = 0 else: print("Unknown key: {}".format(key)) else: key = cv2.waitKey(int(1000 / auto_freq)) if key == 27 or key == ord('q'): # esc break elif key == -1: if t < len(locs) - 1: t += 1 elif key == ord('r'): t = 0 auto_freq = 15 elif key == ord('i'): if auto_freq < 30: auto_freq += 4 elif key == ord('d'): if auto_freq > 3: auto_freq -= 4 else: print("Unknown key: {}".format(key))
def test(rank, params, shared_model, count, lock, best_acc, evaluation=True): if not os.path.exists('./' + params.weight_dir): os.mkdir('./' + params.weight_dir) if not os.path.exists('./log'): os.mkdir('./log') logging.basicConfig(filename='./log/' + params.log_file + '.log', level=logging.INFO) ptitle('Test Agent: {}'.format(rank)) gpu_id = params.gpu_ids_test[rank % len(params.gpu_ids_test)] api = objrender.RenderAPI(w=params.width, h=params.height, device=gpu_id) cfg = load_config('config.json') best_rate = 0.0 save_model_index = 0 n_update = 0 torch.manual_seed(params.seed + rank) if gpu_id >= 0: torch.cuda.manual_seed(params.seed + rank) model = A3C_LSTM_GA() with torch.cuda.device(gpu_id): model = model.cuda() Agent = run_agent(model, gpu_id) house_id = params.house_id if house_id == -1: house_id = rank if house_id >= 20: house_id = house_id % 20 #time.sleep(rank*30) env = Environment(api, get_house_id(house_id), cfg) task = RoomNavTask(env, hardness=params.hardness, segment_input=params.semantic_mode, max_steps=params.max_steps, discrete_action=True) #reward_type='indicator' start_time = time.time() if evaluation is True: max_episode = params.max_episode n_try = params.n_eval else: max_episode = 1 # for loaded model test n_try = params.n_test for episode in range(max_episode): eval = [] if evaluation is True: with lock: n_update = count.value with torch.cuda.device(gpu_id): Agent.model.load_state_dict(shared_model.state_dict()) else: with torch.cuda.device(gpu_id): Agent.model.load_state_dict(shared_model) Agent.model.eval() for i in range(n_try): next_observation = task.reset() target = task.info['target_room'] target = get_instruction_idx(target) with torch.cuda.device(gpu_id): target = Variable(torch.LongTensor(target)).cuda() Agent.cx = Variable(torch.zeros(1, 256).cuda()) Agent.hx = Variable(torch.zeros(1, 256).cuda()) Agent.target = target step, total_rew, good = 0, 0, 0 done = False while not done: observation = next_observation act = Agent.action_test(observation, target) next_observation, rew, done, info = task.step(actions[act[0]]) total_rew += rew if rew == 10: # success good = 1 step += 1 if done: break eval.append((step, total_rew, good)) if len(eval) > 0: succ = [e for e in eval if e[2] > 0] succ_rate = (len(succ) / len(eval)) * 100 if evaluation is True: # evaluation mode with lock: #if best_acc.value >= best_rate: # best_rate = best_acc.value if succ_rate >= best_rate: best_rate = succ_rate with torch.cuda.device(gpu_id): torch.save( Agent.model.state_dict(), params.weight_dir + 'model' + str(n_update) + '.ckpt') save_model_index += 1 #if best_rate > best_acc.value: # best_acc.value = best_rate avg_reward = sum([e[1] for e in eval]) / len(eval) avg_length = sum([e[0] for e in eval]) / len(eval) msg = " ".join([ "++++++++++ Task Stats +++++++++++\n", "Time {}\n".format( time.strftime("%dd %Hh %Mm %Ss", time.gmtime(time.time() - start_time))), "Episode Played: {:d}\n".format(len(eval)), "N_Update = {:d}\n".format(n_update), "House id: {:d}\n".format(house_id), "Avg Reward = {:5.3f}\n".format(avg_reward), "Avg Length = {:.3f}\n".format(avg_length), "Best rate {:3.2f}, Success rate {:3.2f}%".format( best_rate, succ_rate) ]) print(msg) logging.info(msg)
from House3D import objrender, Environment, load_config HOUSEDIR = '/media/z/Data/Object_Searching/code/Environment/houses' CONFIGFILEPATH = "/media/z/Data/Object_Searching/code/Environment/House3D/tests/config.json" house_ids = [ '04f4590d85e296b4c81c5a62f8a99bce', '0880799c157b4dff08f90db221d7f884', '0a12acd2a7039cb122c297fa5b145912', '0b9285c9f090123caaae85500d48ea8f', '0c90efff2ab302c6f31add26cd698bea', '1dba3a1039c6ec1a3c141a1cb0ad0757', '5cf0e1e9493994e483e985c436b9d3bc', '775941abe94306edc1b5820e3a992d75' # 'a7e248efcdb6040c92ac0cdc3b2351a6', # 'f10ce4008da194626f38f937fb9c1a03' ] if __name__ == '__main__': api = objrender.RenderAPI(w=600, h=450, device=0) cfg = load_config(CONFIGFILEPATH) houseid = '5cf0e1e9493994e483e985c436b9d3bc' env = Environment(api, houseid, cfg) # '0a0b9b45a1db29832dd84e80c1347854' env.reset(yaw=0) # put the agent into the house # fourcc = cv2.VideoWriter_fourcc(*'X264') # writer = cv2.VideoWriter('out.avi', fourcc, 30, (1200, 900)) with open('%s/%s/map.txt' % (HOUSEDIR, houseid), 'rb') as mapfile: origin_coor = [ float(i) for i in mapfile.readline().strip('\n').split(' ') ] smap = env.house.smap fmap = env.gen_2dfmap() L_lo = env.house.L_lo L_hi = env.house.L_hi
def _load_envs(self, start_idx=-1, in_order=False): if start_idx == -1: start_idx = self.env_set.index(self.pruned_env_set[-1]) + 1 # Pick envs self.pruned_env_set = self._pick_envs_to_load( split=self.split, max_envs=self.max_threads_per_gpu, start_idx=start_idx, in_order=in_order) if len(self.pruned_env_set) == 0: return # Load api threads start = time.time() if len(self.api_threads) == 0: for i in range(len(self.pruned_env_set)): self.api_threads.append( objrender.RenderAPIThread( w=224, h=224, device=self.gpu_id)) self.cfg = load_config('../House3D/tests/config.json') print('[%.02f] Loaded %d api threads' % (time.time() - start, len(self.api_threads))) start = time.time() # Load houses from multiprocessing import Pool _args = ([h, self.cfg, self.map_resolution] for h in self.pruned_env_set) with Pool(len(self.pruned_env_set)) as pool: self.all_houses = pool.starmap(local_create_house, _args) print('[%.02f] Loaded %d houses' % (time.time() - start, len(self.all_houses))) start = time.time() # Load envs self.env_loaded = {} for i in range(len(self.all_houses)): print('[%02d/%d][split:%s][gpu:%d][house:%s]' % (i + 1, len(self.all_houses), self.split, self.gpu_id, self.all_houses[i].house['id'])) self.env_loaded[self.all_houses[i].house['id']] = House3DUtils( Environment(self.api_threads[i], self.all_houses[i], self.cfg), target_obj_conn_map_dir=self.target_obj_conn_map_dir, build_graph=False) # [TODO] Unused till now self.env_ptr = -1 print('[%.02f] Loaded %d house3d envs' % (time.time() - start, len(self.env_loaded))) # Mark available data indices self.available_idx = [ i for i, v in enumerate(self.env_list) if v in self.env_loaded ] print('Available inds: %d' % len(self.available_idx)) # Flag to check if loaded envs have been cycled through or not # [TODO] Unused till now self.all_envs_loaded = False
from House3D import Environment, objrender, load_config from House3D.roomnav import RoomNavTask from collections import deque import tqdm import numpy as np EPISODE=1000 api = objrender.RenderAPI( w=400, h=300, device=0) cfg = load_config('config.json') houses = ['00065ecbdd7300d35ef4328ffe871505', 'cf57359cd8603c3d9149445fb4040d90', '31966fdc9f9c87862989fae8ae906295', '7995c2a93311717a3a9c48d789563590', '8b8c1994f3286bfc444a7527ffacde86', '32e53679b33adfcc5a5660b8c758cc96', '492c5839f8a534a673c92912aedc7b63', 'e3ae3f7b32cf99b29d3c8681ec3be321', '1dba3a1039c6ec1a3c141a1cb0ad0757', '5f3f959c7b3e6f091898caa8e828f110'] env = Environment(api, np.random.choice(houses, 1)[0], cfg) task = RoomNavTask(env, hardness=0.6, discrete_action=True) succ = deque(maxlen=500) for i in range(EPISODE): step, total_rew, good = 0, 0, 0 task.reset() while True:
def main(): parser = argparse.ArgumentParser() parser.add_argument('--data_file', default='../path_data/cleaned_human_demo_data.json', type=str) parser.add_argument('--demo_id', default=0, type=int) parser.add_argument('--width', type=int, default=600) parser.add_argument('--height', type=int, default=450) parser.add_argument('--filter_height', type=bool, default=True) args = parser.parse_args() with open(args.data_file, 'r') as f: data = json.load(f) demo = data[args.demo_id] print('Total time steps:', len(demo['loc'])) locs = np.array(demo['loc'][1:]) ques = demo['question'] answer = demo['answer'] text = 'Q: {0:s} A: {1:s}'.format(ques, answer) text_height = 60 cfg = load_config('config.json') api = objrender.RenderAPI(w=args.width, h=args.height, device=0) env = Environment(api, demo['house_id'], cfg) L_min = env.house.L_min_coor L_max = env.house.L_max_coor L_min = np.array([[env.house.L_lo[0], L_min[1], env.house.L_lo[1]]]) L_max = np.array([[env.house.L_hi[0], L_max[1], env.house.L_hi[1]]]) grid_size = env.house.grid_det n_row = env.house.n_row grid_num = np.array([ n_row[0] + 1, int((L_max[0][1] - L_min[0][1]) / (grid_size + 1e-8)) + 1, n_row[1] + 1 ]) print('Grid size:', grid_size) print('Number of grid in [x, y, z]:', grid_num) all_grids = np.zeros(tuple(grid_num), dtype=bool) grid_colors = np.zeros(tuple(grid_num) + (3, ), dtype=np.uint8) loc_map = env.gen_locmap() obs_map = env.house.obsMap.T obs_pos = obs_map == 1 for t in tqdm(range(len(locs))): env.reset(x=locs[t][0], y=locs[t][2], yaw=locs[t][3]) depth = env.render(RenderMode.DEPTH) rgb = env.render(RenderMode.RGB) semantic = env.render(RenderMode.SEMANTIC) infmask = depth[:, :, 1] depth = depth[:, :, 0] * (infmask == 0) true_depth = depth.astype(np.float32) / 255.0 * 20.0 extrinsics = env.cam.getExtrinsicsNumpy() points, points_colors = gen_point_cloud(true_depth, rgb, extrinsics) grid_locs = np.floor((points - L_min) / grid_size).astype(int) all_grids[grid_locs[:, 0], grid_locs[:, 1], grid_locs[:, 2]] = True grid_colors[grid_locs[:, 0], grid_locs[:, 1], grid_locs[:, 2]] = points_colors depth = np.stack([depth] * 3, axis=2) loc_map[grid_locs[:, 2], grid_locs[:, 0], :] = np.array([250, 120, 120]) loc_map[obs_pos] = 0 rad = env.house.robotRad / env.house.grid_det x, y = env.cam.pos.x, env.cam.pos.z x, y = env.house.to_grid(x, y) loc_map_cp = loc_map.copy() cv2.circle(loc_map_cp, (x, y), int(rad), (0, 0, 255), thickness=-1) loc_map_resized = cv2.resize(loc_map_cp, env.resolution) concat1 = np.concatenate((rgb, semantic), axis=1) concat2 = np.concatenate((depth, loc_map_resized), axis=1) ret = np.concatenate((concat1, concat2), axis=0) ret = ret[:, :, ::-1] pad_text = np.ones((text_height, ret.shape[1], ret.shape[2]), dtype=np.uint8) mat_w_text = np.concatenate((np.copy(ret), pad_text), axis=0) cv2.putText(mat_w_text, text, (10, mat_w_text.shape[0] - 10), FONT, 1, (255, 255, 255), 2, cv2.LINE_AA) cv2.imshow("human_demo", mat_w_text) cv2.waitKey(20) valid_grids = np.argwhere(all_grids) valid_grid_center = valid_grids * grid_size + L_min valid_grid_color = grid_colors[valid_grids[:, 0], valid_grids[:, 1], valid_grids[:, 2]] if args.filter_height: height_lim = [-0.1, 2.3] valid_idx = np.logical_and(valid_grid_center[:, 1] >= height_lim[0], valid_grid_center[:, 1] <= height_lim[1]) valid_grid_center = valid_grid_center[valid_idx] valid_grid_color = valid_grid_color[valid_idx] valid_grids = valid_grids[valid_idx] loc_map = env.gen_locmap() obs_map = env.house.obsMap.T loc_map[valid_grids[:, 2], valid_grids[:, 0], :] = np.array([250, 120, 120]) loc_map[obs_map == 1] = 0 loc_map = cv2.resize(loc_map, env.resolution) cv2.imwrite('seen_map.png', loc_map) if USE_OPEN3D: pcd = PointCloud() pcd.points = Vector3dVector(valid_grid_center) pcd.colors = Vector3dVector(valid_grid_color / 255.0) coord = create_mesh_coordinate_frame(3, [40, 0, 35]) draw_geometries([pcd, coord])