def get_observation(self, *args: Any, observations, episode,
**kwargs: Any):
episode_id = (episode.episode_id, episode.scene_id)
agent_state = self._sim.get_agent_state()
# At the start of a new episode, reset pose estimate
if self.current_episode_id != episode_id:
self.current_episode_id = episode_id
# Initialize with the ground-truth position, rotation
self._estimated_position = agent_state.position
self._estimated_rotation = agent_state.rotation
self._past_gt_position = agent_state.position
self._past_gt_rotation = agent_state.rotation
# Compute noisy delta
delta_xz_noisy, delta_y_noisy = self._get_noisy_delta(agent_state)
# Update past gt states
self._past_gt_position = agent_state.position
self._past_gt_rotation = agent_state.rotation
# Update noisy pose estimates
(
self._estimated_position,
self._estimated_rotation,
) = compute_updated_pose(
self._estimated_position,
self._estimated_rotation,
delta_xz_noisy,
delta_y_noisy,
)
# Compute sensor readings with noisy estimates
origin = np.array(episode.start_position, dtype=np.float32)
rotation_world_start = quaternion_from_coeff(episode.start_rotation)
agent_position = self._estimated_position
agent_position = quaternion_rotate_vector(
rotation_world_start.inverse(), agent_position - origin)
rotation_world_agent = self._estimated_rotation
rotation_world_start = quaternion_from_coeff(episode.start_rotation)
agent_heading = self._quat_to_xy_heading(
rotation_world_agent.inverse() * rotation_world_start)
# This is rotation from -Z to -X. We want -Z to X for this particular sensor.
agent_heading = -agent_heading
return np.array(
[-agent_position[2], agent_position[0], agent_heading],
dtype=np.float32,
)
def get_position_vector(env):
origin = np.array(env.current_episode.start_position, dtype=np.float32)
rotation_world_start = quaternion_from_coeff(
env.current_episode.start_rotation)
agent_position = quaternion_rotate_vector(rotation_world_start, origin)
rotation_world_start = quaternion_from_coeff(
env.current_episode.start_rotation)
agent_heading = quat_to_xy_heading(rotation_world_start)
# This is rotation from -Z to -X. We want -Z to X for this particular sensor.
agent_heading = -agent_heading
return np.array(
[-agent_position[2], agent_position[0], agent_heading],
dtype=np.float32,
)
def get_position_vector_from_obs(observations):
origin = np.array(observations[0]['start_coords'][:3], dtype=np.float32)
rotation_world_start = quaternion_from_coeff(
observations[0]['start_coords'][3:])
agent_position = quaternion_rotate_vector(rotation_world_start, origin)
rotation_world_start = quaternion_from_coeff(
observations[0]['start_coords'][3:])
agent_heading = quat_to_xy_heading(rotation_world_start)
# This is rotation from -Z to -X. We want -Z to X for this particular sensor.
agent_heading = -agent_heading
return np.array(
[-agent_position[2], agent_position[0], agent_heading],
dtype=np.float32,
)
def pos_real_to_map(pos, episode):
agent_position = pos
origin = np.array(episode.start_position, dtype=np.float32)
rotation_world_start = quaternion_from_coeff(episode.start_rotation)
agent_position = quaternion_rotate_vector(
rotation_world_start.inverse(), agent_position - origin
)
rotation_world_start = quaternion_from_coeff(episode.start_rotation)
direction_vector = np.array([0, 0, -1])
heading_vector = quaternion_rotate_vector(rotation_world_start.inverse(), direction_vector)
phi = cartesian_to_polar(-heading_vector[2], heading_vector[0])[1]
return np.array(
[-agent_position[2], agent_position[0], phi], dtype=np.float32,
)
def get_observation(self, *args: Any, observations, episode, **kwargs: Any):
agent_state = self._sim.get_agent_state()
origin = np.array(episode.start_position, dtype=np.float32)
rotation_world_start = quaternion_from_coeff(episode.start_rotation)
agent_position = agent_state.position
agent_position = quaternion_rotate_vector(
rotation_world_start.inverse(), agent_position - origin
)
rotation_world_agent = agent_state.rotation
rotation_world_start = quaternion_from_coeff(episode.start_rotation)
agent_heading = self._quat_to_xy_heading(
rotation_world_agent.inverse() * rotation_world_start
)
# This is rotation from -Z to -X. We want -Z to X for this particular sensor.
agent_heading = -agent_heading
return np.array(
[-agent_position[2], agent_position[0], agent_heading], dtype=np.float32,
)