def reward(self,collision,move_dist):
done = False
depth_start = time.time()
out_pcl_np = depth_fusion(self.log_dir, first_frame_idx =0, base_frame_idx=1000, num_frames = self.count_steps + 1, save_pcd = False, max_depth = 1.0)
# 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()
if cd > 96.0:
done = True
reward = 100
else:
reward = cd_delta
reward += -0.04 * move_dist
self.cd_old = cd
self.total_distance += move_dist
return reward, done
def reward(self, collision, move_dist):
done = False
# depth_start = time.time()
out_pcl_np = depth_fusion(self.log_dir,
first_frame_idx=0,
base_frame_idx=1000,
num_frames=self.count_steps + 1,
save_pcd=False,
max_depth=1.0)
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()
target_coverage = 96.0
print('coverage', cd)
# target_coverage = 97.0
if cd > target_coverage:
done = True
reward = 100
# print('covered', self.count_steps)
else:
reward = cd_delta
reward += -2 + -0.02 * move_dist # added to push minimization of steps
self.cd_old = cd
self.total_distance += move_dist
return reward, done
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 reward(self, collision, move_dist):
done = False
# reward = - 1.0
# if collision:
# done = True
# reward = -1
# print('Collision Detected!!')
# else:
# #hard code to 1
# reward = -1*move_dist*(1/2000)
# print('dist reward: ', reward)
# distance = self.cauculate_distance(self.target_pos, self.unrealcv.get_pose())
# if distance < 50:
# reward = 10
# done = True
# print ('Get Target Place!')
# print("start depth ")
# print("!!!!!!!!!!!!")
# print('numframes:', self.count_steps+1)
depth_start = time.time()
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)
cd = self.compute_chamfer(out_pcl_np)
cd_delta = cd - self.cd_old
# cd_delta = cd_delta
# print('coverage: ', cd)
# print('cov del: ', cd_delta)
depth_end = time.time()
# print("Depth Fusion time: ", depth_end - depth_start)
if cd > 99.0:
done = True
reward = 50
print('covered', self.count_steps)
else:
reward = cd_delta * 0.2
self.cd_old = cd
return reward, done