def sphere_signed_distances(sphere_positions, sphere_radii, query_positions):
"""
Return the signed distances of a set of query points from the sphere surfaces.\n
`[reference] <https://www.iquilezles.org/www/articles/distfunctions/distfunctions.htm>`_
:param sphere_positions: Positions of the spheres *[batch_shape,num_spheres,3]*
:type sphere_positions: array
:param sphere_radii: Radii of the spheres *[batch_shape,num_spheres,1]*
:type sphere_radii: array
:param query_positions: Points for which to query the signed distances *[batch_shape,num_points,3]*
:type query_positions: array
:return: The distances of the query points from the closest sphere surface *[batch_shape,num_points,1]*
"""
# BS x NS x 1 x 3
sphere_positions = _ivy.expand_dims(sphere_positions, -2)
# BS x 1 x NP x 3
query_positions = _ivy.expand_dims(query_positions, -3)
# BS x NS x NP x 1
distances_to_centre = _ivy.reduce_sum(
(query_positions - sphere_positions)**2, -1, keepdims=True)**0.5
# BS x NS x NP x 1
all_sdfs = distances_to_centre - _ivy.expand_dims(sphere_radii, -2)
# BS x NP x 1
return _ivy.reduce_min(all_sdfs, -3)
def _pairwise_distance(x, y):
# BS x NX x 1 x 1
try:
x = _ivy.expand_dims(x, -2)
except:
d = 0
# BS x 1 x NY x 1
y = _ivy.expand_dims(y, -3)
# BS x NX x NY
return _ivy.reduce_sum((x - y)**2, -1)
def main(interactive=True, try_use_sim=True, f=None):
# config
this_dir = os.path.dirname(os.path.realpath(__file__))
f = choose_random_framework(excluded=['numpy']) if f is None else f
set_framework(f)
sim = Simulator(interactive, try_use_sim)
lr = 0.5
num_anchors = 3
num_sample_points = 100
# spline start
anchor_points = ivy.cast(
ivy.expand_dims(ivy.linspace(0, 1, 2 + num_anchors), -1), 'float32')
query_points = ivy.cast(
ivy.expand_dims(ivy.linspace(0, 1, num_sample_points), -1), 'float32')
# learnable parameters
robot_start_config = ivy.array(ivy.cast(sim.robot_start_config, 'float32'))
robot_target_config = ivy.array(
ivy.cast(sim.robot_target_config, 'float32'))
learnable_anchor_vals = ivy.variable(
ivy.cast(
ivy.transpose(
ivy.linspace(robot_start_config, robot_target_config,
2 + num_anchors)[..., 1:-1], (1, 0)), 'float32'))
# optimizer
optimizer = ivy.SGD(lr=lr)
# optimize
it = 0
colliding = True
clearance = 0
joint_query_vals = None
while colliding:
total_cost, grads, joint_query_vals, link_positions, sdf_vals = ivy.execute_with_gradients(
lambda xs: compute_cost_and_sdfs(xs[
'w'], anchor_points, robot_start_config, robot_target_config,
query_points, sim),
Container({'w': learnable_anchor_vals}))
colliding = ivy.reduce_min(sdf_vals[2:]) < clearance
sim.update_path_visualization(
link_positions, sdf_vals,
os.path.join(this_dir, 'msp_no_sim', 'path_{}.png'.format(it)))
learnable_anchor_vals = optimizer.step(
Container({'w': learnable_anchor_vals}), grads)['w']
it += 1
sim.execute_motion(joint_query_vals)
sim.close()
unset_framework()
def _addressing(self, k, beta, g, s, gamma, prev_M, prev_w):
# Sec 3.3.1 Focusing by Content
# Cosine Similarity
k = ivy.expand_dims(k, axis=2)
inner_product = ivy.matmul(prev_M, k)
k_norm = ivy.reduce_sum(k**2, axis=1, keepdims=True)**0.5
M_norm = ivy.reduce_sum(prev_M**2, axis=2, keepdims=True)**0.5
norm_product = M_norm * k_norm
K = ivy.squeeze(inner_product / (norm_product + 1e-8)) # eq (6)
# Calculating w^c
K_amplified = ivy.exp(ivy.expand_dims(beta, axis=1) * K)
w_c = K_amplified / ivy.reduce_sum(K_amplified, axis=1,
keepdims=True) # eq (5)
if self._addressing_mode == 'content': # Only focus on content
return w_c
# Sec 3.3.2 Focusing by Location
g = ivy.expand_dims(g, axis=1)
w_g = g * w_c + (1 - g) * prev_w # eq (7)
s = ivy.concatenate([
s[:, :self._shift_range + 1],
ivy.zeros(
[s.shape[0], self._memory_size -
(self._shift_range * 2 + 1)]), s[:, -self._shift_range:]
],
axis=1)
t = ivy.concatenate([ivy.flip(s, axis=[1]),
ivy.flip(s, axis=[1])],
axis=1)
s_matrix = ivy.stack([
t[:, self._memory_size - i - 1:self._memory_size * 2 - i - 1]
for i in range(self._memory_size)
],
axis=1)
w_ = ivy.reduce_sum(ivy.expand_dims(w_g, axis=1) * s_matrix,
axis=2) # eq (8)
w_sharpen = w_**ivy.expand_dims(gamma, axis=1)
w = w_sharpen / ivy.reduce_sum(w_sharpen, axis=1,
keepdims=True) # eq (9)
return w
def compute_cost_and_sdfs(learnable_anchor_vals, anchor_points,
start_anchor_val, end_anchor_val, query_points, sim):
anchor_vals = ivy.concatenate(
(ivy.expand_dims(start_anchor_val, 0), learnable_anchor_vals,
ivy.expand_dims(end_anchor_val, 0)), 0)
joint_angles = ivy_robot.sample_spline_path(anchor_points, anchor_vals,
query_points)
link_positions = ivy.transpose(
sim.ivy_manipulator.sample_links(joint_angles), (1, 0, 2))
length_cost = compute_length(link_positions)
sdf_vals = sim.sdf(ivy.reshape(link_positions, (-1, 3)))
coll_cost = -ivy.reduce_mean(sdf_vals)
total_cost = length_cost + coll_cost * 10
return total_cost[0], joint_angles, link_positions, ivy.reshape(
sdf_vals, (-1, 100, 1))
def get_reward(self):
"""
Get reward based on current state
:return: Reward array
"""
# Goal proximity.
x = ivy.reduce_sum(ivy.cos(self.angles), -1)
y = ivy.reduce_sum(ivy.sin(self.angles), -1)
xy = ivy.concatenate([ivy.expand_dims(x, 0),
ivy.expand_dims(y, 0)],
axis=0)
rew = ivy.reshape(
ivy.exp(-1 * ivy.reduce_sum((xy - self.goal_xy)**2, -1)), (-1, ))
return ivy.reduce_mean(rew, axis=0, keepdims=True)
def euler_pose_to_mat_pose(euler_pose, convention='zyx', batch_shape=None):
"""
Convert :math: Euler angle pose
:math:`\mathbf{p}_{abc} = [\mathbf{x}_c, \mathbf{θ}_{xyz}] = [x, y, z, ϕ_a, ϕ_b, ϕ_c]` to matrix pose
:math:`\mathbf{P}\in\mathbb{R}^{3×4}`.\n
`[reference] <https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix>`_
:param euler_pose: Euler angle pose *[batch_shape,6]*
:type euler_pose: array
:param convention: The axes for euler rotation, in order of L.H.S. matrix multiplication.
:type convention: str, optional
:param batch_shape: Shape of batch. Inferred from inputs if None.
:type batch_shape: sequence of ints, optional
:return: Matrix pose *[batch_shape,3,4]*
"""
if batch_shape is None:
batch_shape = euler_pose.shape[:-1]
# BS x 3 x 3
rot_mat = _ivy_rot_mat.euler_to_rot_mat(euler_pose[..., 3:], convention,
batch_shape)
# BS x 3 x 4
return _ivy.concatenate(
(rot_mat, _ivy.expand_dims(euler_pose[..., 0:3], -1)), -1)
def __init__(self,
img_meas: Dict[str, ESMCamMeasurement],
agent_rel_mat: ivy.Array,
control_mean: ivy.Array = None,
control_cov: ivy.Array = None):
"""
Create esm observation container
:param img_meas: dict of ESMImageMeasurement objects, with keys for camera names.
:type: img_meas: Ivy container
:param agent_rel_mat: The pose of the agent relative to the previous pose, in matrix form
*[batch_size, timesteps, 3, 4]*.
:type agent_rel_mat: array
:param control_mean: The pose of the agent relative to the previous pose, in rotation vector pose form.
Inferred from agent_rel_mat if None. *[batch_size, timesteps, 6]*
:type control_mean: array, optional
:param control_cov: The convariance of the agent relative pose, in rotation vector form.
Assumed all zero if None. *[batch_size, timesteps, 6, 6]*.
:type control_cov: array, optional
"""
self['img_meas'] = Container(img_meas)
agent_rel_mat = _pad_to_batch_n_time_dims(agent_rel_mat, 4)
self['agent_rel_mat'] = agent_rel_mat
if control_mean is None:
control_mean = ivy_mech.mat_pose_to_rot_vec_pose(agent_rel_mat)
else:
control_mean = _pad_to_batch_n_time_dims(control_mean, 3)
self['control_mean'] = control_mean
if control_cov is None:
control_cov = ivy.tile(ivy.expand_dims(ivy.zeros_like(control_mean), -1), (1, 1, 1, 6))
else:
control_cov = _pad_to_batch_n_time_dims(control_cov, 4)
self['control_cov'] = control_cov
def _group_tensor_into_windowed_tensor(self, x, valid_first_frame):
if self._window_size == 1:
valid_first_frame_pruned = ivy.cast(valid_first_frame[:, 0],
'bool')
else:
valid_first_frame_pruned = ivy.cast(
valid_first_frame[:1 - self._window_size, 0], 'bool')
if ivy.reduce_sum(ivy.cast(valid_first_frame_pruned, 'int32'))[0] == 0:
valid_first_frame_pruned =\
ivy.cast(ivy.one_hot(0, self._sequence_lengths[0] - self._window_size + 1), 'bool')
window_idxs_single = ivy.indices_where(valid_first_frame_pruned)
gather_idxs_list = list()
for w_idx in window_idxs_single:
gather_idxs_list.append(
ivy.expand_dims(
ivy.arange(w_idx[0] + self._window_size, w_idx[0], 1), 0))
gather_idxs = ivy.concatenate(gather_idxs_list, 0)
gather_idxs = ivy.reshape(gather_idxs, (-1, 1))
num_valid_windows_for_seq = ivy.shape(window_idxs_single)[0:1]
return ivy.reshape(
ivy.gather_nd(x, gather_idxs),
ivy.concatenate(
(num_valid_windows_for_seq, ivy.array(
[self._window_size]), ivy.shape(x)[1:]), 0))
def compute_cost_and_sdfs(learnable_anchor_vals, anchor_points,
start_anchor_val, end_anchor_val, query_points, sim):
anchor_vals = ivy.concatenate(
(ivy.expand_dims(start_anchor_val, 0), learnable_anchor_vals,
ivy.expand_dims(end_anchor_val, 0)), 0)
poses = ivy_robot.sample_spline_path(anchor_points, anchor_vals,
query_points)
inv_ext_mat_query_vals = ivy_mech.rot_vec_pose_to_mat_pose(poses)
body_positions = ivy.transpose(
sim.ivy_drone.sample_body(inv_ext_mat_query_vals), (1, 0, 2))
length_cost = compute_length(body_positions)
sdf_vals = sim.sdf(ivy.reshape(body_positions, (-1, 3)))
coll_cost = -ivy.reduce_mean(sdf_vals)
total_cost = length_cost + coll_cost * 10
return total_cost[0], poses, body_positions, ivy.reshape(
sdf_vals, (-1, 100, 1))
def batch_array(x, _):
return [
ivy.concatenate([
ivy.expand_dims(item, 0)
for item in x[i * batch_size:i * batch_size + batch_size]
], 0) for i in range(int(len(x) / batch_size))
]
def focal_lengths_and_pp_offsets_to_calib_mat(focal_lengths, pp_offsets, batch_shape=None, dev_str=None):
"""
Compute calibration matrix :math:`\mathbf{K}\in\mathbb{R}^{3×3}` from focal lengths :math:`f_x, f_y` and
principal-point offsets :math:`p_x, p_y`.\n
`[reference] <localhost:63342/ivy/docs/source/references/mvg_textbook.pdf#page=173>`_
page 155, section 6.1, equation 6.4
:param focal_lengths: Focal lengths *[batch_shape,2]*
:type focal_lengths: array
:param pp_offsets: Principal-point offsets *[batch_shape,2]*
:type pp_offsets: array
:param batch_shape: Shape of batch. Inferred from inputs if None.
:type batch_shape: sequence of ints, optional
:param dev_str: device on which to create the array 'cuda:0', 'cuda:1', 'cpu' etc. Same as x if None.
:type dev_str: str, optional
:return: Calibration matrix *[batch_shape,3,3]*
"""
if batch_shape is None:
batch_shape = focal_lengths.shape[:-1]
if dev_str is None:
dev_str = _ivy.dev_str(focal_lengths)
# shapes as list
batch_shape = list(batch_shape)
# BS x 1 x 1
zeros = _ivy.zeros(batch_shape + [1, 1], dev_str=dev_str)
ones = _ivy.ones(batch_shape + [1, 1], dev_str=dev_str)
# BS x 2 x 1
focal_lengths_reshaped = _ivy.expand_dims(focal_lengths, -1)
pp_offsets_reshaped = _ivy.expand_dims(pp_offsets, -1)
# BS x 1 x 3
row1 = _ivy.concatenate((focal_lengths_reshaped[..., 0:1, :], zeros, pp_offsets_reshaped[..., 0:1, :]), -1)
row2 = _ivy.concatenate((zeros, focal_lengths_reshaped[..., 1:2, :], pp_offsets_reshaped[..., 1:2, :]), -1)
row3 = _ivy.concatenate((zeros, zeros, ones), -1)
# BS x 3 x 3
return _ivy.concatenate((row1, row2, row3), -2)
def _se_to_mask(se: ivy.Array) -> ivy.Array:
se_h, se_w = se.shape
se_flat = ivy.reshape(se, (-1,))
num_feats = se_h * se_w
i_s = ivy.expand_dims(ivy.arange(num_feats, dev_str=ivy.dev_str(se)), -1)
y_s = i_s % se_h
x_s = i_s // se_h
indices = ivy.concatenate((i_s, ivy.zeros_like(i_s, dtype_str='int32'), x_s, y_s), -1)
out = ivy.scatter_nd(
indices, ivy.cast(se_flat >= 0, ivy.dtype_str(se)), (num_feats, 1, se_h, se_w), dev_str=ivy.dev_str(se))
return out
def __init__(self,
img_mean: ivy.Array,
cam_rel_mat: ivy.Array,
img_var: ivy.Array = None,
validity_mask: ivy.Array = None,
pose_mean: ivy.Array = None,
pose_cov: ivy.Array = None):
"""
Create esm image measurement container
:param img_mean: Camera-relative co-ordinates and image features
*[batch_size, timesteps, height, width, 3 + feat]*
:type: img_mean: array
:param cam_rel_mat: The pose of the camera relative to the current agent pose, in matrix form
*[batch_size, timesteps, 3, 4]*
:type cam_rel_mat: array
:param img_var: Image depth and feature variance values, assumed all zero if None.
*[batch_size, timesteps, height, width, 1 + feat]*
:type: img_var: array, optional
:param validity_mask: Validity mask, for which pixels should be considered. Assumed all valid if None
*[batch_size, timesteps, height, width, 1]*
:type validity_mask: array, optional
:param pose_mean: The pose of the camera relative to the current agent pose, in rotation vector pose form.
Inferred from cam_rel_mat if None. *[batch_size, timesteps, 6]*
:type pose_mean: array, optional
:param pose_cov: The convariance of the camera relative pose, in rotation vector form. Assumed all zero if None.
*[batch_size, timesteps, 6, 6]*
:type pose_cov: array, optional
"""
img_mean = _pad_to_batch_n_time_dims(img_mean, 5)
cam_rel_mat = _pad_to_batch_n_time_dims(cam_rel_mat, 4)
self['img_mean'] = img_mean
self['cam_rel_mat'] = cam_rel_mat
if img_var is None:
img_var = ivy.zeros_like(img_mean)
else:
img_var = _pad_to_batch_n_time_dims(img_var, 5)
self['img_var'] = img_var
if validity_mask is None:
validity_mask = ivy.ones_like(img_mean[..., 0:1])
else:
validity_mask = _pad_to_batch_n_time_dims(validity_mask, 5)
self['validity_mask'] = validity_mask
if pose_mean is None:
pose_mean = ivy_mech.mat_pose_to_rot_vec_pose(cam_rel_mat)
else:
pose_mean = _pad_to_batch_n_time_dims(pose_mean, 3)
self['pose_mean'] = pose_mean
if pose_cov is None:
pose_cov = ivy.tile(ivy.expand_dims(ivy.zeros_like(pose_mean), -1), (1, 1, 1, 6))
else:
pose_cov = _pad_to_batch_n_time_dims(pose_cov, 4)
self['pose_cov'] = pose_cov
def render_rays_via_termination_probabilities(ray_term_probs,
features,
render_variance=False):
"""
Render features onto the image plane, given rays sampled at radial depths with readings of
feature values and densities at these sample points.
:param ray_term_probs: The ray termination probabilities *[batch_shape,num_samples_per_ray]*
:type ray_term_probs: array
:param features: Feature values at the sample points *[batch_shape,num_samples_per_ray,feat_dim]*
:type features: array
:param render_variance: Whether to also render the feature variance. Default is False.
:type render_variance: bool, optional
:return: The feature renderings along the rays, computed via the termination probabilities *[batch_shape,feat_dim]*
"""
# BS x NSPR
rendering = ivy.reduce_sum(
ivy.expand_dims(ray_term_probs, -1) * features, -2)
if not render_variance:
return rendering
var = ivy.reduce_sum(
ray_term_probs * (ivy.expand_dims(rendering, -2) - features)**2, -2)
return rendering, var
def expand_dims(self, axis):
"""
Expand dims of all sub-arrays of container object.
:param axis: Axis along which to expand dimensions of the sub-arrays.
:type axis: int
:return: Container object at with all sub-array dimensions expanded along the axis.
"""
return_dict = dict()
for key, value in sorted(self.items()):
if isinstance(value, Container):
return_dict[key] = value.expand_dims(axis)
else:
return_dict[key] = _ivy.expand_dims(value, axis)
return Container(return_dict)
def create_trimesh_indices_for_image(batch_shape, image_dims, dev_str='cpu:0'):
"""
Create triangle mesh for image with given image dimensions
:param batch_shape: Shape of batch.
:type batch_shape: sequence of ints
:param image_dims: Image dimensions.
:type image_dims: sequence of ints
:param dev_str: device on which to create the array 'cuda:0', 'cuda:1', 'cpu' etc.
:type dev_str: str, optional
:return: Triangle mesh indices for image *[batch_shape,h*w*some_other_stuff,3]*
"""
# shapes as lists
batch_shape = list(batch_shape)
image_dims = list(image_dims)
# other shape specs
num_batch_dims = len(batch_shape)
tri_dim = 2 * (image_dims[0] - 1) * (image_dims[1] - 1)
flat_shape = [1] * num_batch_dims + [tri_dim] + [3]
tile_shape = batch_shape + [1] * 2
# 1 x W-1
t00_ = _ivy.reshape(_ivy.arange(image_dims[1] - 1, dtype_str='float32', dev_str=dev_str), (1, -1))
# H-1 x 1
k_ = _ivy.reshape(_ivy.arange(image_dims[0] - 1, dtype_str='float32', dev_str=dev_str), (-1, 1)) * image_dims[1]
# H-1 x W-1
t00_ = _ivy.matmul(_ivy.ones((image_dims[0] - 1, 1), dev_str=dev_str), t00_)
k_ = _ivy.matmul(k_, _ivy.ones((1, image_dims[1] - 1), dev_str=dev_str))
# (H-1xW-1) x 1
t00 = _ivy.expand_dims(t00_ + k_, -1)
t01 = t00 + 1
t02 = t00 + image_dims[1]
t10 = t00 + image_dims[1] + 1
t11 = t01
t12 = t02
# (H-1xW-1) x 3
t0 = _ivy.concatenate((t00, t01, t02), -1)
t1 = _ivy.concatenate((t10, t11, t12), -1)
# BS x 2x(H-1xW-1) x 3
return _ivy.tile(_ivy.reshape(_ivy.concatenate((t0, t1), 0),
flat_shape), tile_shape)
def axis_angle_pose_to_mat_pose(axis_angle_pose):
"""
Convert axis-angle pose :math:`\mathbf{p}_{aa} = [\mathbf{x}_c, \mathbf{e}, θ] = [x, y, z, e_x, e_y, e_z, θ]` to
matrix pose :math:`\mathbf{P}\in\mathbb{R}^{3×4}`.
:param axis_angle_pose: Quaternion pose *[batch_shape,7]*
:type axis_angle_pose: array
:return: Matrix pose *[batch_shape,3,4]*
"""
# BS x 3 x 3
rot_mat = _ivy_rot_mat.axis_angle_to_rot_mat(axis_angle_pose[..., 3:])
# BS x 3 x 4
return _ivy.concatenate(
(rot_mat, _ivy.expand_dims(axis_angle_pose[..., :3], -1)), -1)
def quaternion_pose_to_mat_pose(quat_pose):
"""
Convert quaternion pose :math:`\mathbf{p}_{q} = [\mathbf{x}_c, \mathbf{q}] = [x, y, z, q_i, q_j, q_k, q_r]` to
matrix pose :math:`\mathbf{P}\in\mathbb{R}^{3×4}`.\n
`[reference] <https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#Conversion_to_and_from_the_matrix_representation>`_
:param quat_pose: Quaternion pose *[batch_shape,7]*
:type quat_pose: array
:return: Matrix pose *[batch_shape,3,4]*
"""
# BS x 3 x 3
rot_mat = _ivy_rot_mat.quaternion_to_rot_mat(quat_pose[..., 3:])
# BS x 3 x 1
rhs = _ivy.expand_dims(quat_pose[..., 0:3], -1)
# BS x 3 x 4
return _ivy.concatenate((rot_mat, rhs), -1)
def rot_vec_pose_to_mat_pose(rot_vec_pose):
"""
Convert rotation vector pose :math:`\mathbf{p}_{rv} = [\mathbf{x}_c, \mathbf{θ}_{rv}] = [x, y, z, θe_x, θe_y, θe_z]`
to matrix pose :math:`\mathbf{P}\in\mathbb{R}^{3×4}`.\n
`[reference] <https://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions#Euler_axis_and_angle_(rotation_vector)>`_
:param rot_vec_pose: Rotation vector pose *[batch_shape,6]*
:type rot_vec_pose: array
:return: Matrix pose *[batch_shape,3,4]*
"""
# BS x 4
quaternion = _ivy_quat.rotation_vector_to_quaternion(rot_vec_pose[..., 3:])
# BS x 3 x 3
rot_mat = _ivy_rot_mat.quaternion_to_rot_mat(quaternion)
# BS x 3 x 4
return _ivy.concatenate(
(rot_mat, _ivy.expand_dims(rot_vec_pose[..., 0:3], -1)), -1)
def _group_tensor_into_windowed_tensor_simple(self, x, seq_info):
seq_info = self._update_seq_info_for_window(seq_info)
if self._fixed_sequence_length:
return ivy.reshape(ivy.gather_nd(x, ivy.array(self._gather_idxs)),
(self._windows_per_seq, self._window_size) +
x.shape[1:])
else:
num_windows_in_seq = int(
ivy.to_numpy(
ivy.maximum(seq_info.length[0] - self._window_size + 1,
1)))
window_idxs_in_seq = ivy.arange(num_windows_in_seq, 0, 1)
gather_idxs = ivy.tile(
ivy.reshape(ivy.arange(self._window_size, 0, 1),
(1, self._window_size)),
(num_windows_in_seq, 1)) + ivy.expand_dims(
window_idxs_in_seq, -1)
gather_idxs_flat = ivy.reshape(
gather_idxs, (self._window_size * num_windows_in_seq, 1))
return ivy.reshape(ivy.gather_nd(x, gather_idxs_flat),
(num_windows_in_seq, self._window_size) +
x.shape[1:])
def stratified_sample(starts, ends, num_samples, batch_shape=None):
"""
Perform stratified sampling, between start and end arrays. This operation divides the range into equidistant bins,
and uniformly samples value within the ranges for each of these bins.
:param starts: Start values *[batch_shape]*
:type starts: array
:param ends: End values *[batch_shape]*
:type ends: array
:param num_samples: The number of samples to generate between starts and ends
:type num_samples: int
:param batch_shape: Shape of batch, Inferred from inputs if None.
:type batch_shape: sequence of ints, optional
:return: The stratified samples, with each randomly placed in uniformly spaced bins *[batch_shape,num_samples]*
"""
# shapes
if batch_shape is None:
batch_shape = starts.shape
# shapes as lists
batch_shape = list(batch_shape)
# BS
bin_sizes = (ends - starts) / num_samples
# BS x NS
linspace_vals = ivy.linspace(starts, ends - bin_sizes, num_samples)
# BS x NS
random_uniform = ivy.random_uniform(shape=batch_shape + [num_samples],
dev_str=ivy.dev_str(starts))
# BS x NS
random_offsets = random_uniform * ivy.expand_dims(bin_sizes, -1)
# BS x NS
return linspace_vals + random_offsets
def cap(self):
self._pr_obj.handle_explicitly()
return ivy.expand_dims(ivy.array(self._pr_obj.capture_depth(True).tolist()), -1),\
ivy.array(self._pr_obj.capture_rgb().tolist())
def main(f=None):
# Framework Setup #
# ----------------#
# choose random framework
f = choose_random_framework() if f is None else f
set_framework(f)
# Orientation #
# ------------#
# rotation representations
# 3
rot_vec = ivy.array([0., 1., 0.])
# 3 x 3
rot_mat = ivy_mech.rot_vec_to_rot_mat(rot_vec)
# 3
euler_angles = ivy_mech.rot_mat_to_euler(rot_mat, 'zyx')
# 4
quat = ivy_mech.euler_to_quaternion(euler_angles)
# 4
axis_and_angle = ivy_mech.quaternion_to_axis_angle(quat)
# 3
rot_vec_again = axis_and_angle[..., :-1] * axis_and_angle[..., -1:]
# Pose #
# -----#
# pose representations
# 3
position = ivy.ones_like(rot_vec)
# 6
rot_vec_pose = ivy.concatenate((position, rot_vec), 0)
# 3 x 4
mat_pose = ivy_mech.rot_vec_pose_to_mat_pose(rot_vec_pose)
# 6
euler_pose = ivy_mech.mat_pose_to_euler_pose(mat_pose)
# 7
quat_pose = ivy_mech.euler_pose_to_quaternion_pose(euler_pose)
# 6
rot_vec_pose_again = ivy_mech.quaternion_pose_to_rot_vec_pose(quat_pose)
# Position #
# ---------#
# conversions of positional representation
# 3
cartesian_coord = ivy.random_uniform(0., 1., (3, ))
# 3
polar_coord = ivy_mech.cartesian_to_polar_coords(cartesian_coord)
# 3
cartesian_coord_again = ivy_mech.polar_to_cartesian_coords(polar_coord)
# cartesian co-ordinate frame-of-reference transformations
# 3 x 4
trans_mat = ivy.random_uniform(0., 1., (3, 4))
# 4
cartesian_coord_homo = ivy_mech.make_coordinates_homogeneous(
cartesian_coord)
# 3
trans_cartesian_coord = ivy.matmul(
trans_mat, ivy.expand_dims(cartesian_coord_homo, -1))[:, 0]
# 4
trans_cartesian_coord_homo = ivy_mech.make_coordinates_homogeneous(
trans_cartesian_coord)
# 4 x 4
trans_mat_homo = ivy_mech.make_transformation_homogeneous(trans_mat)
# 3 x 4
inv_trans_mat = ivy.inv(trans_mat_homo)[0:3]
# 3
cartesian_coord_again = ivy.matmul(
inv_trans_mat, ivy.expand_dims(trans_cartesian_coord_homo, -1))[:, 0]
# message
print('End of Run Through Demo!')
def _forward(self,
obs: ESMObservation,
memory: ESMMemory = None,
batch_size=None,
num_timesteps=None,
num_cams=None,
image_dims=None):
"""
Perform ESM update step.
:param obs: Observations
:type obs: ESMObservation
:param memory: Memory from the previous time-step, uses internal parameter if None.
:type memory: ESMMemory, optional.
:param batch_size: Size of batch, inferred from inputs if None.
:type batch_size: int, optional
:param num_timesteps: Number of timesteps, inferred from inputs if None.
:type num_timesteps: int, optional
:param num_cams: Number of cameras, inferred from inputs if None.
:type num_cams: int, optional
:param image_dims: Image dimensions of captured images, inferred from inputs if None.
:type image_dims: sequence of ints, optional
:return: New memory of type ESMMemory
"""
# get shapes
img_meas = (next(iter(obs.img_meas.values()))).img_mean
if batch_size is None:
batch_size = int(img_meas.shape[0])
if num_timesteps is None:
num_timesteps = int(img_meas.shape[1])
if num_cams is None:
num_cams = len(obs.img_meas.values())
if image_dims is None:
image_dims = list(img_meas.shape[2:4])
# get only previous memory
# extract from memory #
# --------------------#
if memory:
prev_mem = memory.slice((slice(None, None, None), -1))
elif self._stateful and self._memory is not None:
prev_mem = self._memory.slice((slice(None, None, None), -1))
else:
prev_mem = self.empty_memory(batch_size, 1).slice(
(slice(None, None, None), -1))
# holes prior #
# ------------#
# B x N x OH x OW x 1
self._sphere_depth_prior = \
ivy.ones([batch_size, num_timesteps] + self._sphere_img_dims + [1], dev_str=self._dev_str) * \
self._sphere_depth_prior_val
# B x N x OH x OW x F
self._sphere_feat_prior = \
ivy.ones([batch_size, num_timesteps] + self._sphere_img_dims + [self._feat_dim], dev_str=self._dev_str) * \
self._sphere_feat_prior_val
# B x N x OH x OW x (1+F)
holes_prior = ivy.concatenate([self._sphere_depth_prior] +
[self._sphere_feat_prior], -1)
# holes prior variances #
# ----------------------#
# B x N x OH x OW x 2
sphere_ang_pix_prior_var = \
ivy.ones([batch_size, num_timesteps] + self._sphere_img_dims + [2], dev_str=self._dev_str) * self._ang_pix_prior_var_val
# B x N x OH x OW x 1
sphere_depth_prior_var = \
ivy.ones([batch_size, num_timesteps] + self._sphere_img_dims + [1], dev_str=self._dev_str) * self._depth_prior_var_val
# B x N x OH x OW x F
sphere_feat_prior_var = \
ivy.ones([batch_size, num_timesteps] + self._sphere_img_dims + [self._feat_dim], dev_str=self._dev_str) * \
self._feat_prior_var_val
# B x N x OH x OW x (3+F)
holes_prior_var = ivy.concatenate([sphere_ang_pix_prior_var] +
[sphere_depth_prior_var] +
[sphere_feat_prior_var], -1)
# variance threshold #
# -------------------#
# B x N x (3+F) x 1
var_threshold_min = ivy.tile(
ivy.reshape(
ivy.stack([self._min_ang_pix_var] * 2 + [self._min_depth_var] +
[self._min_feat_var] * self._feat_dim),
[1, 1, 3 + self._feat_dim, 1]),
[batch_size, num_timesteps, 1, 1])
var_threshold_max = ivy.tile(
ivy.reshape(
ivy.stack([self._ang_pix_var_threshold] * 2 +
[self._depth_var_threshold] +
[self._feat_var_threshold] * self._feat_dim),
[1, 1, 3 + self._feat_dim, 1]),
[batch_size, num_timesteps, 1, 1])
self._var_threshold = ivy.concatenate(
(var_threshold_min, var_threshold_max), -1)
# measurements #
# -------------#
# B x N x OH x OW x 3
uniform_sphere_pixel_coords = ivy_vision.create_uniform_pixel_coords_image(
self._sphere_img_dims, (batch_size, num_timesteps),
dev_str=self._dev_str)
# B x N x OH x OW x (3+F), B x N x OH x OW x (3+F)
meas_means, meas_vars = self._convert_images_to_omni_observations(
obs.img_meas, uniform_sphere_pixel_coords, holes_prior, batch_size,
num_timesteps, num_cams, image_dims)
# filtering #
# ----------#
# list of B x OH x OW x (3+F), list of B x OH x OW x (3+F)
fused_measurements_list, fused_variances_list = \
self._kalman_filter_on_measurement_sequence(
prev_mem.mean, prev_mem.var, holes_prior[:, 0], holes_prior_var[:, 0], meas_means, meas_vars,
uniform_sphere_pixel_coords[:, 0], obs.control_mean, obs.control_cov, obs.agent_rel_mat,
batch_size, num_timesteps)
# new variance #
# -------------#
# B x N x OH x OW x (3+F)
fused_variance = ivy.concatenate(
[ivy.expand_dims(item, 1) for item in fused_variances_list], 1)
# variance clipping
# B x N x OH x OW x 1
fused_depth_variance = ivy.clip(fused_variance[..., 0:1],
self._min_depth_var,
self._depth_prior_var_val)
# B x N x OH x OW x 3
fused_feat_variance = ivy.clip(fused_variance[...,
1:], self._min_feat_var,
self._feat_prior_var_val)
# B x N x OH x OW x (3+F)
fused_variance = ivy.concatenate([fused_depth_variance] +
[fused_feat_variance], -1)
# new mean #
# ---------#
# B x N x OH x OW x (3+F)
fused_measurement = ivy.concatenate(
[ivy.expand_dims(item, 1) for item in fused_measurements_list], 1)
# value clipping
# B x N x OH x OW x 2
fused_pixel_coords = fused_measurement[..., 0:2]
# B x N x OH x OW x 1
fused_depth = ivy.clip(fused_measurement[..., 2:3], self._min_depth,
self._max_depth)
# B x N x OH x OW x 3
fused_feat = fused_measurement[..., 3:]
# B x N x OH x OW x (3+F)
fused_measurement = ivy.concatenate([fused_pixel_coords] +
[fused_depth] + [fused_feat], -1)
# update memory #
# --------------#
# B x N x OH x OW x (3+F), B x N x OH x OW x (3+F)
self._memory = ESMMemory(mean=fused_measurement, var=fused_variance)
# return #
# -------#
return self._memory
def _kalman_filter_on_measurement_sequence(
self, prev_fused_val, prev_fused_variance, hole_prior,
hole_prior_var, meas, meas_vars, uniform_sphere_pixel_coords,
agent_rel_poses, agent_rel_pose_covs, agent_rel_mats, batch_size,
num_timesteps):
"""
Perform kalman filter on measurement sequence
:param prev_fused_val: Fused value from previous timestamp *[batch_size, oh, ow, (3+f)]*
:param prev_fused_variance: Fused variance from previous timestamp *[batch_size, oh, ow, (3+f)]*
:param hole_prior: Prior for holes in quantization *[batch_size, oh, ow, (1+f)]*
:param hole_prior_var: Prior variance for holes in quantization *[batch_size, oh, ow, (3+f)]*
:param meas: Measurements *[batch_size, num_timesteps, oh, ow, (3+f)]*
:param meas_vars: Measurement variances *[batch_size, num_timesteps, oh, ow, (3+f)]*
:param uniform_sphere_pixel_coords: Uniform sphere pixel co-ordinates *[batch_size, oh, ow, 3]*
:param agent_rel_poses: Relative poses of agents to the previous step *[batch_size, num_timesteps, 6]*
:param agent_rel_pose_covs: Agent relative pose covariances *[batch_size, num_timesteps, 6, 6]*
:param agent_rel_mats: Relative transformations matrix of agents to the previous step
*[batch_size, num_timesteps, 3, 4]*
:param batch_size: Size of batch
:param num_timesteps: Number of frames
:return: list of *[batch_size, oh, ow, (3+f)]*, list of *[batch_size, oh, ow, (3+f)]*
"""
fused_list = list()
fused_variances_list = list()
for i in range(num_timesteps):
# project prior from previous frame #
# ----------------------------------#
# B x OH x OW x (3+F)
prev_prior = prev_fused_val
prev_prior_variance = prev_fused_variance
# B x 3 x 4
agent_rel_mat = agent_rel_mats[:, i]
# B x 6
agent_rel_pose = agent_rel_poses[:, i]
# B x 6 x 6
agent_rel_pose_cov = agent_rel_pose_covs[:, i]
# B x OH x OW x (3+F) B x OH x OW x (3+F)
fused_projected, fused_projected_variance = self._omni_frame_to_omni_frame_projection(
agent_rel_pose, agent_rel_mat, uniform_sphere_pixel_coords,
prev_prior[..., 0:2], prev_prior[..., 2:3],
prev_prior[..., 3:], agent_rel_pose_cov, prev_prior_variance,
hole_prior, hole_prior_var, batch_size)
# reset prior
# B x OH x OW x (3+F)
prior = fused_projected
prior_var = fused_projected_variance
# per-pixel fusion with measurements #
# -----------------------------------#
# extract slice for frame
# B x OH x OW x (3+F)
measurement = meas[:, i]
measurement_variance = meas_vars[:, i]
# fuse prior and measurement
# B x 2 x OH x OW x (3+F)
prior_and_meas = ivy.concatenate(
(ivy.expand_dims(prior, 1), ivy.expand_dims(measurement, 1)),
1)
prior_and_meas_variance = ivy.concatenate((ivy.expand_dims(
prior_var, 1), ivy.expand_dims(measurement_variance, 1)), 1)
# B x OH x OW x (3+F)
low_var_mask = ivy.reduce_sum(
ivy.cast(
prior_and_meas_variance <
ivy.expand_dims(hole_prior_var, 1) *
self._threshold_var_factor, 'int32'), 1) > 0
# B x 1 x OH x OW x (3+F) B x 1 x OH x OW x (3+F)
# ToDo: handle this properly once re-implemented with a single scatter operation only
# currently depth values are fused even if these are clearly far apart
fused_val_unsmoothed, fused_variance_unsmoothed = \
self._fuse_measurements_with_uncertainty(prior_and_meas, prior_and_meas_variance, 1)
# B x OH x OW x (3+F)
# This prevents accumulating certainty from duplicate re-projections from prior measurements
fused_variance_unsmoothed = ivy.where(
low_var_mask, fused_variance_unsmoothed[:, 0], hole_prior_var)
# B x OH x OW x (3+F)
fused_val = fused_val_unsmoothed[:, 0]
fused_variance = fused_variance_unsmoothed
low_var_mask = fused_variance < hole_prior_var
# B x OH x OW x (3+F) B x OH x OW x (3+F)
fused_val, fused_variance = self.smooth(fused_val, fused_variance,
low_var_mask,
self._smooth_mean,
self._smooth_kernel_size,
True, True, batch_size)
# append to list for returning
# B x OH x OW x (3+F)
fused_list.append(fused_val)
# B x OH x OW x (3+F)
fused_variances_list.append(fused_variance)
# update for next time step
prev_fused_val = fused_val
prev_fused_variance = fused_variance
# list of *[batch_size, oh, ow, (3+f)]*, list of *[batch_size, oh, ow, (3+f)]*
return fused_list, fused_variances_list
def _convert_images_to_omni_observations(self, measurements,
uniform_sphere_pixel_coords,
holes_prior, batch_size,
num_timesteps, num_cams,
image_dims):
"""
Convert image to omni-directional measurements
:param measurements: perspective captured images and relative poses container
:param uniform_sphere_pixel_coords: Uniform sphere pixel coords *[batch_size, num_timesteps, oh, ow, 3]*
:param holes_prior: Prior for quantization holes *[batch_size, num_timesteps, oh, ow, 1+f]*
:param batch_size: Size of batch
:param num_timesteps: Number of frames
:param num_cams: Number of cameras
:param image_dims: Image dimensions
:return: *[batch_size, n, oh, ow, 3+f]* *[batch_size, n, oh, ow, 3+f]*
"""
# coords from all scene cameras wrt world
images_list = list()
images_var_list = list()
cam_rel_poses_list = list()
cam_rel_poses_cov_list = list()
cam_rel_mats_list = list()
validity_mask_list = list()
for key, item in measurements.to_iterator():
if key == 'img_mean':
# B x N x 1 x H x W x (3+f)
images_list.append(ivy.expand_dims(item, 2))
elif key == 'img_var':
# B x N x 1 x H x W x (3+f)
images_var_list.append(ivy.expand_dims(item, 2))
elif key == 'pose_mean':
# B x N x 1 x 6
cam_rel_poses_list.append(ivy.expand_dims(item, 2))
elif key == 'pose_cov':
# B x N x 1 x 6 x 6
cam_rel_poses_cov_list.append(ivy.expand_dims(item, 2))
elif key == 'cam_rel_mat':
# B x N x 1 x 3 x 4
cam_rel_mats_list.append(ivy.expand_dims(item, 2))
elif key == 'validity_mask':
validity_mask_list.append(ivy.expand_dims(item, 2))
else:
raise Exception('Invalid image key: {}'.format(key))
# B x N x C x H x W x (3+f)
images = ivy.concatenate(images_list, 2)
# B x N x C x H x W x (3+f)
var_to_project = ivy.concatenate(images_var_list, 2)
# B x N x C x 6
cam_to_cam_poses = ivy.concatenate(cam_rel_poses_list, 2)
# B x N x C x 3 x 4
cam_to_cam_mats = ivy.concatenate(cam_rel_mats_list, 2)
# B x N x C x 6 x 6
cam_to_cam_pose_covs = ivy.concatenate(cam_rel_poses_cov_list, 2)
# B x N x C x 1
validity_masks = ivy.concatenate(validity_mask_list, 2) > 0
# B x N x OH x OW x (3+f)
holes_prior_var = ivy.ones(
[batch_size, num_timesteps] + self._sphere_img_dims +
[3 + self._feat_dim],
dev_str=self._dev_str) * 1e12
# reset invalid regions to prior
# B x N x C x H x W x (3+f)
images = ivy.where(
validity_masks, images,
ivy.concatenate(
(images[..., 0:2],
ivy.zeros_like(images[..., 2:], dev_str=self._dev_str)), -1))
# B x N x C x H x W x (3+f)
var_to_project = ivy.where(
validity_masks, var_to_project,
ivy.ones_like(var_to_project, dev_str=self._dev_str) * 1e12)
# B x N x OH x OW x (3+f) # B x N x OH x OW x (3+f)
return self._frame_to_omni_frame_projection(
cam_to_cam_poses, cam_to_cam_mats, uniform_sphere_pixel_coords,
images[..., 0:3], images[..., 3:], cam_to_cam_pose_covs,
var_to_project, holes_prior, holes_prior_var, batch_size,
num_timesteps, num_cams, image_dims)
def lstm_update(x, init_h, init_c, kernel, recurrent_kernel, bias=None, recurrent_bias=None):
"""
Perform long-short term memory update by unrolling time dimension of input array.
:param x: input tensor of LSTM layer *[batch_shape, t, in]*.
:type x: array
:param init_h: initial state tensor for the cell output *[batch_shape, out]*.
:type init_h: array
:param init_c: initial state tensor for the cell hidden state *[batch_shape, out]*.
:type init_c: array
:param kernel: weights for cell kernel *[in, 4 x out]*.
:type kernel: array
:param recurrent_kernel: weights for cell recurrent kernel *[out, 4 x out]*.
:type recurrent_kernel: array
:param bias: bias for cell kernel *[4 x out]*.
:type bias: array
:param recurrent_bias: bias for cell recurrent kernel *[4 x out]*.
:type recurrent_bias: array
:return: hidden state for all timesteps *[batch_shape,t,out]* and cell state for last timestep *[batch_shape,out]*
"""
# get shapes
x_shape = list(x.shape)
batch_shape = x_shape[:-2]
timesteps = x_shape[-2]
input_channels = x_shape[-1]
x_flat = ivy.reshape(x, (-1, input_channels))
# input kernel
Wi = kernel
Wi_x = ivy.reshape(ivy.matmul(x_flat, Wi) + (bias if bias is not None else 0),
batch_shape + [timesteps, -1])
Wii_x, Wif_x, Wig_x, Wio_x = ivy.split(Wi_x, 4, -1)
# recurrent kernel
Wh = recurrent_kernel
# lstm states
ht = init_h
ct = init_c
# lstm outputs
ot = x
hts_list = list()
# unrolled time dimension with lstm steps
for Wii_xt, Wif_xt, Wig_xt, Wio_xt in zip(ivy.unstack(Wii_x, axis=-2), ivy.unstack(Wif_x, axis=-2),
ivy.unstack(Wig_x, axis=-2), ivy.unstack(Wio_x, axis=-2)):
htm1 = ht
ctm1 = ct
Wh_htm1 = ivy.matmul(htm1, Wh) + (recurrent_bias if recurrent_bias is not None else 0)
Whi_htm1, Whf_htm1, Whg_htm1, Who_htm1 = ivy.split(Wh_htm1, num_sections=4, axis=-1)
it = ivy.sigmoid(Wii_xt + Whi_htm1)
ft = ivy.sigmoid(Wif_xt + Whf_htm1)
gt = ivy.tanh(Wig_xt + Whg_htm1)
ot = ivy.sigmoid(Wio_xt + Who_htm1)
ct = ft * ctm1 + it * gt
ht = ot * ivy.tanh(ct)
hts_list.append(ivy.expand_dims(ht, -2))
return ivy.concatenate(hts_list, -2), ct
def _forward(self, x):
for layer in self._layers:
x = layer(x)
return ivy.expand_dims(x, 0)
def cuboid_signed_distances(cuboid_ext_mats,
cuboid_dims,
query_positions,
batch_shape=None):
"""
Return the signed distances of a set of query points from the cuboid surfaces.\n
`[reference] <https://www.iquilezles.org/www/articles/distfunctions/distfunctions.htm>`_
:param cuboid_ext_mats: Extrinsic matrices of the cuboids *[batch_shape,num_cuboids,3,4]*
:type cuboid_ext_mats: array
:param cuboid_dims: Dimensions of the cuboids, in the order x, y, z *[batch_shape,num_cuboids,3]*
:type cuboid_dims: array
:param query_positions: Points for which to query the signed distances *[batch_shape,num_points,3]*
:type query_positions: array
:param batch_shape: Shape of batch. Assumed no batches if None.
:type batch_shape: sequence of ints, optional
:return: The distances of the query points from the closest cuboid surface *[batch_shape,num_points,1]*
"""
if batch_shape is None:
batch_shape = cuboid_ext_mats.shape[:-3]
# shapes as list
batch_shape = list(batch_shape)
num_batch_dims = len(batch_shape)
batch_dims_for_trans = list(range(num_batch_dims))
num_cuboids = cuboid_ext_mats.shape[-3]
num_points = query_positions.shape[-2]
# BS x 3 x NP
query_positions_trans = _ivy.transpose(
query_positions,
batch_dims_for_trans + [num_batch_dims + 1, num_batch_dims])
# BS x 1 x NP
ones = _ivy.ones_like(query_positions_trans[..., 0:1, :])
# BS x 4 x NP
query_positions_trans_homo = _ivy.concatenate(
(query_positions_trans, ones), -2)
# BS x NCx3 x 4
cuboid_ext_mats_flat = _ivy.reshape(cuboid_ext_mats, batch_shape + [-1, 4])
# BS x NCx3 x NP
rel_query_positions_trans_flat = _ivy.matmul(cuboid_ext_mats_flat,
query_positions_trans_homo)
# BS x NC x 3 x NP
rel_query_positions_trans = _ivy.reshape(
rel_query_positions_trans_flat,
batch_shape + [num_cuboids, 3, num_points])
# BS x NC x NP x 3
rel_query_positions = _ivy.transpose(
rel_query_positions_trans, batch_dims_for_trans +
[num_batch_dims, num_batch_dims + 2, num_batch_dims + 1])
q = _ivy.abs(rel_query_positions) - _ivy.expand_dims(cuboid_dims / 2, -2)
q_max_clipped = _ivy.maximum(q, 1e-12)
# BS x NC x NP x 1
q_min_clipped = _ivy.minimum(_ivy.reduce_max(q, -1, keepdims=True), 0.)
q_max_clipped_len = _ivy.reduce_sum(q_max_clipped**2, -1,
keepdims=True)**0.5
sdfs = q_max_clipped_len + q_min_clipped
# BS x NP x 1
return _ivy.reduce_min(sdfs, -3)
