def example_get_topdown_map(): config = habitat.get_config(config_file="tasks/pointnav.yaml") dataset = habitat.make_dataset( id_dataset=config.DATASET.TYPE, config=config.DATASET ) env = habitat.Env(config=config, dataset=dataset) env.reset() top_down_map = maps.get_topdown_map(env.sim, map_resolution=(5000, 5000)) recolor_map = np.array( [[255, 255, 255], [128, 128, 128], [0, 0, 0]], dtype=np.uint8 ) range_x = np.where(np.any(top_down_map, axis=1))[0] range_y = np.where(np.any(top_down_map, axis=0))[0] padding = int(np.ceil(top_down_map.shape[0] / 125)) range_x = ( max(range_x[0] - padding, 0), min(range_x[-1] + padding + 1, top_down_map.shape[0]), ) range_y = ( max(range_y[0] - padding, 0), min(range_y[-1] + padding + 1, top_down_map.shape[1]), ) top_down_map = top_down_map[ range_x[0] : range_x[1], range_y[0] : range_y[1] ] top_down_map = recolor_map[top_down_map] imageio.imsave(os.path.join(IMAGE_DIR, "top_down_map.png"), top_down_map)
def generate_map_frame(env, obs_dim): """Generates a map that displays the state of the agent in the given environment, for the current frame. Args: env: sim environment. obs_dim: dimension of observations. Returns: the height x width x 1 map """ # draw map top_down_map = maps.get_topdown_map(sim=env.sim, map_resolution=(1250, 1250, 1), num_samples=200) # draw agent # TODO: figure out how to convert quaternion to angles to get agent_rotation top_down_map = maps.draw_agent( image=top_down_map, agent_center_coord=(1, 1), agent_rotation=0.0, agent_radius_px=8, ) # scale top down map to align with rgb view old_h, old_w, _ = top_down_map.shape top_down_height = obs_dim[0] top_down_width = int(float(top_down_height) / old_h * old_w) # cv2 resize (dsize is width first) top_down_map = cv2.resize( top_down_map, (top_down_width, top_down_height), interpolation=cv2.INTER_CUBIC, ) top_down_map = np.expand_dims(top_down_map, axis=2) return top_down_map
def get_topdown_map(self, map_resolution=(1250, 1250), num_samples=20000, draw_border=True): assert np.abs(self.start_height - self.position[1]) <= 1.5 house_aabb = self.get_house_dimensions() min_x = house_aabb.center[0] - house_aabb.sizes[0] / 2 max_x = house_aabb.center[0] + house_aabb.sizes[0] / 2 min_z = house_aabb.center[2] - house_aabb.sizes[2] / 2 max_z = house_aabb.center[2] + house_aabb.sizes[2] / 2 min_coord = min(min_x, min_z) max_coord = max(max_x, max_z) maps.COORDINATE_MIN = min_coord maps.COORDINATE_MAX = max_coord map = maps.get_topdown_map(self.sim, map_resolution=map_resolution, num_samples=num_samples, draw_border=draw_border) return map
def get_topdown_map(config_paths, map_name): config = habitat.get_config(config_paths=config_paths) dataset = habitat.make_dataset(id_dataset=config.DATASET.TYPE, config=config.DATASET) env = habitat.Env(config=config, dataset=dataset) env.reset() square_map_resolution = 5000 top_down_map = maps.get_topdown_map(env.sim, map_resolution=(square_map_resolution, square_map_resolution)) # Image containing 0 if occupied, 1 if unoccupied, and 2 if border (if # the flag is set) top_down_map[np.where(top_down_map == 0)] = 125 top_down_map[np.where(top_down_map == 1)] = 255 top_down_map[np.where(top_down_map == 2)] = 0 imageio.imsave(os.path.join(MAP_DIR, map_name + ".pgm"), top_down_map) complete_name = os.path.join(MAP_DIR, map_name + ".yaml") f = open(complete_name, "w+") f.write("image: " + map_name + ".pgm\n") f.write("resolution: " + str((COORDINATE_MAX - COORDINATE_MIN) / square_map_resolution) + "\n") f.write("origin: [" + str(-1) + "," + str(-1) + ", 0.000000]\n") f.write("negate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.196") f.close()
def get_original_map(self): top_down_map = maps.get_topdown_map( self._sim, self._map_resolution, self._num_samples, False, ) return top_down_map
def get_map(env, map_res): # top down map top_down_map = maps.get_topdown_map(env.sim, num_samples=1000000, map_resolution=(map_res, map_res)) top_down_map, rmin, rmax, cmin, cmax = crop_map(top_down_map) # target/goal target_position = env.current_episode.goals[0].position target_position_grid = maps.to_grid(target_position[0], target_position[2], maps.COORDINATE_MIN, maps.COORDINATE_MAX, (map_res, map_res)) # agent's current position # to_grid converts real world (x,y) to pixel (x,y) agent_position = env.sim.get_agent_state().position agent_position_grid = maps.to_grid(agent_position[0], agent_position[2], maps.COORDINATE_MIN, maps.COORDINATE_MAX, (map_res, map_res)) # grid offset for map agent_position_grid = (agent_position_grid[0] - rmin, agent_position_grid[1] - cmin) target_position_grid = (target_position_grid[0] - rmin, target_position_grid[1] - cmin) tdm = top_down_map.copy() # agent pos m1 = 4 tdm[agent_position_grid[0] - m1:agent_position_grid[0] + m1, agent_position_grid[1] - m1:agent_position_grid[1] + m1] = 0 # target pos m2 = 7 tdm[target_position_grid[0] - m1:target_position_grid[0] + m1, target_position_grid[1] - m2:target_position_grid[1] + m2] = 0 tdm[target_position_grid[0] - m2:target_position_grid[0] + m2, target_position_grid[1] - m1:target_position_grid[1] + m1] = 0 assert agent_position_grid[0] > 0 and agent_position_grid[ 0] < top_down_map.shape[0], print('assert1 ', agent_position_grid, top_down_map.shape) assert target_position_grid[0] > 0 and target_position_grid[ 0] < top_down_map.shape[0], print('assert2 ', target_position_grid, top_down_map.shape) assert agent_position_grid[1] > 0 and agent_position_grid[ 1] < top_down_map.shape[1], print('assert3 ', agent_position_grid, top_down_map.shape) assert target_position_grid[1] > 0 and target_position_grid[ 1] < top_down_map.shape[1], print('assert4 ', target_position_grid, top_down_map.shape) return tdm, agent_position_grid, target_position_grid
def topdown_map(env, recolor=False): top_down_map = maps.get_topdown_map(env.sim, map_resolution=(5000, 5000)) padding = int(np.ceil(top_down_map.shape[0] / 125)) top_down_map = crop(top_down_map, padding) # background is 0 # border is 1, # interior is 2 # recolor if recolor: map_colors = np.array([[255, 255, 255], [128, 128, 128], [0, 0, 0]], dtype=np.uint8) top_down_map = map_colors[top_down_map] return top_down_map
def get_original_map(self): top_down_map = maps.get_topdown_map( self._sim, self._map_resolution, self._num_samples, self._config.DRAW_BORDER, ) range_x = np.where(np.any(top_down_map, axis=1))[0] range_y = np.where(np.any(top_down_map, axis=0))[0] self._ind_x_min = range_x[0] self._ind_x_max = range_x[-1] self._ind_y_min = range_y[0] self._ind_y_max = range_y[-1] return top_down_map
def get_original_map(self, episode): top_down_map = maps.get_topdown_map( self._sim, self._map_resolution, self._num_samples, self._config.DRAW_BORDER, ) range_x = np.where(np.any(top_down_map, axis=1))[0] range_y = np.where(np.any(top_down_map, axis=0))[0] self._ind_x_min = range_x[0] self._ind_x_max = range_x[-1] self._ind_y_min = range_y[0] self._ind_y_max = range_y[-1] if self._config.DRAW_SOURCE_AND_TARGET: # mark source point s_x, s_y = maps.to_grid( episode.start_position[0], episode.start_position[2], self._coordinate_min, self._coordinate_max, self._map_resolution, ) point_padding = 2 * int( np.ceil(self._map_resolution[0] / MAP_THICKNESS_SCALAR)) top_down_map[s_x - point_padding:s_x + point_padding + 1, s_y - point_padding:s_y + point_padding + 1, ] = maps.MAP_SOURCE_POINT_INDICATOR # mark target point t_x, t_y = maps.to_grid( episode.goals[0].position[0], episode.goals[0].position[2], self._coordinate_min, self._coordinate_max, self._map_resolution, ) top_down_map[t_x - point_padding:t_x + point_padding + 1, t_y - point_padding:t_y + point_padding + 1, ] = maps.MAP_TARGET_POINT_INDICATOR return top_down_map
def get_original_map(self): top_down_map = maps.get_topdown_map( self._sim, self._map_resolution, self._num_samples, self._config.DRAW_BORDER, ) range_x = np.where(np.any(top_down_map, axis=1))[0] range_y = np.where(np.any(top_down_map, axis=0))[0] self._ind_x_min = range_x[0] self._ind_x_max = range_x[-1] self._ind_y_min = range_y[0] self._ind_y_max = range_y[-1] if self._config.FOG_OF_WAR.DRAW: self._fog_of_war_mask = np.zeros_like(top_down_map) return top_down_map
def _get_navigable_coord(self): # get the navigable coord coordinate_max = maps.COORDINATE_MAX coordinate_min = maps.COORDINATE_MIN # to low the memory requirement self.top_down_grid_size = self._rl_config.ANS.MAPPER.map_scale*2.5 resolution = (coordinate_max - coordinate_min) / self.top_down_grid_size grid_resolution = (int(resolution), int(resolution)) top_down_map = maps.get_topdown_map( self.habitat_env.sim, grid_resolution, 20000, draw_border=False, ) map_w, map_h = top_down_map.shape intervals = (max(int(1 / self.top_down_grid_size), 1), max(int(0.5 / self.top_down_grid_size), 1)) x_vals = np.arange(0, map_w, intervals[0], dtype=int) y_vals = np.arange(0, map_h, intervals[1], dtype=int) coors = np.stack(np.meshgrid(x_vals, y_vals), axis=2) # (H, W, 2) coors = coors.reshape(-1, 2) # (H*W, 2) map_vals = top_down_map[coors[:, 0], coors[:, 1]] self.valid_coors = coors[map_vals > 0] * self.top_down_grid_size
def act(self, observations): # print (observations.keys()) # import ipdb; ipdb.set_trace() goal_pos = self.env.current_episode.goals[0].position best_action = self.follower.get_next_action(goal_pos) if self.episode_i == 0: cv2.imwrite( './temp/ep%d-step%d.png' % (self.episode_i, self.step_i), observations['rgb']) if self.step_i == 0: top_down_map = maps.get_topdown_map(self.env.sim, map_resolution=(5000, 5000)) import matplotlib.pyplot as plt plt.imshow(top_down_map) plt.show() plt.waitforbuttonpress(0.001) # global_map = observations['top_down_map'] # cv2.imwrite('./temp/map-%d.png'%self.episode_i, global_map) self.step_i += 1 return {"action": best_action} # 0: stop, forward, left, right
print("The NavMesh bounds are: " + str(sim.pathfinder.get_bounds())) if not custom_height: # get bounding box minumum elevation for automatic height height = sim.pathfinder.get_bounds()[0][1] if not sim.pathfinder.is_loaded: print("Pathfinder not initialized, aborting.") else: # @markdown You can get the topdown map directly from the Habitat-sim API with *PathFinder.get_topdown_view*. # This map is a 2D boolean array sim_topdown_map = sim.pathfinder.get_topdown_view(meters_per_pixel, height) if display: # @markdown Alternatively, you can process the map using the Habitat-Lab [maps module](https://github.com/facebookresearch/habitat-api/blob/master/habitat/utils/visualizations/maps.py) hablab_topdown_map = maps.get_topdown_map( sim.pathfinder, height, meters_per_pixel=meters_per_pixel ) recolor_map = np.array( [[255, 255, 255], [128, 128, 128], [0, 0, 0]], dtype=np.uint8 ) hablab_topdown_map = recolor_map[hablab_topdown_map] print("Displaying the raw map from get_topdown_view:") display_map(sim_topdown_map) print("Displaying the map from the Habitat-Lab maps module:") display_map(hablab_topdown_map) # easily save a map to file: map_filename = os.path.join(output_path, "top_down_map.png") imageio.imsave(map_filename, hablab_topdown_map)
panorama=config.PANORAMA, num_floors=house.num_floors) env.reset(floor) locs = house.object_locations_for_habitat_dest goals = [ gibson_info.relevant_locations(env.agent_state()[0], locs[k]) for k in sorted(locs.keys()) ] ims = [] corrs = [] positions = [] top_down_map = maps.get_topdown_map(env.env.sim, map_resolution=(map_resolution, map_resolution), draw_border=False) points = np.argwhere(top_down_map == 1) hposes = poses[(house_name, floor)] if rerender: pos,rot = hposes[ind] env.set_agent_state(pos, rot) im, cor = vis_panorama(env, 12, model, goals) im.savefig(f'vis/pano/final/{num}.pdf', bbox_inches='tight', pad_inches=0.05) num += 1 env.close() continue for pos, rot in tqdm(hposes): env.set_agent_state(pos, rot)
def get_eval_map(env:ExpRLEnv, trainer:SemAntExpTrainer, M:int,depth_projection_net:DepthProjectionNet, step:int, objectName:str): """Given the environment and the configuration, compute the global top-down wall and seen area maps by sampling maps for individual locations along a uniform grid in the environment, and registering them. step (m): the length between two measure points """ # Initialize a global map for the episode scene_name = env.habitat_env.current_episode.scene_id.split('/')[-1].split('.')[0] dirPath = './data/debug/data/' + objectName + "/" + scene_name + '_' + env.habitat_env.current_episode.episode_id + '/' + "record/" safe_mkdir(dirPath) mapper = trainer.mapper config = trainer.config.TASK_CONFIG device = trainer.device scene = env.habitat_env.sim.semantic_scene obj_pos = {} obj_pos = {obj.id : [obj.aabb.center,obj.aabb.sizes] for obj in scene.objects if objectName == obj.category.name()} with open(dirPath + 'obj_pos.pkl', 'wb') as f: pickle.dump(obj_pos, f) objectIndexes = [int(key.split('_')[-1]) for key in obj_pos.keys()] global_wall_map = torch.zeros(1, 2, M, M).to(device) global_seen_map = torch.zeros(1, 2, M, M).to(device) global_object_map = torch.zeros(1, 2, M, M).to(device) global_se_map = torch.zeros(1, 2, M, M).to(device) global_se_filtered_map = torch.zeros(1, 2, M, M).to(device) global_se_seen_map = torch.zeros(1, 2, M, M).to(device) global_se_map_mit = torch.zeros(1, 2, M, M).to(device) global_se_map_gt = torch.zeros(1, 2, M, M).to(device) grid_size = config.TASK.GT_EGO_MAP.MAP_SCALE coordinate_max = maps.COORDINATE_MAX coordinate_min = maps.COORDINATE_MIN resolution = (coordinate_max - coordinate_min) / grid_size grid_resolution = (int(resolution), int(resolution)) top_down_map = maps.get_topdown_map( env.habitat_env.sim, grid_resolution, 20000, draw_border=False, ) map_w, map_h = top_down_map.shape intervals = (max(int(step / grid_size), 1), max(int(step / grid_size), 1)) x_vals = np.arange(0, map_w, intervals[0], dtype=int) y_vals = np.arange(0, map_h, intervals[1], dtype=int) coors = np.stack(np.meshgrid(x_vals, y_vals), axis=2) # (H, W, 2) coors = coors.reshape(-1, 2) # (H*W, 2) map_vals = top_down_map[coors[:, 0], coors[:, 1]] valid_coors = coors[map_vals > 0] real_x_vals = coordinate_max - valid_coors[:, 0] * grid_size real_z_vals = coordinate_min + valid_coors[:, 1] * grid_size start_y = env.habitat_env.sim.get_agent_state().position[1] index = 0 records = [] totalOffsetX = np.random.uniform(-1.5, 1.5) totalOffsetY = np.random.uniform(-1.5, 1.5) for j in tqdm.tqdm(range(real_x_vals.shape[0]),desc='occupacy map', position=2): for theta in np.arange(-np.pi, np.pi, np.pi ): index +=1 randomAngle = np.random.uniform(-np.pi,np.pi) randomX = np.random.uniform(-0.5, 0.5) randomY = np.random.uniform(-0.5, 0.5) position = [ real_x_vals[j].item() + randomX + totalOffsetX, start_y.item(), real_z_vals[j].item() + randomY + totalOffsetY, ] rotation = [ 0.0, np.sin((theta + randomAngle) / 2).item(), 0.0, np.cos((theta + randomAngle) / 2).item(), ] sim_obs = env.habitat_env.sim.get_observations_at( position, rotation, keep_agent_at_new_pose=True, ) episode = env.habitat_env.current_episode obs = obs, _, _, _ = env.step(action={'action':1}) batch = batch_obs([obs], device=trainer.device) semantic = batch["semantic"] mask = torch.zeros_like(semantic,dtype=bool) for objectIndex in objectIndexes: mask ^= (semantic == objectIndex) mask = rearrange(mask, "b h w -> b h w ()") batch["object_mask_gt"] = mask ego_map_gt_b = depth_projection_net( rearrange(batch["depth"], "b h w c -> b c h w") ) object_depth = batch["depth"]*(mask > 0) + (mask == 0)*100 ego_map_gt_object = depth_projection_net(rearrange(object_depth, "b h w c -> b c h w")) batch["ego_map_gt_object"] = ego_map_gt_object #begin use MIT Seg img_data = pil_to_tensor(rearrange(batch["rgb"], "b h w c -> b c h w")[0] / 255) singleton_batch = {'img_data': img_data[None]} output_size = img_data.shape[1:] with torch.no_grad(): scores = segmentation_module(singleton_batch, segSize=output_size) _, pred = torch.max(scores, dim=1) # visualize_result(pred) #end use MIT Seg batch["mit"] = pred #mask for chair if objectName == "chair": mask = (pred == 19)^(pred == 30)^(pred == 75) elif objectName == "bed": mask = (pred == 7) elif objectName == "table": mask = (pred == 15) else: mask = pred > 0 mask = rearrange(mask, "b h w -> b h w ()") batch["object_mask_mit"] = mask object_depth2 = batch["depth"]*(mask > 0) + (mask == 0)*100 ego_map_gt_object2 = depth_projection_net(rearrange(object_depth2, "b h w c -> b c h w")) batch["ego_map_mit_object"] = ego_map_gt_object2 ego_map_gt_anti = transpose_image(batch["ego_map_gt_anticipated"]).to(trainer.device) pu_inputs_t = { "rgb": process_image(batch["rgb"], trainer.mapper.img_mean_t, trainer.mapper.img_std_t).to(trainer.device), "depth": transpose_image(batch["depth"]).to(trainer.device), "ego_map_gt": transpose_image( rearrange(ego_map_gt_b, "b c h w -> b h w c") ).to(trainer.device), "ego_map_gt_anticipated": ego_map_gt_anti, } pu_inputs = trainer.mapper._safe_cat(pu_inputs_t, pu_inputs_t) torch.save(batch, dirPath + 'pu_inputs_t_'+str(index)+'.pt') pu_output = trainer.mapper.projection_unit(pu_inputs) se_map = asnumpy(pu_output['sem_estimate'])[0,:,:,:] ego_map_gt_b_np = asnumpy(ego_map_gt_b[0,...]) se_seen = ego_map_gt_b_np*se_map dilation_mask = np.ones((100, 100)) current_mask = cv2.dilate( se_seen.astype(np.float32), dilation_mask, iterations=1, ).astype(np.float32) se_filtered_map = se_map se_seen_map = torch.Tensor(se_seen).to(device) se_seen_map = rearrange(se_seen_map, "c h w -> () c h w") se_seen_map = (se_seen_map[:,0])[None,:] se_map = torch.Tensor(se_map).to(device) se_map = rearrange(se_map, "c h w -> () c h w") se_filtered_map = torch.Tensor(se_filtered_map).to(device) se_filtered_map = rearrange(se_filtered_map, "c h w -> () c h w") torch.save(se_filtered_map, dirPath + 'se_filtered_map_'+str(index)+'.pt') ego_map_gt = torch.Tensor(obs["ego_map_gt"]).to(device) ego_map_gt = rearrange(ego_map_gt, "h w c -> () c h w") ego_wall_map_gt = torch.Tensor(obs["ego_wall_map_gt"]).to(device) ego_wall_map_gt = rearrange(ego_wall_map_gt, "h w c -> () c h w") pose_gt = torch.Tensor(obs["pose_gt"]).unsqueeze(0).to(device) global_se_seen_map = mapper.ext_register_map( global_se_seen_map, se_seen_map, pose_gt ) global_se_map_gt = mapper.ext_register_map( global_se_map_gt, ego_map_gt_anti, pose_gt ) global_seen_map = mapper.ext_register_map( global_seen_map, ego_map_gt_b, pose_gt ) global_object_map = mapper.ext_register_map( global_object_map, ego_map_gt_object, pose_gt ) global_wall_map = mapper.ext_register_map( global_wall_map, ego_wall_map_gt, pose_gt ) global_se_map = mapper.ext_register_map( global_se_map, se_map, pose_gt ) global_se_filtered_map = mapper.ext_register_map( global_se_filtered_map, se_filtered_map, pose_gt ) global_se_map_mit = mapper.ext_register_map( global_se_map_mit, ego_map_gt_object2, pose_gt ) se_filtered_map = None batch = None se_map = None ego_map_gt = None ego_wall_map_gt = None obs = None pu_inputs_t = None pu_output=None _ = None pu_inputs=None gc.collect() torch.cuda.empty_cache() mask = dilate_tensor(global_seen_map*(1 - global_wall_map > 0)*global_se_map,(51, 51)) global_se_filtered_map = global_se_filtered_map*mask global_se_seen_map = global_se_seen_map*mask mask = dilate_tensor(global_seen_map*(1 - global_wall_map > 0)*global_object_map,(51, 51)) global_se_map_gt = global_se_map_gt*mask global_wall_map_np = asnumpy(rearrange(global_wall_map, "b c h w -> b h w c")[0]) global_seen_map_np = asnumpy(rearrange(global_seen_map, "b c h w -> b h w c")[0]) global_se_map_np = asnumpy(rearrange(global_se_map, "b c h w -> b h w c")[0]) global_se_global_se_filtered_map_np= asnumpy(rearrange(global_se_filtered_map, "b c h w -> b h w c")[0]) global_se_map_mit_np= asnumpy(rearrange(global_se_map_mit, "b c h w -> b h w c")[0]) global_se_map_gt_np= asnumpy(rearrange(global_se_map_gt, "b c h w -> b h w c")[0]) global_se_seen_map_np = asnumpy(rearrange(global_se_seen_map, "b c h w -> b h w c")[0]) global_object_map_np = asnumpy(rearrange(global_object_map, "b c h w -> b h w c")[0]) return global_seen_map_np, global_wall_map_np, global_se_map_np,global_se_global_se_filtered_map_np, global_se_map_mit_np,global_se_map_gt_np,global_se_seen_map_np,global_object_map_np
def get_map_with_path(env, map_res, agent_path): # top down map top_down_map = maps.get_topdown_map(env.sim, num_samples=1000000, map_resolution=(map_res, map_res)) top_down_map, rmin, rmax, cmin, cmax = crop_map(top_down_map) tdm = top_down_map.copy() # target/goal target_position = env.current_episode.goals[0].position target_position_grid = maps.to_grid(target_position[0], target_position[2], maps.COORDINATE_MIN, maps.COORDINATE_MAX, (map_res, map_res)) for k in range(len(agent_path)): pos = agent_path[k] # agent's position # to_grid converts real world (x,y) to pixel (x,y) agent_position = pos #env.sim.get_agent_state().position agent_position_grid = maps.to_grid(agent_position[0], agent_position[2], maps.COORDINATE_MIN, maps.COORDINATE_MAX, (map_res, map_res)) # grid offset for map agent_position_grid = (agent_position_grid[0] - rmin, agent_position_grid[1] - cmin) # agent pos m1 = 1 color = [255, 0, 0] if (k == 0): m1 = 3 color = [0, 255, 0] tdm[agent_position_grid[0] - m1:agent_position_grid[0] + m1, agent_position_grid[1] - m1:agent_position_grid[1] + m1, :] = color assert agent_position_grid[0] > 0 and agent_position_grid[ 0] < top_down_map.shape[0], print('assert1 ', agent_position_grid, top_down_map.shape) assert agent_position_grid[1] > 0 and agent_position_grid[ 1] < top_down_map.shape[1], print('assert3 ', agent_position_grid, top_down_map.shape) target_position_grid = (target_position_grid[0] - rmin, target_position_grid[1] - cmin) # target pos m2 = 3 color = [0, 0, 255] tdm[target_position_grid[0] - m2:target_position_grid[0] + m2, target_position_grid[1] - m2:target_position_grid[1] + m2, :] = color #tdm[target_position_grid[0]-m1:target_position_grid[0]+m1,target_position_grid[1]-m2:target_position_grid[1]+m2,:] = color #tdm[target_position_grid[0]-m2:target_position_grid[0]+m2,target_position_grid[1]-m1:target_position_grid[1]+m1,:] = color assert target_position_grid[0] > 0 and target_position_grid[ 0] < top_down_map.shape[0], print('assert2 ', target_position_grid, top_down_map.shape) assert target_position_grid[1] > 0 and target_position_grid[ 1] < top_down_map.shape[1], print('assert4 ', target_position_grid, top_down_map.shape) return tdm
def main(): parser = argparse.ArgumentParser() parser.add_argument("--task-config", type=str, default="configs/tasks/pointnav.yaml") parser.add_argument("--publish-odom", type=bool, default=True) parser.add_argument("--create-map", type=bool, default=False) parser.add_argument("--save-observations", type=bool, default=False) parser.add_argument("--preset-trajectory", type=bool, default=False) args = parser.parse_args() # Now define the config for the sensor config = habitat.get_config(args.task_config) config.defrost() config.TASK.AGENT_POSITION_SENSOR = habitat.Config() # Use the custom name config.TASK.AGENT_POSITION_SENSOR.TYPE = "position_sensor" config.TASK.AGENT_POSITION_SENSOR.ANSWER_TO_LIFE = 42 # Add the sensor to the list of sensors in use config.TASK.SENSORS.append("AGENT_POSITION_SENSOR") config.freeze() max_depth = config.SIMULATOR.DEPTH_SENSOR.MAX_DEPTH print(args.create_map) agent = KeyboardAgent(args.save_observations) env = habitat.Env(config=config) if args.create_map: print('create map') top_down_map = maps.get_topdown_map(env.sim, map_resolution=(5000, 5000)) print(top_down_map.min(), top_down_map.mean(), top_down_map.max()) recolor_map = np.array([[0, 0, 0], [128, 128, 128], [255, 255, 255]], dtype=np.uint8) range_x = np.where(np.any(top_down_map, axis=1))[0] range_y = np.where(np.any(top_down_map, axis=0))[0] padding = int(np.ceil(top_down_map.shape[0] / 125)) range_x = ( max(range_x[0] - padding, 0), min(range_x[-1] + padding + 1, top_down_map.shape[0]), ) range_y = ( max(range_y[0] - padding, 0), min(range_y[-1] + padding + 1, top_down_map.shape[1]), ) top_down_map = top_down_map[ range_x[0] : range_x[1], range_y[0] : range_y[1] ] top_down_map = recolor_map[top_down_map] imsave('top_down_map.png', top_down_map) observations = env.reset() if not args.preset_trajectory: while not keyboard.is_pressed('q'): action = agent.act(observations) observations = env.step(action) else: fin = open('actions.txt', 'r') actions = [int(x) for x in fin.readlines()] for action in actions: agent.act(observations) observations = env.step(action) fin.close() if args.save_observations: with h5py.File('observations.hdf5', 'w') as f: f.create_dataset("points", data=np.array(agent.points)) f.create_dataset("true_positions", data=np.array(agent.true_positions)) f.create_dataset("true_rotations", data=np.array(agent.true_rotations)) f.create_dataset("predicted_positions", data=np.array(agent.predicted_positions)) f.create_dataset("predicted_rotations", data=np.array(agent.predicted_rotations)) f.create_dataset("rgb", data=np.array(agent.rgbs)) f.create_dataset("depth", data=np.array(agent.depths)) f.create_dataset("true_timestamps", data=np.array(agent.true_timestamps)) f.create_dataset("predicted_timestamps", data=np.array(agent.predicted_timestamps)) print(np.array(agent.points).shape, np.array(agent.rgbs).shape) fout = open('actions.txt', 'w') for action in agent.actions: print(action, file=fout) fout.close()
def train_rl(): print('create env') env = habitat.Env(config) # save a copy of the map print('map res: ', map_res) top_down_map = maps.get_topdown_map(env.sim, num_samples=1000000, map_resolution=(map_res, map_res)) top_down_map, rmin, rmax, cmin, cmax = crop_map(top_down_map) np.save('TDM_maps/tdm', top_down_map) print('create tf session') sess = tf.Session() Policy = Policy_net('policy', sess, env, S_DIM, A_DIM) Old_Policy = Policy_net('old_policy', sess, env, S_DIM, A_DIM) PPO = PPOTrain(Policy, Old_Policy, sess, gamma=gamma) ppo_vars = Policy.get_trainable_variables() + Old_Policy.get_trainable_variables() saver_ppo = tf.train.Saver(ppo_vars) rec_loss_rgb = 'l2' lat_loss_rgb = 'kld' beta_rgb = 1.0 rec_loss_depth = 'l2' lat_loss_depth = 'kld' beta_depth = 1.0 vae_rgb = VAE(sess, lr=learning_rate, batch_size=batch_size, n_z=n_z, img_size=img_size, nchannels=3, recon_loss=rec_loss_rgb, lat_loss=lat_loss_rgb, scope='vae_rgb') vae_rgb_vars = vae_rgb.get_vae_vars() vae_depth = VAE(sess, lr=learning_rate, batch_size=batch_size, n_z=n_z, img_size=img_size, nchannels=1, recon_loss=rec_loss_depth, lat_loss=lat_loss_depth, scope='vae_depth') vae_depth_vars = vae_depth.get_vae_vars() saver_vae_rgb = tf.train.Saver(vae_rgb_vars) saver_vae_depth = tf.train.Saver(vae_depth_vars) sess.run(tf.global_variables_initializer()) if (load_ckpt): saver_ppo.restore(sess, 'ckpt/ppo/model') saver_vae_rgb.restore(sess, 'ckpt/vae_rgb/model') saver_vae_depth.restore(sess, 'ckpt/vae_depth/model') print("total number of trainable params: ", Policy.num_train_params()) for ep in range(ep_start, nepisodes_max): #if (ep < 175000): epsilon_ppo = EPSILON_CLIP stochastic = True c_2 = c_2_ENTROPY #else: # epsilon_ppo = EPSILON_CLIP * (1.0 - float(ep-175000)/float(100000)) # stochastic = False # c_2 = c_2_ENTROPY * (1.0 - float(ep-175000)/float(100000)) epsilon_ppo = max(epsilon_ppo, 0.02) c_2 = max(c_2, 0.001) print('epsilon: ', epsilon_ppo, ' | c_2: ', c_2) s = env.reset() observations, actions, rewards, v_preds = [], [], [], [] init_agent_position = env.sim.get_agent_state().position dist = get_dist(env) init_geodesic_dist = dist rnn_state = Policy.state_init done = False success = False nsteps = 0 ep_rewards = 0.0 dist_traveled_by_agent = 0.0 old_position = init_agent_position ncollisions = 0 agent_path = [] while (not done) and (not env.episode_over) and (not success): agent_path.append(env.sim.get_agent_state().position) rgb = s["rgb"] depth = s["depth"] pointgoal = s["pointgoal"]; pointgoal = [pointgoal[0], pointgoal[2]] heading = s["heading"] # perception_embedding is the costly operation in the entire one time step (~ 3-4X slower than env.step/Policy.act) obs, z_rgb_t, z_depth_t = perception_embedding(vae_rgb, vae_depth, rgb, depth, pointgoal, heading, S_DIM, n_z, beta_rgb, beta_depth) act, v_pred, rnn_state = Policy.act(obs=obs, rnn_state=rnn_state, stochastic=stochastic) act = np.asscalar(act) v_pred = np.asscalar(v_pred) s2 = env.step(allowed_actions[act]) nsteps += 1 new_dist = get_dist(env) reward, done, success = get_reward(dist, new_dist, nsteps, env) #----------------------------- # collision penalty if (act == 0): if (was_there_a_collision(env, old_position, config, act)): ncollisions += 1 # reward -= 0.025 # 0.1 #----------------------------- new_position = env.sim.get_agent_state().position if (new_dist <= 0.2): s2 = env.step(SimulatorActions.STOP) new_dist = get_dist(env) a_t = np.zeros((1,1),dtype=np.int32); a_t[0,0] = act pg_t = np.zeros((1,2),dtype=np.float32); pg_t[0,:] = pointgoal h_t = np.zeros((1,1),dtype=np.float32); h_t[0,0] = heading dist_traveled_by_agent += abs(new_dist - dist) #----------------------------- # SPL reward for success (SPL bonus reward) #if (success): # spl = float(success)*init_geodesic_dist/max(dist_traveled_by_agent, init_geodesic_dist) # reward += spl * 10.0 #----------------------------- ep_rewards += reward observations.append(obs) actions.append(act) rewards.append(reward) v_preds.append(v_pred) if done or success: rgb = s2["rgb"] depth = s2["depth"] pointgoal = s2["pointgoal"]; pointgoal = [pointgoal[0], pointgoal[2]] heading = s2["heading"] next_obs, _, _ = perception_embedding(vae_rgb, vae_depth, rgb, depth, pointgoal, heading, S_DIM, n_z, beta_rgb, beta_depth) _, v_last, rnn_state = Policy.act(obs=next_obs, rnn_state=rnn_state, stochastic=stochastic) v_preds_next = v_preds[1:] + [v_last] break else: s = s2 dist = new_dist old_position = new_position if (ep % 50 == 0): saver_ppo.save(sess, 'ckpt/ppo/model') gaes = PPO.get_gaes(rewards=rewards, v_preds=v_preds, v_preds_next=v_preds_next) observations = np.array(observations).astype(dtype=np.float32) observations = np.squeeze(observations, 1) actions = np.array(actions).astype(dtype=np.int32) rewards = np.array(rewards).astype(dtype=np.float32) v_preds_next = np.array(v_preds_next).astype(dtype=np.float32) gaes = np.array(gaes).astype(dtype=np.float32) gaes = (gaes - gaes.mean()) / gaes.std() gaes = np.squeeze(gaes,2) gaes = np.squeeze(gaes,1) PPO.assign_policy_parameters() inp = [observations, actions, rewards, v_preds_next, gaes] # train sample_indices = np.arange(observations.shape[0]) sampled_inp = [np.take(a=a, indices=sample_indices, axis=0) for a in inp] for epoch in range(5): PPO.train(obs=sampled_inp[0], actions=sampled_inp[1], rewards=sampled_inp[2], v_preds_next=sampled_inp[3], gaes=sampled_inp[4], epsilon_ppo=epsilon_ppo, c_2=c_2) final_agent_position = env.sim.get_agent_state().position target_position = env.current_episode.goals[0].position geo_dist_traveled_by_agent = env.sim.geodesic_distance(init_agent_position, final_agent_position) SPL = float(success)*init_geodesic_dist/max(dist_traveled_by_agent, init_geodesic_dist) final_dist_to_target = env.sim.geodesic_distance(final_agent_position, target_position) # final position of agent agent_path.append(env.sim.get_agent_state().position) # TDM if (ep % 10 == 0): tdm_with_path = get_map_with_path(env, map_res, agent_path) plt.imshow(tdm_with_path); plt.tight_layout(); plt.axis('off') fname = 'TDM_maps/tdm_' + str(ep) + '_' + str(int(SPL*100.0)) + '.png'; plt.savefig(fname) print(' ') print('episode: ', ep, ' | ep_rewards: ', ep_rewards, ' | ep_steps: ', nsteps, ' | SPL: ', SPL) print('init geodesic dist to target: ', init_geodesic_dist) print('final agent pos: ', final_agent_position) print('target pos: ', target_position) print(' ') print('total dist traveled by agent: ', dist_traveled_by_agent) print('geo dist traveled by agent: ', geo_dist_traveled_by_agent) print('final dist to target: ', final_dist_to_target) print('ncollisions: ', ncollisions) print('-'*50) f = open("performance/performance_ppo.txt", "a+") f.write(str(ep) + " " + str(ep_rewards) + " " + str(nsteps) + " " + str(SPL) + " " + " " + str(init_geodesic_dist) + " " + str(final_dist_to_target) + '\n') f.close()
def get_episode_map(env:habitat.RLEnv, mapper:Mapper, M:int, config:CN, device:torch.device): """Given the environment and the configuration, compute the global top-down wall and seen area maps by sampling maps for individual locations along a uniform grid in the environment, and registering them. """ # Initialize a global map for the episode global_wall_map = torch.zeros(1, 2, M, M).to(device) global_seen_map = torch.zeros(1, 2, M, M).to(device) grid_size = config.TASK.GT_EGO_MAP.MAP_SCALE coordinate_max = maps.COORDINATE_MAX coordinate_min = maps.COORDINATE_MIN resolution = (coordinate_max - coordinate_min) / grid_size grid_resolution = (int(resolution), int(resolution)) top_down_map = maps.get_topdown_map( env.habitat_env.sim, grid_resolution, 20000, draw_border=False, ) map_w, map_h = top_down_map.shape intervals = (max(int(0.5 / grid_size), 1), max(int(0.5 / grid_size), 1)) x_vals = np.arange(0, map_w, intervals[0], dtype=int) y_vals = np.arange(0, map_h, intervals[1], dtype=int) coors = np.stack(np.meshgrid(x_vals, y_vals), axis=2) # (H, W, 2) coors = coors.reshape(-1, 2) # (H*W, 2) map_vals = top_down_map[coors[:, 0], coors[:, 1]] valid_coors = coors[map_vals > 0] real_x_vals = coordinate_max - valid_coors[:, 0] * grid_size real_z_vals = coordinate_min + valid_coors[:, 1] * grid_size start_y = env.habitat_env.sim.get_agent_state().position[1] for j in tqdm.tqdm(range(real_x_vals.shape[0]),desc='occupacy map', position=2): for theta in np.arange(-np.pi, np.pi, np.pi / 3.0): position = [ real_x_vals[j].item(), start_y.item(), real_z_vals[j].item(), ] rotation = [ 0.0, np.sin(theta / 2).item(), 0.0, np.cos(theta / 2).item(), ] sim_obs = env.habitat_env.sim.get_observations_at( position, rotation, keep_agent_at_new_pose=True, ) episode = env.habitat_env.current_episode obs = env.habitat_env.task.sensor_suite.get_observations( observations=sim_obs, episode=episode, task=env.habitat_env.task ) ego_map_gt = torch.Tensor(obs["ego_map_gt"]).to(device) ego_map_gt = rearrange(ego_map_gt, "h w c -> () c h w") ego_wall_map_gt = torch.Tensor(obs["ego_wall_map_gt"]).to(device) ego_wall_map_gt = rearrange(ego_wall_map_gt, "h w c -> () c h w") pose_gt = torch.Tensor(obs["pose_gt"]).unsqueeze(0).to(device) global_seen_map = mapper.ext_register_map( global_seen_map, ego_map_gt, pose_gt ) global_wall_map = mapper.ext_register_map( global_wall_map, ego_wall_map_gt, pose_gt ) global_wall_map_np = asnumpy(rearrange(global_wall_map, "b c h w -> b h w c")[0]) global_seen_map_np = asnumpy(rearrange(global_seen_map, "b c h w -> b h w c")[0]) return global_seen_map_np, global_wall_map_np
"scene": test_scene, # Scene path "default_agent": 0, "sensor_height": 1.5, # Height of sensors in meters "color_sensor": rgb_sensor, # RGB sensor "depth_sensor": depth_sensor, # Depth sensor "semantic_sensor": semantic_sensor, # Semantic sensor "seed": 1, # used in the random navigation "enable_physics": False, # kinematics only } cfg = make_cfg(sim_settings) sim = habitat_sim.Simulator(cfg) #height = sim.pathfinder.get_bounds()[0][1] height = 0.1 meters_per_pixel = 0.01 hablab_topdown_map = maps.get_topdown_map( sim.pathfinder, height, meters_per_pixel=meters_per_pixel) first_list = read_file_list(first_file) second_list = read_file_list(second_file) matches = first_list.keys() first_xyz = [[float(value) for value in first_list[a][0:3]] for a in matches] second_xyz = [[float(value) * float(1.0) for value in second_list[b][0:3]] for b in matches] first_rot = [[float(value) for value in first_list[a][3:]] for a in matches] grid_dimensions = (hablab_topdown_map.shape[0], hablab_topdown_map.shape[1]) trajectory = [ maps.to_grid( -(path_point[2]) + 19.25,
def ours_evaluate(config, env, ep, house, epind, model, visualize, model_config): hn, floor, class_label, goal_dist, pos, rot = ep if config.SCORE == 'detector' or config.COMBINE_DETECTOR: predictor = get_predictor() predictor_class_index = predictor.metadata.thing_classes.index( class_label) rng = random.Random() rng.seed(config.SEED) if goal_dist == float('inf'): return np.array([]) if config.STOP else 0 class_index = class_labels.index(class_label) def model_score(ims): torch_ims = util.torch.to_imgnet(torch.tensor(ims).to('cuda')) with torch.no_grad(): return model(torch_ims.unsqueeze(0))[0, class_index, :].max().item() # compute the frame score combined with detector def score(ims): sc = model_score(ims['rgb']) if config.COMBINE_DETECTOR: size = ims['rgb'].shape[1] left_lim, right_lim = int(size / 3), int(size * 2 / 3) im = ims['rgb'][0] if len(ims['rgb']) == 4 else ims['rgb'] boxes, scores = get_scores(predictor, im, predictor_class_index) if len(scores) > 0 and scores.max() > config.CONFIDENCE_THRESHOLD: box = boxes[scores.argmax()] if box[0] <= right_lim or box[2] >= left_lim: if len(ims['rgb']) == 4: ims['rgb'][0] = draw_box(ims['rgb'][0], box, scores.max().item()) else: ims['rgb'] = draw_box(ims['rgb'], box, scores.max().item()) sc += (scores.max().item() + 1) return sc def output(): print(f"SPL: {spl}: {goal_dist}/{dist_traveled}") if config.SLAM and visualize: planner.write_combined( f'%04d_{class_label}-%dm-spl%.2f-steps%d' % (epind, int(goal_dist), spl, agent_steps_taken)) # np.save(f'data_dump/good_trajectory{epind}',np.array(log)) return np.array(log) if config.STOP else spl locs = house.object_locations_for_habitat_dest all_goals = [locs[k] for k in sorted(locs.keys())] from habitat.utils.visualizations import maps top_down_map = maps.get_topdown_map(env.env.sim, map_resolution=(map_resolution, map_resolution)) rrange, crange = util.habitat.crop_range(top_down_map) point_min = util.habitat.from_grid([rrange[0], crange[0]], map_resolution, 0) point_max = util.habitat.from_grid([rrange[1], crange[1]], map_resolution, 0) max_dim = np.abs(point_max - point_min).max() out_dir = f'{config.VIDEO_LOCATION}/{name_from_config(config)}' util.ensure_folders(out_dir, True) planner = DepthMapperAndPlanner(dt=30, out_dir=out_dir, map_size_cm=max_dim * 230, mark_locs=True, close_small_openings=True, log_visualization=visualize) polygons = relevant_objects(env.pos, house.objects[class_label]) planner._reset(goal_dist, global_goals=polygons, start_pos=env.pos, start_ang=env.angle) openlist = [] visited = [] dist_traveled = 0 log = [] spl = 0 agent_steps_taken = 0 full_log = [] episode_ims = [env.get_observation()] no_move = False def semantic_reasoning(): # log for visualizations planner.log_reasoning() images = [] display_values = [] for _ in range(NUM_ROTATIONS): ims, _, _, _ = env.step(1) loc = [*planner.pos_to_loc(env.pos), env.angle] planner.add_observation(ims['depth'] * 1000, loc) dest = check_movement(config, env, env.angle, planner=planner, rng=rng) sc = score(ims) # for visualizations images.append(ims) display_values.append(sc) if dest is not None: openlist.append((sc, dest)) if visualize and config.SLAM: ims_to_render = [ e['rgb'][0] if len(e['rgb'].shape) == 4 else e['rgb'] for e in images ] current_pan = join_images( ims_to_render, -np.array(display_values), bl_text='Predicted Values', br_text=f'Object Class: {class_label.title()}') planner.set_current_pan(current_pan) macro_steps = 50 if config.SLAM else 30 print("goals ", env.goals) # initial steps to scan env and choose dest semantic_reasoning() agent_steps_taken += NUM_ROTATIONS for macro_step_num in range(macro_steps): print(agent_steps_taken) if config.BACKTRACK_REJECTION and len(visited) > 0: vis_stack = np.stack(visited) def reject(point): dists = np.linalg.norm((vis_stack - point)[:, [0, 2]], axis=1) return (dists < (success_distance - 0.1)).sum() > 0 openlist = [e for e in openlist if not reject(e[1])] def maxfunc(x): s, d = x dist = np.linalg.norm(env.pos - d) return s + config.CONSISTENCY_WEIGHT * max(10 - dist, 0) / 10 if len(openlist) == 0: print("openlist exhausted") if visualize: planner.write_combined() return output() ind, (sc, next_pos), _ = util.argmax(openlist, maxfunc) openlist.pop(ind) original_openlist = openlist.copy() # remove points which we cannot move toward, with an exception # for the first step due to the initilization process dist_est = planner.fmmDistance(next_pos) while not planner.action_toward(next_pos): if len(openlist) == 0: print("openlist exhausted fmm") if visualize: planner.write_combined() return output() ind, (sc, next_pos), _ = util.argmax(openlist, maxfunc) openlist.pop(ind) dist_est = planner.fmmDistance(next_pos) print('score of', sc) if visualize and config.SLAM: planner.set_current_open(openlist) obs = env.get_observation() planner.set_goal(next_pos) goal_reached = False # 6 for initial rotation, and 2x distance*4 to # account for 1 rotation per forward step on average step_estimate = math.ceil(2 * (dist_est / 0.25) + 6) cur_dist_est = dist_est for step in range(step_estimate): new_dist_est = planner.fmmDistance(next_pos) # replan if the estimated distance jumps up too much if new_dist_est > cur_dist_est + 0.1: print('replan') break cur_dist_est = new_dist_est action = planner.get_action_toward(next_pos) print('action: ', action) if action == 3: print('subgoal reached') break before_pos = env.pos obs, _, _, _ = env.step(action) if action == 0: dist_traveled += 0.25 planner.pos_to_loc(env.pos) planner.log_act(obs, env.pos, env.angle, action) episode_ims.append(obs) visited.append(env.pos) log.append([ env.pos, env.rot, dist_traveled, env.distance_to_goal(), step == 0 ]) agent_steps_taken += 1 if env._dist_to_goal( env.pos) < success_distance and not config.STOP: spl = min(goal_dist / (dist_traveled + 1e-5), 1) return output() if agent_steps_taken >= MAX_STEPS: return output() semantic_reasoning() agent_steps_taken += NUM_ROTATIONS if agent_steps_taken >= MAX_STEPS: return output() return output()