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])
