def _reset(self, start_pose_rel=None):
x, y, z, _, yaw, _ = self.startpose
if self.reset_type == 'random':
distance = 1000
azimuth = 0
elevation = 45
p = 90
distance = distance + random.randint(-250, 250)
azimuth = random.randint(0, 359)
elevation = random.randint(35, 55)
yaw_exp = 270 - azimuth
pitch = -1 * elevation
y = distance * sin(radians(p - elevation)) * cos(radians(azimuth))
x = distance * sin(radians(p - elevation)) * sin(radians(azimuth))
z = distance * cos(radians(p - elevation))
self.unrealcv.set_pose(self.cam_id,
[x, y, z, 0, yaw_exp, pitch
]) # pose = [x, y, z, roll, yaw, pitch]
else:
self.unrealcv.set_pose(
self.cam_id,
self.startpose) # pose = [x, y, z, roll, yaw, pitch]
# state = self.unrealcv.read_image(self.cam_id, 'lit')
state = self.unrealcv.get_observation(self.cam_id,
self.observation_type)
self.count_steps = 0
depth_pt = self.unrealcv.read_depth(self.cam_id, mode='depthFusion')
pose = self.unrealcv.get_pose(self.cam_id, 'soft')
depth = depth_conversion(depth_pt, 320)
pose_filename = self.log_dir + 'frame-{:06}.pose.txt'.format(
self.count_steps)
depth_filename = self.log_dir + 'frame-{:06}.depth.npy'.format(
self.count_steps)
write_pose(pose, pose_filename)
np.save(depth_filename, depth)
out_pcl_np = depth_fusion(self.log_dir,
first_frame_idx=0,
base_frame_idx=1000,
num_frames=self.count_steps + 1,
save_pcd=True,
max_depth=1.0)
# out_fn = 'log/house-' + '{:06}'.format(self.count_steps+1) + '.ply'
# out_pcl = pcl.load(out_fn)
# out_pcl_np = np.asarray(out_pcl)
out_pcl_np = np.expand_dims(out_pcl_np, axis=0)
self.cd_old = self.compute_chamfer(out_pcl_np)
# print('cd old ', self.cd_old)
return state
def _step(self, action=0):
self.count_steps += 1
azimuth, elevation, distance = self.discrete_actions[action]
change_pose = np.array((azimuth, elevation, distance))
pose_prev = np.array(self.pose_prev)
pose_new = pose_prev + change_pose
if pose_new[2] > self.max_distance:
pose_new[2] = self.max_distance
elif pose_new[2] < self.min_distance:
pose_new[2] = self.min_distance
if (pose_new[1] >= self.max_elevation):
pose_new[1] = self.max_elevation
elif (pose_new[1] <= self.min_elevation):
pose_new[1] = self.min_elevation
else:
pose_new[1] = 45.0
if (pose_new[0] < 0):
pose_new[0] = 360 + pose_new[0]
elif (pose_new[0] >= 359):
pose_new[0] = pose_new[0] - 360
print('pose new', pose_new)
collision, move_dist = self.unrealcv.move_rel2(self.cam_id,
pose_new[0],
pose_new[1],
pose_new[2])
self.pose_prev = pose_new
state = self.unrealcv.get_observation(self.cam_id,
self.observation_type)
depth_pt = self.unrealcv.read_depth(self.cam_id, mode='depthFusion')
pose = self.unrealcv.get_pose(self.cam_id, 'soft')
depth = depth_conversion(depth_pt, 320)
pose_filename = self.log_dir + 'frame-{:06}.pose.txt'.format(
self.count_steps)
depth_filename = self.log_dir + 'frame-{:06}.depth.npy'.format(
self.count_steps)
write_pose(pose, pose_filename)
np.save(depth_filename, depth)
reward, done = self.reward(collision, move_dist)
# limit max step per episode
if self.count_steps > self.max_steps:
done = True
# print('Reach Max Steps')
return state, reward, done, {}
def _step(self, action=0):
# (velocity, angle) = self.ACTION_LIST[action]
self.count_steps += 1
# collision = self.unrealcv.move(self.cam_id, angle, velocity)
# collision = self.unrealcv.move_2d(self.cam_id, angle, velocity)
# collision = self.unrealcv.move_2d(self.cam_id, angle, velocity)
azimuth, elevation, distance = action
# azimuth, elevation, distance = self.ACTION_LIST[action]
# azimuth, elevation, distance = self.discrete_actions[action]
# print('action ', action)
# print('pose')
# print(azimuth, elevation, distance )
collision, move_dist = self.unrealcv.move_rel(self.cam_id, azimuth,
elevation, distance)
# print('distance: ', move_dist)
# state = self.unrealcv.read_image(self.cam_id, 'lit')
state = self.unrealcv.get_observation(self.cam_id,
self.observation_type)
# print('state shape', state.shape)
depth_pt = self.unrealcv.read_depth(self.cam_id, mode='depthFusion')
pose = self.unrealcv.get_pose(self.cam_id, 'soft')
depth = depth_conversion(depth_pt, 320)
pose_filename = self.log_dir + 'frame-{:06}.pose.txt'.format(
self.count_steps)
depth_filename = self.log_dir + 'frame-{:06}.depth.npy'.format(
self.count_steps)
write_pose(pose, pose_filename)
np.save(depth_filename, depth)
reward, done = self.reward(collision, move_dist)
# limit max step per episode
if self.count_steps > self.max_steps:
done = True
print('Reach Max Steps')
return state, reward, done, {}
def reward(self, collision, move_dist):
done = False
depth_start = time.time()
out_pcl_np = depth_fusion_mult(self.log_dir,
first_frame_idx=0,
base_frame_idx=1000,
num_frames=self.count_house_frames + 1,
save_pcd=False,
max_depth=1.0,
house_id=self.house_id)
# print('out_pcl_np', out_pcl_np.shape)
if out_pcl_np.shape[0] != 0:
out_pcl_np = np.expand_dims(out_pcl_np, axis=0)
cd = self.compute_chamfer(out_pcl_np)
else:
cd = 0.0
cd_delta = cd - self.cd_old
depth_end = time.time()
# print("Depth Fusion time: ", depth_end - depth_start)
# print('coverage: ', cd)
if cd > 96.0:
self.unrealcv.hide_obj(self.houses[self.house_id])
self.house_id += 1
if (self.house_id == len(self.houses)):
done = True
# reward = 50
reward = 50
else:
# print('covered', self.count_steps)
# print('new house')
self.unrealcv.show_obj(self.houses[self.house_id])
self.count_house_frames = 0
reward = 100
self.unrealcv.set_pose(self.cam_id, self.startpose)
depth_pt = self.unrealcv.read_depth(self.cam_id,
mode='depthFusion')
pose = self.unrealcv.get_pose(self.cam_id, 'soft')
depth = depth_conversion(depth_pt, 320)
# depth_filename = self.log_dir+'frame-{:06}.depth-{:06}.npy'.format(self.count_steps)
# pose_filename = self.log_dir+'frame-{:06}.pose-{:06}.txt'.format(self.count_steps)
pose_filename = self.log_dir + 'frame-{:06}.pose-{:06}.txt'.format(
self.count_house_frames, self.house_id)
depth_filename = self.log_dir + 'frame-{:06}.depth-{:06}.npy'.format(
self.count_house_frames, self.house_id)
write_pose(pose, pose_filename)
np.save(depth_filename, depth)
out_pcl_np = depth_fusion_mult(
self.log_dir,
first_frame_idx=0,
base_frame_idx=1000,
num_frames=self.count_house_frames + 1,
save_pcd=False,
max_depth=1.0,
house_id=self.house_id)
# out_fn = 'log/house-' + '{:06}'.format(self.count_steps+1) + '.ply'
# out_pcl = pcl.load(out_fn)
# out_pcl_np = np.asarray(out_pcl)
out_pcl_np = np.expand_dims(out_pcl_np, axis=0)
self.cd_old = self.compute_chamfer(out_pcl_np)
self.pose_prev = np.array(self.start_pose_rel)
else:
# reward = cd_delta*0.2
reward = cd_delta
# reward = cd_delta*0.4
reward += -2 # added to push minimization of steps
self.cd_old = cd
self.total_distance += move_dist
return reward, done
def _step(self, action=0):
# (velocity, angle) = self.ACTION_LIST[action]
self.count_steps += 1
self.count_house_frames += 1
azimuth, elevation, distance = self.discrete_actions[action]
change_pose = np.array((azimuth, elevation, distance))
pose_prev = np.array(self.pose_prev)
# print('pose prev', pose_prev)
# print('action', change_pose)
MIN_elevation = 20
MAX_elevation = 70
MIN_distance = 100
# MAX_distance = 150
MAX_distance = 125
pose_new = pose_prev + change_pose
# pose_new = pose_prev + np.array([30,0,0]) # to test ICM
if pose_new[2] > MAX_distance:
pose_new[2] = MAX_distance
elif pose_new[2] < MIN_distance:
pose_new[2] = MIN_distance
if (pose_new[1] >= MAX_elevation):
pose_new[1] = MAX_elevation
elif (pose_new[1] <= MIN_elevation):
pose_new[1] = MIN_elevation
else:
pose_new[1] = 45.0
if (pose_new[0] < 0):
pose_new[0] = 360 + pose_new[0]
elif (pose_new[0] >= 359):
pose_new[0] = pose_new[0] - 360
# print('action ', action)
# print('pose new', pose_new)
# print(azimuth, elevation, distance )
# collision, move_dist = self.unrealcv.move_rel2(self.cam_id, azimuth, elevation, distance)
collision, move_dist = self.unrealcv.move_rel2(self.cam_id,
pose_new[0],
pose_new[1],
pose_new[2])
# print('collision', collision)
# print('distance: ', move_dist)
self.pose_prev = pose_new
# state = self.unrealcv.read_image(self.cam_id, 'lit')
state = self.unrealcv.get_observation(self.cam_id,
self.observation_type)
# print('state shape', state.shape)
depth_pt = self.unrealcv.read_depth(self.cam_id, mode='depthFusion')
pose = self.unrealcv.get_pose(self.cam_id, 'soft')
depth = depth_conversion(depth_pt, 320)
# pose_filename = self.log_dir+'frame-{:06}.pose.txt'.format(self.count_steps)
# depth_filename = self.log_dir+'frame-{:06}.depth.npy'.format(self.count_steps)
pose_filename = self.log_dir + 'frame-{:06}.pose-{:06}.txt'.format(
self.count_house_frames, self.house_id)
depth_filename = self.log_dir + 'frame-{:06}.depth-{:06}.npy'.format(
self.count_house_frames, self.house_id)
write_pose(pose, pose_filename)
np.save(depth_filename, depth)
reward, done = self.reward(collision, move_dist)
# limit max step per episode
if self.count_steps > self.max_steps:
done = True
# print('Reach Max Steps')
return state, reward, done, {}