def create_env(house="02f594bb5d8c5871bde0d8c8db20125b"): api = objrender.RenderAPI(w=600, h=450, device=0) cfg = create_default_config('/home/rythei/guide/SUNCG/house') if house is None: env = Environment(api, TEST_HOUSE, cfg) else: env = Environment(api, house, cfg) env.reset() return env
def _initialize(self): h, w = self.screen_size api = objrender.RenderAPI(w=w, h=h, device=0) env = Environment(api, self.scene, self.configuration) env.reset() env = RoomNavTask(env, discrete_action=True, depth_signal=False, segment_input=False, reward_type=None, hardness=self.hardness) self._env = env
def GymHouseEnv2(scene='05cac5f7fdd5f8138234164e76a97383', screen_size=(84, 84), goals=['living_room'], configuration=None): h, w = screen_size api = objrender.RenderAPI(w=w, h=h, device=0) env = Environment(api, scene, create_configuration(configuration)) env.reset() env = RoomNavTask2(env, discrete_action=True, depth_signal=False, segment_input=False, reward_type=None) env.observation_space.dtype = np.uint8 return GymHouseWrapper(env, room_types=goals, screen_size=screen_size)
def worker(idx, house_id, device): colormapFile = "../metadata/colormap_coarse.csv" api = objrender.RenderAPI(w=args.width, h=args.height, device=device) env = Environment(api, house_id, cfg) N = 15000 start = time.time() cnt = 0 env.reset() for t in range(N): cnt += 1 env.move_forward(random.random() * 3, random.random() * 3) mat = env.render() if (cnt % 50 == 0): env.reset() end = time.time() print("Worker {}, speed {:.3f} fps".format(idx, N / (end - start)))
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)
# Copyright 2017-present, Facebook, Inc. # All rights reserved. # # This source code is licensed under the license found in the # LICENSE file in the root directory of this source tree. import cv2 from House3D import objrender, Environment, load_config if __name__ == '__main__': api = objrender.RenderAPI(w=600, h=450, device=0) cfg = load_config('config.json') env = Environment(api, '00065ecbdd7300d35ef4328ffe871505', cfg) env.reset() # put the agent into the house # fourcc = cv2.VideoWriter_fourcc(*'X264') # writer = cv2.VideoWriter('out.avi', fourcc, 30, (1200, 900)) while True: mat = env.debug_render() # writer.write(mat) cv2.imshow("aaa", mat) key = cv2.waitKey(0) if not env.keyboard_control(key): break # writer.release()
# Copyright 2017-present, Facebook, Inc. # All rights reserved. # # This source code is licensed under the license found in the # LICENSE file in the root directory of this source tree. import tqdm import cv2 from House3D import objrender, Environment, load_config if __name__ == '__main__': api = objrender.RenderAPI(w=600, h=450, device=0) cfg = load_config('config.json') env = Environment(api, '00065ecbdd7300d35ef4328ffe871505', cfg) # fourcc = cv2.VideoWriter_fourcc(*'X264') # writer = cv2.VideoWriter('out.avi', fourcc, 30, (1200, 900)) for t in tqdm.trange(1000): if t % 1000 == 0: env.reset() mat = env.debug_render() # writer.write(mat) cv2.imshow("aaa", mat) key = cv2.waitKey(0) if not env.keyboard_control(key): break # writer.release()
class House3DRGBD: 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 seed(self, seed=None): self.np_random, seed = seeding.np_random(seed) return [seed] def reset(self, house_id=None, x=None, y=None, yaw=None): if house_id is None: house_id = self.np_random.choice(self.houses_id, 1)[0] self.hid = house_id if self.env is not None: del self.api del self.env self.api = objrender.RenderAPI(self.render_width, self.render_height, device=RENDERING_GPU) self.env = Environment(self.api, house_id, self.config, GridDet=self.configs['GridDet'], RenderDoor=self.render_door, StartIndoor=self.start_indoor) if not self.train_mode: self.loc_map = self.env.gen_locmap() obs_map = self.env.house.obsMap.T self.obs_pos = obs_map == 1 self.traj = [] self.traj_actions = [] self.grid_traj = [] self.L_min = self.env.house.L_lo self.L_max = self.env.house.L_hi self.grid_size = self.env.house.grid_det grid_num = np.array( [self.env.house.n_row[0] + 1, self.env.house.n_row[1] + 1]) self.grids_mat = np.zeros(tuple(grid_num), dtype=np.uint8) self.max_grid_size = np.max(grid_num) self.max_seen_area = float(np.prod(grid_num)) self.env.reset(x=x, y=y, yaw=yaw) self.start_pos, self.grid_start_pos = self.get_camera_grid_pos() if not self.train_mode: print('start pose: ', self.start_pos) self.traj.append(self.start_pos.tolist()) self.grid_traj.append(self.grid_start_pos.tolist()) rgb, depth, extrinsics = self.get_obs() large_loc_map, small_loc_map = self.get_loc_map() self.seen_area = self.get_seen_area(rgb, depth, extrinsics, self.grids_mat) self.ep_len = 0 self.ep_reward = 0 self.collision_times = 0 ob = (self.resize_img(rgb), large_loc_map, small_loc_map) return ob def step(self, action): if self.ignore_collision: collision_flag = self.motion_primitive_no_check(action) else: collision_flag = self.motion_primitive(action) rgb, depth, extrinsics = self.get_obs() large_loc_map, small_loc_map = self.get_loc_map() if not self.train_mode: current_pos, grid_current_pos = self.get_camera_grid_pos() self.traj_actions.append(int(action)) self.traj.append(current_pos.tolist()) self.grid_traj.append(grid_current_pos.tolist()) reward, seen_area, raw_reward = self.cal_reward( rgb, depth, extrinsics, collision_flag) self.ep_len += 1 self.ep_reward += reward if collision_flag: self.collision_times += 1 info = { 'reward_so_far': self.ep_reward, 'steps_so_far': self.ep_len, 'seen_area': seen_area, 'collisions': self.collision_times, 'start_pose': self.start_pos, 'house_id': self.hid, 'collision_flag': collision_flag } info = {**info, **raw_reward} done = False ob = (self.resize_img(rgb), large_loc_map, small_loc_map) return ob, reward, done, info def render(self): loc_map, small_map = self.get_loc_map() if not self.train_mode: rad = self.env.house.robotRad / self.env.house.grid_det x = int(loc_map.shape[0] / 2) y = int(loc_map.shape[1] / 2) cv2.circle(loc_map, (x, y), 1, (255, 0, 255), thickness=-1) x = int(small_map.shape[0] / 2) y = int(small_map.shape[1] / 2) cv2.circle(small_map, (x, y), int(rad) * 2, (255, 0, 255), thickness=-1) loc_map = cv2.resize(loc_map, (self.render_width, self.render_height), interpolation=cv2.INTER_CUBIC) if not self.train_mode: x = int(loc_map.shape[0] / 2) y = int(loc_map.shape[1] / 2) cv2.circle(loc_map, (x, y), 2, (255, 0, 255), thickness=-1) self.env.set_render_mode('rgb') rgb = self.env.render() img = np.concatenate((rgb, loc_map), axis=1) img = img[:, :, ::-1] img = cv2.resize(img, (img.shape[1] * 3, img.shape[0] * 3), interpolation=cv2.INTER_CUBIC) cv2.imshow("nav", img) cv2.waitKey(40) def resize_img(self, img): return cv2.resize(img, (self.img_width, self.img_height), interpolation=cv2.INTER_AREA) def motion_primitive(self, action): # 0: Forward # 1: Turn Left # 2: Turn Right # 3: Strafe Left # 4: Strafe Right # 5: Backward collision_flag = False if action == 0: if not self.train_mode: print('Action: Forward') if not self.env.move_forward(self.move_sensitivity): if not self.train_mode: print('Cannot move forward, collision!!!') collision_flag = True elif action == 1: if not self.train_mode: print('Action: Turn Left') self.env.rotate(-self.rot_sensitivity) elif action == 2: if not self.train_mode: print('Action: Turn Right') self.env.rotate(self.rot_sensitivity) elif action == 3: if not self.train_mode: print('Action: Strafe Left') if not self.env.move_forward(dist_fwd=0, dist_hor=-self.move_sensitivity): if not self.train_mode: print('Cannot strafe left, collision!!!') collision_flag = True elif action == 4: if not self.train_mode: print('Action: Strafe Right') if not self.env.move_forward(dist_fwd=0, dist_hor=self.move_sensitivity): if not self.train_mode: print('Cannot strafe right, collision!!!') collision_flag = True elif action == 5: if not self.train_mode: print('Action: Backward') if not self.env.move_forward(-self.move_sensitivity): if not self.train_mode: print('Cannot move backward, collision!!!') collision_flag = True else: raise ValueError('unknown action type: [{0:d}]'.format(action)) return collision_flag def move_forward(self, dist_fwd, dist_hor=0): """ Move with `fwd` distance to the front and `hor` distance to the right. Both distance are float numbers. Ignore collision !!! """ pos = self.env.cam.pos pos = pos + self.env.cam.front * dist_fwd pos = pos + self.env.cam.right * dist_hor self.env.cam.pos.x = pos.x self.env.cam.pos.z = pos.z def motion_primitive_no_check(self, action): # motion primitive without collision checking # 0: Forward # 1: Turn Left # 2: Turn Right # 3: Strafe Left # 4: Strafe Right # 5: Backward collision_flag = False if action == 0: self.move_forward(self.move_sensitivity) elif action == 1: self.env.rotate(-self.rot_sensitivity) elif action == 2: self.env.rotate(self.rot_sensitivity) elif action == 3: self.move_forward(dist_fwd=0, dist_hor=-self.move_sensitivity) elif action == 4: self.move_forward(dist_fwd=0, dist_hor=self.move_sensitivity) elif action == 5: self.move_forward(-self.move_sensitivity) else: raise ValueError('unknown action type: [{0:d}]'.format(action)) return collision_flag def get_obs(self): self.env.set_render_mode('rgb') rgb = self.env.render() self.env.set_render_mode('depth') depth = self.env.render() infmask = depth[:, :, 1] depth = depth[:, :, 0] * (infmask == 0) true_depth = depth.astype(np.float32) / 255.0 * 20.0 extrinsics = self.env.cam.getExtrinsicsNumpy() return rgb, true_depth, extrinsics def get_seen_area(self, rgb, depth, extrinsics, out_mat, inv_E=True): points, points_colors = gen_point_cloud( depth, rgb, extrinsics, depth_threshold=self.depth_threshold, inv_E=inv_E) grid_locs = np.floor( (points[:, [0, 2]] - self.L_min) / self.grid_size).astype(int) grids_mat = np.zeros( (self.grids_mat.shape[0], self.grids_mat.shape[1]), dtype=np.uint8) high_filter_idx = points[:, 1] < HEIGHT_THRESHOLD[1] low_filter_idx = points[:, 1] > HEIGHT_THRESHOLD[0] obstacle_idx = np.logical_and(high_filter_idx, low_filter_idx) self.safe_assign(grids_mat, grid_locs[high_filter_idx, 0], grid_locs[high_filter_idx, 1], 2) kernel = np.ones((3, 3), np.uint8) grids_mat = cv2.morphologyEx(grids_mat, cv2.MORPH_CLOSE, kernel) obs_mat = np.zeros((self.grids_mat.shape[0], self.grids_mat.shape[1]), dtype=np.uint8) self.safe_assign(obs_mat, grid_locs[obstacle_idx, 0], grid_locs[obstacle_idx, 1], 1) kernel = np.ones((self.ob_dilation_kernel, self.ob_dilation_kernel), np.uint8) obs_mat = cv2.morphologyEx(obs_mat, cv2.MORPH_CLOSE, kernel) obs_idx = np.where(obs_mat == 1) self.safe_assign(grids_mat, obs_idx[0], obs_idx[1], 1) out_mat[np.where(grids_mat == 2)] = 2 out_mat[np.where(grids_mat == 1)] = 1 seen_area = np.sum(out_mat > 0) return seen_area def cal_reward(self, rgb, depth, extrinsics, collision_flag): filled_grid_num = self.get_seen_area(rgb, depth, extrinsics, self.grids_mat, inv_E=True) area_reward = (filled_grid_num - self.seen_area) reward = area_reward * self.area_reward_scale if collision_flag: reward -= self.collision_penalty reward -= self.step_penalty self.seen_area = filled_grid_num raw_reward = {'area': area_reward, 'collision_flag': collision_flag} return reward, filled_grid_num, raw_reward def get_loc_map(self): top_down_map = self.grids_mat.T.copy() half_size = max(top_down_map.shape[0], top_down_map.shape[1], self.configs['large_map_range']) * 3 ego_map = np.ones( (half_size * 2, half_size * 2, 3), dtype=np.uint8) * 255 loc_map = np.zeros((top_down_map.shape[0], top_down_map.shape[1], 3), dtype=np.uint8) loc_map[top_down_map == 0] = np.array([255, 255, 255]) loc_map[top_down_map == 1] = np.array([0, 0, 255]) loc_map[top_down_map == 2] = np.array([0, 255, 0]) current_pos, grid_current_pos = self.get_camera_grid_pos() x_start = half_size - grid_current_pos[1] y_start = half_size - grid_current_pos[0] x_end = x_start + top_down_map.shape[0] y_end = y_start + top_down_map.shape[1] assert x_start >= 0 and y_start >= 0 and \ x_end <= ego_map.shape[0] and y_end <= ego_map.shape[1] ego_map[x_start:x_end, y_start:y_end] = loc_map center = (half_size, half_size) rot_angle = self.constrain_to_pm_pi(90 + current_pos[2]) M = cv2.getRotationMatrix2D(center, rot_angle, 1.0) ego_map = cv2.warpAffine(ego_map, M, (ego_map.shape[1], ego_map.shape[0]), flags=cv2.INTER_AREA, borderMode=cv2.BORDER_CONSTANT, borderValue=(255, 255, 255)) start = half_size - self.configs['small_map_range'] end = half_size + self.configs['small_map_range'] small_ego_map = ego_map[start:end, start:end] start = half_size - self.configs['large_map_range'] end = half_size + self.configs['large_map_range'] assert start >= 0 assert end <= ego_map.shape[0] large_ego_map = ego_map[start:end, start:end] return cv2.resize( large_ego_map, (self.configs['large_map_size'], self.configs['large_map_size']), interpolation=cv2.INTER_AREA), small_ego_map def safe_assign(self, im_map, x_idx, y_idx, value): try: im_map[x_idx, y_idx] = value except IndexError: valid_idx1 = np.logical_and(x_idx >= 0, x_idx < im_map.shape[0]) valid_idx2 = np.logical_and(y_idx >= 0, y_idx < im_map.shape[1]) valid_idx = np.logical_and(valid_idx1, valid_idx2) im_map[x_idx[valid_idx], y_idx[valid_idx]] = value def constrain_to_pm_pi(self, theta): # make sure theta is within [-180, 180) return (theta + 180) % 360 - 180 def get_camera_grid_pos(self): current_pos = np.array([ self.env.cam.pos.x, self.env.cam.pos.z, self.constrain_to_pm_pi(self.env.cam.yaw) ]) grid_pos = np.array( self.env.house.to_grid(current_pos[0], current_pos[1])) return current_pos, grid_pos def truncated_norm(self, mu, sigma, lower_limit, upper_limit, size): if sigma == 0: return mu lower_limit = (lower_limit - mu) / sigma upper_limit = (upper_limit - mu) / sigma r = truncnorm(lower_limit, upper_limit, loc=mu, scale=sigma) return r.rvs(size)
samples_per_room = 20 screen_size = (512, 512) cfg = create_configuration(deep_rl.configuration.get('house3d').as_dict()) room_target_object = load_target_object_data(cfg['roomTargetFile']) api = objrender.RenderAPI(w=screen_size[1], h=screen_size[0], device=0) for i, houseID in enumerate(houses): print('Processing house %s (%s/%s)' % (houseID, i + 1, len(houses))) house = create_house(houseID, cfg) env = Environment(api, house, cfg) types_to_rooms = get_valid_room_dict(house) for room_type, rooms in types_to_rooms.items(): print('Processing house %s (%s/%s) - %s' % (houseID, i + 1, len(houses), room_type)) locations = get_valid_locations(house, rooms) if len(locations) == 0: print('ERROR: no locations for room %s' % room_type) continue for j in range(samples_per_room): #print_progress(0, samples_per_room) location = sample_true_object(room_target_object, env, house, locations, room_type) if location is not None: env.reset(*location[1]) render_current_location(env, houseID, room_type, j, cfg) #print_progress(samples_per_room, samples_per_room)
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 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])
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 L_det = env.house.L_det n_row = env.house.n_row robotRad = env.house.robotRad savedata = {
class House3DEnv(gym.Env): 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 seed(self, seed=None): self.np_random, seed = seeding.np_random() return [seed] def constrain_to_pm_pi(self, theta): return (theta + 180) % 360 - 180 def get_camera_grid_pos(self): current_pos = np.array([ self.env.cam.pos.x, self.env.cam.pos.z, self.constrain_to_pm_pi(self.env.cam.yaw) ]) grid_pos = np.array( self.env.house.to_grid(current_pos[0], current_pos[1])) return current_pos, grid_pos def get_obs(self): self.env.set_render_mode('rgb') rgb = self.env.render() self.env.set_render_mode('depth') depth = self.env.render() infmask = depth[:, :, 1] depth = depth[:, :, 0] * (infmask == 0) true_depth = depth.astype(np.float32) / 255.0 * 20.0 extrinsics = self.env.cam.getExtrinsicsNumpy() return rgb, np.expand_dims(depth, -1), true_depth, extrinsics def safe_assign(self, im_map, x_idx, y_idx, value): try: im_map[x_idx, y_idx] = value except IndexError: valid_idx1 = np.logical_and(x_idx >= 0, x_idx < im_map.shape[0]) valid_idx2 = np.logical_and(y_idx >= 0, y_idx < im_map.shape[1]) valid_idx = np.logical_and(valid_idx1, valid_idx2) im_map[x_idx[valid_idx], y_idx[valid_idx]] = value def get_seen_area(self, rgb, depth, extrinsics, out_mat, inv_E=True): points, points_colors = gen_point_cloud( depth, rgb, extrinsics, depth_threshold=self.depth_threshold, inv_E=inv_E) grid_locs = np.floor( (points[:, [0, 2]] - self.L_min) / self.grid_size).astype(int) grids_mat = np.zeros( (self.grids_mat.shape[0], self.grids_mat.shape[1]), dtype=np.uint8) high_filter_idx = points[:, 1] < HEIGHT_THRESHOLD[1] low_filter_idx = points[:, 1] > HEIGHT_THRESHOLD[0] obstacle_idx = np.logical_and(high_filter_idx, low_filter_idx) self.safe_assign(grids_mat, grid_locs[high_filter_idx, 0], grid_locs[high_filter_idx, 1], 2) kernel = np.ones((3, 3), np.uint8) grids_mat = cv2.morphologyEx(grids_mat, cv2.MORPH_CLOSE, kernel) obs_mat = np.zeros((self.grids_mat.shape[0], self.grids_mat.shape[1]), dtype=np.uint8) self.safe_assign(obs_mat, grid_locs[obstacle_idx, 0], grid_locs[obstacle_idx, 1], 1) kernel = np.ones((self.ob_dilation_kernel, self.ob_dilation_kernel), np.uint8) obs_mat = cv2.morphologyEx(obs_mat, cv2.MORPH_CLOSE, kernel) obs_idx = np.where(obs_mat == 1) self.safe_assign(grids_mat, obs_idx[0], obs_idx[1], 1) out_mat[np.where(grids_mat == 2)] = 2 out_mat[np.where(grids_mat == 1)] = 1 seen_area = np.sum(out_mat > 0) #cal_seen_area = np.sum(out_mat == 1) return seen_area def cal_reward(self, rgb, depth, extrinsics, collision_flag): if collision_flag: reward = -1.0 * self.collision_penalty area_reward = 0.0 filled_grid_num = self.seen_area else: filled_grid_num = self.get_seen_area(rgb, depth, extrinsics, self.grids_mat, inv_E=True) area_reward = (filled_grid_num - self.seen_area) reward = area_reward * self.area_reward_scale reward -= self.step_penalty self.seen_area = filled_grid_num raw_reward = {'area': area_reward, 'collision_flag': collision_flag} return reward, filled_grid_num, raw_reward def reset(self, house_id=None, x=None, y=None, yaw=None): if not self.train_mode: obs_map = self.env.house.obsMap.T self.obs_pos = obs_map == 1 self.traj = [] self.traj_actions = [] self.grid_traj = [] if house_id is None: house_id = self.np_random.choice(self.houses_id, 1)[0] self.hid = house_id if self.env is not None: del self.api del self.env self.api = objrender.RenderAPI(self.render_width, self.render_height, device=RENDERING_GPU) self.env = Environment(self.api, house_id, self.config, GridDet=self.configs['GridDet'], RenderDoor=self.render_door, StartIndoor=self.start_indoor) self.tracker = [] self.num_rotate = 0 self.right_rotate = 0 self.L_min = self.env.house.L_lo self.L_max = self.env.house.L_hi self.grid_size = self.env.house.grid_det grid_num = np.array( [self.env.house.n_row[0] + 1, self.env.house.n_row[1] + 1]) self.grids_mat = np.zeros(tuple(grid_num), dtype=np.uint8) self.max_grid_size = np.max(grid_num) self.max_seen_area = float(np.prod(grid_num)) self.env.reset(x=x, y=y, yaw=yaw) self.start_pos, self.grid_start_pos = self.get_camera_grid_pos() if not self.train_mode: self.traj.append(self.start_pos.tolist()) self.grid_traj.append(self.grid_start_pos.tolist()) rgb, depth, true_depth, extrinsics = self.get_obs() self.seen_area = self.get_seen_area(rgb, true_depth, extrinsics, self.grids_mat) self.ep_len = 0 self.ep_reward = 0 self.collision_times = 0 ret_obs = np.concatenate((rgb, depth), axis=-1) return ret_obs def motion_primitive(self, action): collision_flag = False det_fwd = np.clip(action[0] + 0.8, 0.0, 2.0) / 2.0 tmp_alpha = 0.0 if action[2] > 0: tmp_alpha = 1.0 elif action[2] < 0: tmp_alpha = -1.0 det_rot = np.clip(action[1] + 0.8, 0.0, 1.6) * 0.5 * tmp_alpha move_fwd = det_fwd * self.move_sensitivity rotation = det_rot * self.rot_sensitivity if not self.env.move_forward(move_fwd, 0.0): collision_flag = True else: self.env.rotate(rotation) return collision_flag def step(self, action): collision_flag = self.motion_primitive(action) rgb, depth, true_depth, extrinsics = self.get_obs() current_pos, grid_current_pos = self.get_camera_grid_pos() if not self.train_mode: self.traj_actions.append(int(action)) self.traj.append(current_pos.tolist()) self.grid_traj.append(grid_current_pos.tolist()) self.tracker.append([ grid_current_pos[0], grid_current_pos[1], self.env.cam.front.x, self.env.cam.front.z, self.env.cam.right.x, self.env.cam.right.z ]) if action[1] > -0.8 and action[2] > 0: self.right_rotate += 1 if action[1] > -0.8 and action[2] != 0: self.num_rotate += 1 reward, seen_area, raw_reward = self.cal_reward( rgb, true_depth, extrinsics, collision_flag) self.ep_len += 1 self.ep_reward += reward if collision_flag: self.collision_times += 1 info = { 'reward_so_far': self.ep_reward, 'steps_so_far': self.ep_len, 'seen_area': seen_area, 'collisions': self.collision_times, 'start_pose': self.start_pos, 'house_id': self.hid, 'collision_flag': collision_flag, 'grid_current_pos': grid_current_pos - self.grid_start_pos, 'current_rot': current_pos[2] - self.start_pos[2] } info = {**info, **raw_reward, **self.info} if self.ep_len >= self.max_step: done = True info.update({'bad_transition': True}) else: done = False ret_obs = np.concatenate((rgb, depth), axis=-1) return ret_obs, reward, done, info @property def house(self): return self.env.house @property def info(self): ret = self.env.info ret['track'] = self.tracker ret['total_rotate'] = self.num_rotate ret['right_rotate'] = self.right_rotate if not self.train_mode: ret['traj_actions'] = self.traj_actions ret['traj'] = self.traj ret['grid_traj'] = self.grid_traj return ret