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 setup_primitive_scene(self):
# shape matrices
shape_matrices = ivy.concatenate([ivy.reshape(ivy.array(obj.get_matrix(), 'float32'), (1, 3, 4))
for obj in self._objects], 0)
# shape dims
x_dims = ivy.concatenate([ivy.reshape(ivy.array(
obj.get_bounding_box()[1] - obj.get_bounding_box()[0], 'float32'), (1, 1)) for obj in self._objects], 0)
y_dims = ivy.concatenate([ivy.reshape(ivy.array(
obj.get_bounding_box()[3] - obj.get_bounding_box()[2], 'float32'), (1, 1)) for obj in self._objects], 0)
z_dims = ivy.concatenate([ivy.reshape(ivy.array(
obj.get_bounding_box()[5] - obj.get_bounding_box()[4], 'float32'), (1, 1)) for obj in self._objects], 0)
shape_dims = ivy.concatenate((x_dims, y_dims, z_dims), -1)
# primitve scene visualization
if self._with_primitive_scene_vis:
scene_vis = [Shape.create(PrimitiveShape.CUBOID, ivy.to_numpy(shape_dim).tolist())
for shape_dim in shape_dims]
[obj.set_matrix(ivy.to_numpy(shape_mat).reshape(-1).tolist())
for shape_mat, obj in zip(shape_matrices, scene_vis)]
[obj.set_transparency(0.5) for obj in scene_vis]
# sdf
primitive_scene = PrimitiveScene(cuboid_ext_mats=ivy.inv(ivy_mech.make_transformation_homogeneous(
shape_matrices))[..., 0:3, :], cuboid_dims=shape_dims)
self.sdf = primitive_scene.sdf
def get_observation(self):
"""
Get observation from environment.
:return: observation array
"""
ob = (ivy.reshape(self.urchin_xys,
(-1, 2)), ivy.reshape(self.xy, (-1, 2)),
ivy.reshape(self.xy_vel,
(-1, 2)), ivy.reshape(self.goal_xy, (-1, 2)))
ob = ivy.concatenate(ob, axis=0)
return ivy.reshape(ob, (-1, ))
def create_uniform_pixel_coords_image(image_dims, batch_shape=None, normalized=False, dev_str=None):
"""
Create image of homogeneous integer :math:`xy` pixel co-ordinates :math:`\mathbf{X}\in\mathbb{Z}^{h×w×3}`, stored
as floating point values. The origin is at the top-left corner of the image, with :math:`+x` rightwards, and
:math:`+y` downwards. The final homogeneous dimension are all ones. In subsequent use of this image, the depth of
each pixel can be represented using this same homogeneous representation, by simply scaling each 3-vector by the
depth value. The final dimension therefore always holds the depth value, while the former two dimensions hold depth
scaled pixel co-ordinates.\n
`[reference] <localhost:63342/ivy/docs/source/references/mvg_textbook.pdf#page=172>`_
deduction from top of page 154, section 6.1, equation 6.1
:param image_dims: Image dimensions.
:type image_dims: sequence of ints.
:param batch_shape: Shape of batch. Assumed no batch dimensions if None.
:type batch_shape: sequence of ints, optional
:param normalized: Whether to normalize x-y pixel co-ordinates to the range 0-1.
:type normalized: bool
:param dev_str: device on which to create the array 'cuda:0', 'cuda:1', 'cpu' etc.
:type dev_str: str, optional
:return: Image of homogeneous pixel co-ordinates *[batch_shape,height,width,3]*
"""
# shapes as lists
batch_shape = [] if batch_shape is None else batch_shape
batch_shape = list(batch_shape)
image_dims = list(image_dims)
# other shape specs
num_batch_dims = len(batch_shape)
flat_shape = [1] * num_batch_dims + image_dims + [3]
tile_shape = batch_shape + [1] * 3
# H x W x 1
pixel_x_coords = _ivy.cast(_ivy.reshape(_ivy.tile(_ivy.arange(image_dims[1], dev_str=dev_str), [image_dims[0]]),
(image_dims[0], image_dims[1], 1)), 'float32')
if normalized:
pixel_x_coords = pixel_x_coords / (float(image_dims[1]) + MIN_DENOMINATOR)
# W x H x 1
pixel_y_coords_ = _ivy.cast(_ivy.reshape(_ivy.tile(_ivy.arange(image_dims[0], dev_str=dev_str), [image_dims[1]]),
(image_dims[1], image_dims[0], 1)), 'float32')
# H x W x 1
pixel_y_coords = _ivy.transpose(pixel_y_coords_, (1, 0, 2))
if normalized:
pixel_y_coords = pixel_y_coords / (float(image_dims[0]) + MIN_DENOMINATOR)
# H x W x 1
ones = _ivy.ones_like(pixel_x_coords, dev_str=dev_str)
# BS x H x W x 3
return _ivy.tile(_ivy.reshape(_ivy.concatenate((pixel_x_coords, pixel_y_coords, ones), -1),
flat_shape), tile_shape)
def get_observation(self):
"""
Get observation from environment.
:return: observation array
"""
ob = (ivy.reshape(ivy.cos(self.angles),
(1, 2)), ivy.reshape(ivy.sin(self.angles), (1, 2)),
ivy.reshape(self.angle_vels,
(1, 2)), ivy.reshape(self.goal_xy, (1, 2)))
ob = ivy.concatenate(ob, axis=0)
return ivy.reshape(ob, (-1, ))
def _forward(self, inputs, hidden=None):
inputs_shape = list(inputs.shape)
batch_shape = inputs_shape[:-2]
time_dim = inputs_shape[-2]
inputs = ivy.reshape(inputs, [-1] + list(inputs.shape[-2:]))
if hidden is None:
hidden = self._ntm_cell.get_start_state(inputs, inputs.shape[0], inputs.dtype, v=self.v.ntm_cell)
outputs = []
for x in ivy.unstack(inputs, 1):
output, hidden = self._ntm_cell(x, hidden)
outputs.append(output)
ret = ivy.stack(outputs, 1)
return ivy.reshape(ret, batch_shape + [time_dim, -1])
def smooth(self, fused_val, fused_variance, low_var_mask, smooth_mean,
smooth_kernel_size, variance_mode, fix_low_var_pixels,
batch_size):
variance_mode = True
var_for_smoothing = fused_variance
# image smoothing
if smooth_mean:
# pad borders
pad_size = int(smooth_kernel_size / 2)
fused_val_pad = ivy_vision.pad_omni_image(fused_val, pad_size,
self._sphere_img_dims)
fused_variance_pad = ivy_vision.pad_omni_image(
var_for_smoothing, pad_size, self._sphere_img_dims)
expanded_sphere_img_dims = [
item + 2 * pad_size for item in self._sphere_img_dims
]
# reshape for dilation and erosion
fused_val_pad_flat = ivy.reshape(
fused_val_pad[..., 2:],
[batch_size] + expanded_sphere_img_dims + [1 + self._feat_dim])
fused_var_pad_flat = ivy.reshape(
fused_variance_pad[..., 2:],
[batch_size] + expanded_sphere_img_dims + [1 + self._feat_dim])
if variance_mode:
smoothed_fused_val_flat, smoothed_fused_var_flat = \
ivy_vision.smooth_image_fom_var_image(fused_val_pad_flat, fused_var_pad_flat, smooth_kernel_size,
ivy.array([1.] * (1 + self._feat_dim), dev_str=self._dev_str))
else:
smoothed_fused_val_flat, smoothed_fused_var_flat = \
ivy_vision.weighted_image_smooth(fused_val_pad_flat, 1 - fused_var_pad_flat, smooth_kernel_size)
# reshape to image dims
smoothed_fused_val = ivy.reshape(
smoothed_fused_val_flat,
[batch_size] + self._sphere_img_dims + [1 + self._feat_dim])
smoothed_fused_val = ivy.concatenate(
(fused_val[..., 0:2], smoothed_fused_val), -1)
# replace temporary zeros with their prior values
# This ensures that the smoothing operation only changes the values for regions of high variance
if fix_low_var_pixels:
fused_val = ivy.where(low_var_mask, fused_val,
smoothed_fused_val)
else:
fused_val = smoothed_fused_val
return fused_val, fused_variance
def transform(coords, trans, batch_shape=None, image_dims=None):
"""
Transform image of :math:`n`-dimensional co-ordinates :math:`\mathbf{x}\in\mathbb{R}^{h×w×n}` by
transformation matrix :math:`\mathbf{f}\in\mathbb{R}^{m×n}`, to produce image of transformed co-ordinates
:math:`\mathbf{x}_{trans}\in\mathbb{R}^{h×w×m}`.\n
`[reference] <https://en.wikipedia.org/wiki/Matrix_multiplication>`_
:param coords: Co-ordinate image *[batch_shape,height,width,n]*
:type coords: array
:param trans: Transformation matrix *[batch_shape,m,n]*
:type trans: array
:param batch_shape: Shape of batch. Inferred from inputs if None.
:type batch_shape: sequence of ints, optional
:param image_dims: Image dimensions. Inferred from inputs if None.
:type image_dims: sequence of ints
:return: Transformed co-ordinate image *[batch_shape,height,width,m]*
"""
if batch_shape is None:
batch_shape = coords.shape[:-3]
if image_dims is None:
image_dims = coords.shape[-3:-1]
# shapes as list
batch_shape = list(batch_shape)
image_dims = list(image_dims)
# transpose idxs
num_batch_dims = len(batch_shape)
transpose_idxs = list(
range(num_batch_dims)) + [num_batch_dims + 1, num_batch_dims]
# BS x (HxW) x N
coords_flattened = _ivy.reshape(
coords, batch_shape + [image_dims[0] * image_dims[1], -1])
# BS x N x (HxW)
coords_reshaped = _ivy.transpose(coords_flattened, transpose_idxs)
# BS x M x (HxW)
transformed_coords_vector = _ivy.matmul(trans, coords_reshaped)
# BS x (HxW) x M
transformed_coords_vector_transposed = _ivy.transpose(
transformed_coords_vector, transpose_idxs)
# BS x H x W x M
return _ivy.reshape(transformed_coords_vector_transposed,
batch_shape + image_dims + [-1])
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 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 concat(containers, dim, f=None):
"""
Concatenate containers together along the specified dimension.
:param containers: containers to _concatenate
:type containers: sequence of Container objects
:param dim: dimension along which to _concatenate
:type dim: int
:param f: Machine learning framework. Inferred from inputs if None.
:type f: ml_framework, optional
:return: Concatenated containers
"""
container0 = containers[0]
if isinstance(container0, dict):
return_dict = dict()
for key in container0.keys():
return_dict[key] = Container.concat([container[key] for container in containers], dim)
return Container(return_dict)
else:
f = _get_framework(container0, f=f)
# noinspection PyBroadException
try:
if len(containers[0].shape) == 0:
return _ivy.concatenate([_ivy.reshape(item, [1] * (dim + 1)) for item in containers], dim, f=f)
else:
return _ivy.concatenate(containers, dim, f=f)
except Exception as e:
raise Exception(str(e) + '\nContainer concat operation only valid for containers of arrays')
def main(interactive=True, try_use_sim=True, f=None):
f = choose_random_framework() if f is None else f
set_framework(f)
sim = Simulator(interactive, try_use_sim)
vis = Visualizer(ivy.to_numpy(sim.default_camera_ext_mat_homo))
pix_per_deg = 2
om_pix = sim.get_pix_coords()
plr_degs = om_pix / pix_per_deg
plr_rads = plr_degs * math.pi / 180
iterations = 10 if sim.with_pyrep else 1
for _ in range(iterations):
depth, rgb = sim.omcam.cap()
plr = ivy.concatenate([plr_rads, depth], -1)
xyz_wrt_cam = ivy_mech.polar_to_cartesian_coords(plr)
xyz_wrt_cam = ivy.reshape(xyz_wrt_cam, (-1, 3))
xyz_wrt_cam_homo = ivy_mech.make_coordinates_homogeneous(xyz_wrt_cam)
inv_ext_mat_trans = ivy.transpose(sim.omcam.get_inv_ext_mat(), (1, 0))
xyz_wrt_world = ivy.matmul(xyz_wrt_cam_homo, inv_ext_mat_trans)[..., 0:3]
with ivy.numpy.use:
omni_cam_inv_ext_mat = ivy_mech.make_transformation_homogeneous(
ivy.to_numpy(sim.omcam.get_inv_ext_mat()))
vis.show_point_cloud(xyz_wrt_world, rgb, interactive,
sphere_inv_ext_mats=[omni_cam_inv_ext_mat], sphere_radii=[0.025])
if not interactive:
sim.omcam.set_pos(sim.omcam.get_pos()
+ ivy.array([-0.01, 0.01, 0.]))
sim.close()
unset_framework()
def rot_mat_and_cam_center_to_ext_mat(rotation_mat, camera_center, batch_shape=None):
"""
Get extrinsic matrix :math:`\mathbf{E}\in\mathbb{R}^{3×4}` from rotation matrix
:math:`\mathbf{R}\in\mathbb{R}^{3×3}` and camera centers :math:`\overset{\sim}{\mathbf{C}}\in\mathbb{R}^{3×1}`.\n
`[reference] <localhost:63342/ivy/docs/source/references/mvg_textbook.pdf#page=175>`_
page 157, section 6.1, equation 6.11
:param rotation_mat: Rotation matrix *[batch_shape,3,3]*
:type rotation_mat: array
:param camera_center: Camera center *[batch_shape,3,1]*
:type camera_center: array
:param batch_shape: Shape of batch. Inferred from inputs if None.
:type batch_shape: sequence of ints, optional
:return: Extrinsic matrix *[batch_shape,3,4]*
"""
if batch_shape is None:
batch_shape = rotation_mat.shape[:-2]
# shapes as list
batch_shape = list(batch_shape)
# num batch dims
num_batch_dims = len(batch_shape)
# BS x 3 x 3
identity = _ivy.tile(_ivy.reshape(_ivy.identity(3), [1] * num_batch_dims + [3, 3]),
batch_shape + [1, 1])
# BS x 3 x 4
identity_w_cam_center = _ivy.concatenate((identity, -camera_center), -1)
# BS x 3 x 4
return _ivy.matmul(rotation_mat, identity_w_cam_center)
def make_transformation_homogeneous(matrices, batch_shape=None, dev_str=None):
"""
Append to set of 3x4 non-homogeneous matrices to make them homogeneous.
:param matrices: set of 3x4 non-homogeneous matrices *[batch_shape,3,4]*
:type matrices: 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: 4x4 Homogeneous matrices *[batch_shape,4,4]*
"""
if batch_shape is None:
batch_shape = matrices.shape[:-2]
if dev_str is None:
dev_str = _ivy.dev_str(matrices)
# shapes as list
batch_shape = list(batch_shape)
num_batch_dims = len(batch_shape)
# BS x 1 x 4
last_row = _ivy.tile(
_ivy.reshape(_ivy.array([0., 0., 0., 1.], dev_str=dev_str),
[1] * num_batch_dims + [1, 4]), batch_shape + [1, 1])
# BS x 4 x 4
return _ivy.concatenate((matrices, last_row), -2)
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 concat(containers, dim):
"""
Concatenate containers together along the specified dimension.
:param containers: containers to concatenate
:type containers: sequence of Container objects
:param dim: dimension along which to concatenate
:type dim: int
:return: Concatenated containers
"""
container0 = containers[0]
if isinstance(container0, dict):
return_dict = dict()
for key in container0.keys():
return_dict[key] = Container.concat(
[container[key] for container in containers], dim)
return Container(return_dict)
else:
# noinspection PyBroadException
try:
if len(containers[0].shape) == 0:
return _ivy.concatenate([
_ivy.reshape(item, [1] * (dim + 1))
for item in containers
], dim)
else:
return _ivy.concatenate(containers, dim)
except Exception as e:
raise Exception(
str(e) +
'\nContainer concat operation only valid for containers of arrays'
)
def measure_incremental_mat(self):
inv_ext_mat = ivy.reshape(ivy.array(self._handle.get_matrix()), (3, 4))
inv_ext_mat_homo = ivy_mech.make_transformation_homogeneous(inv_ext_mat)
ext_mat_homo = ivy.inv(inv_ext_mat_homo)
ext_mat = ext_mat_homo[0:3, :]
rel_mat = ivy.matmul(ext_mat, self._inv_ext_mat_homo)
self._inv_ext_mat_homo = inv_ext_mat_homo
return rel_mat
def get_reward(self):
"""
Get reward based on current state
:return: Reward array
"""
# Goal proximity.
return ivy.reshape(ivy.exp(-5 * ((self.x - self.goal_x) ** 2)), (1,))
def erosion(tensor: ivy.Array, kernel: ivy.Array) -> ivy.Array:
r"""Returns the eroded image applying the same kernel in each channel.
The kernel must have 2 dimensions, each one defined by an odd number.
Args:
tensor (ivy.Array): Image with shape :math:`(B, C, H, W)`.
kernel (ivy.Array): Structuring element with shape :math:`(H, W)`.
Returns:
ivy.Array: Eroded image with shape :math:`(B, C, H, W)`.
Example:
>>> tensor = ivy.random_uniform(shape=(1, 3, 5, 5))
>>> kernel = ivy.ones((5, 5))
>>> output = erosion(tensor, kernel)
"""
if ivy.backend == 'torch' and not isinstance(tensor, ivy.Array):
raise TypeError("Input type is not an ivy.Array. Got {}".format(type(tensor)))
if len(tensor.shape) != 4:
raise ValueError("Input size must have 4 dimensions. Got {}".format(
ivy.get_num_dims(tensor)))
if ivy.backend == 'torch' and not isinstance(kernel, ivy.Array):
raise TypeError("Kernel type is not a ivy.Array. Got {}".format(type(kernel)))
if len(kernel.shape) != 2:
raise ValueError("Kernel size must have 2 dimensions. Got {}".format(
ivy.get_num_dims(kernel)))
# prepare kernel
se_e: ivy.Array = kernel - 1.
kernel_e: ivy.Array = ivy.transpose(_se_to_mask(se_e), (2, 3, 1, 0))
# pad
se_h, se_w = kernel.shape
pad_e: List[List[int]] = [[0]*2, [0]*2, [se_h // 2, se_w // 2], [se_h // 2, se_w // 2]]
output: ivy.Array = ivy.reshape(tensor,
(tensor.shape[0] * tensor.shape[1], 1, tensor.shape[2], tensor.shape[3]))
output = ivy.constant_pad(output, pad_e, value=1.)
output = ivy.reduce_min(ivy.conv2d(output, kernel_e, 1, 'VALID', data_format='NCHW') -
ivy.reshape(se_e, (1, -1, 1, 1)), [1])
return ivy.reshape(output, tensor.shape)
def get_reward(self):
"""
Get reward based on current state
:return: Reward array
"""
# Pole verticality.
rew = (ivy.cos(self.angle) + 1) / 2
return ivy.reshape(rew, (1, ))
def _pad_to_batch_n_time_dims(data, expected_dims):
# ToDo: remove this once ESM supports arbitrary batch dimensions
found_dims = len(data.shape)
if found_dims == expected_dims:
return data
elif found_dims < expected_dims:
return ivy.reshape(data, [1]*(expected_dims - found_dims) + list(data.shape))
else:
raise Exception('found more dims {} than expected {}'.format(found_dims, expected_dims))
def dilation(tensor: ivy.Array, kernel: ivy.Array) -> ivy.Array:
r"""Returns the dilated image applying the same kernel in each channel.
The kernel must have 2 dimensions, each one defined by an odd number.
Args:
tensor (ivy.Array): Image with shape :math:`(B, C, H, W)`.
kernel (ivy.Array): Structuring element with shape :math:`(H, W)`.
Returns:
ivy.Array: Dilated image with shape :math:`(B, C, H, W)`.
Example:
>>> tensor = ivy.random_uniform(shape=(1, 3, 5, 5))
>>> kernel = ivy.ones((3, 3))
>>> dilated_img = dilation(tensor, kernel)
"""
if ivy.backend == 'torch' and not isinstance(tensor, ivy.Array):
raise TypeError("Input type is not an ivy.Array. Got {}".format(type(tensor)))
if len(tensor.shape) != 4:
raise ValueError("Input size must have 4 dimensions. Got {}".format(
ivy.get_num_dims(tensor)))
if ivy.backend == 'torch' and not isinstance(kernel, ivy.Array):
raise TypeError("Kernel type is not a ivy.Array. Got {}".format(type(kernel)))
if len(kernel.shape) != 2:
raise ValueError("Kernel size must have 2 dimensions. Got {}".format(
ivy.get_num_dims(kernel)))
# prepare kernel
se_d: ivy.Array = kernel - 1.
kernel_d: ivy.Array = ivy.transpose(_se_to_mask(se_d), (2, 3, 1, 0))
# pad
se_h, se_w = kernel.shape
output: ivy.Array = ivy.reshape(tensor,
(tensor.shape[0] * tensor.shape[1], 1, tensor.shape[2], tensor.shape[3]))
output = ivy.reduce_max(ivy.conv2d(output, kernel_d, 1, 'SAME', data_format='NCHW') +
ivy.reshape(se_d, (1, -1, 1, 1)), [1])
return ivy.reshape(output, tensor.shape)
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, interactive, try_use_sim):
super().__init__(interactive, try_use_sim)
# initialize scene
if self.with_pyrep:
for i in range(6):
self._vision_sensors[i].remove()
self._vision_sensor_bodies[i].remove()
[item.remove() for item in self._vision_sensor_rays[i]]
self._default_camera.set_position(np.array([-2.3518, 4.3953, 2.8949]))
self._default_camera.set_orientation(np.array([i*np.pi/180 for i in [112.90, 27.329, -10.978]]))
inv_ext_mat = ivy.reshape(ivy.array(self._default_vision_sensor.get_matrix(), 'float32'), (3, 4))
self.default_camera_ext_mat_homo = ivy.inv(ivy_mech.make_transformation_homogeneous(inv_ext_mat))
# public objects
self.omcam = SimCam(self._spherical_vision_sensor)
# wait for user input
self._user_prompt('\nInitialized scene with an omni-directional camera in the centre.\n\n'
'You can click on the omni directional camera, which appears as a small floating black sphere, '
'then select the box icon with four arrows in the top panel of the simulator, '
'and then drag the camera around dynamically.\n'
'Starting to drag and then holding ctrl allows you to also drag the camera up and down. \n\n'
'This demo enables you to capture 10 different omni-directional images from the camera, '
'and render the associated 10 point clouds in an open3D visualizer.\n\n'
'Both visualizers can be translated and rotated by clicking either the left mouse button or the wheel, '
'and then dragging the mouse.\n'
'Scrolling the mouse wheel zooms the view in and out.\n\n'
'Both visualizers can be rotated and zoomed by clicking either the left mouse button or the wheel, '
'and then dragging with the mouse.\n\n'
'Press enter in the terminal to use method ivy_mech.polar_coords_to_cartesian_coords and '
'show the first cartesian point cloud reconstruction of the scene, '
'converted from the polar co-ordinates captured from the omni-directional camera.\n\n')
else:
# public objects
self.omcam = DummyOmCam()
self.default_camera_ext_mat_homo = ivy.array(
[[-0.872, -0.489, 0., 0.099],
[-0.169, 0.301, -0.938, 0.994],
[0.459, -0.818, -0.346, 5.677],
[0., 0., 0., 1.]])
# message
print('\nInitialized dummy scene with an omni-directional camera in the centre.'
'\nClose the visualization window to use method ivy_mech.polar_coords_to_cartesian_coords and show'
'a cartesian point cloud reconstruction of the scene, '
'converted from the omni-directional camera polar co-ordinates\n')
# plot scene before rotation
if interactive:
plt.imshow(mpimg.imread(os.path.join(os.path.dirname(os.path.realpath(__file__)),
'ptcc_no_sim', 'before_capture.png')))
plt.show()
def sample_body(self, inv_ext_mats, batch_shape=None):
"""
Sample links of the robot at uniformly distributed cartesian positions.
:param inv_ext_mats: Inverse extrinsic matrices *[batch_shape,3,4]*
:type inv_ext_mats: array
:param batch_shape: Shape of batch. Inferred from inputs if None.
:type batch_shape: sequence of ints, optional
:return: The sampled body cartesian positions, in the world reference frame *[batch_shape,num_body_points,3]*
"""
if batch_shape is None:
batch_shape = inv_ext_mats.shape[:-2]
batch_shape = list(batch_shape)
# (BSx3) x NBP
body_points_trans = _ivy.matmul(_ivy.reshape(inv_ext_mats, (-1, 4)), self._rel_body_points_homo_trans)
# BS x NBP x 3
return _ivy.swapaxes(_ivy.reshape(body_points_trans, batch_shape + [3, -1]), -1, -2)
def get_reward(self):
"""
Get reward based on current state
:return: Reward array
"""
# Center proximity.
rew = ivy.exp(-1 * (self.x**2))
# Pole verticality.
rew = rew * (ivy.cos(self.angle) + 1) / 2
return ivy.reshape(rew[0], (1, ))
def bilinearly_interpolate_image(image, sampling_pixel_coords, batch_shape=None, image_dims=None):
"""
Bilinearly interpolate image :math:`\mathbf{X}\in\mathbb{R}^{h×w×d}` at sampling pixel locations
:math:`\mathbf{S}\in\mathbb{R}^{h×w×2}`, to return interpolated image :math:`\mathbf{X}_I\in\mathbb{R}^{h×w×d}`.\n
`[reference] <https://en.wikipedia.org/wiki/Bilinear_interpolation>`_
:param image: Image to be interpolated *[batch_shape,h,w,d]*
:type image: array
:param sampling_pixel_coords: Pixel co-ordinates to sample the image at *[batch_shape,h,w,2]*
:type sampling_pixel_coords: array
:param batch_shape: Shape of batch. Inferred from inputs if None.
:type batch_shape: sequence of ints, optional
:param image_dims: Image dimensions. Inferred from inputs in None.
:type image_dims: sequence of ints, optional
:return: Interpolated image *[batch_shape,h,w,d]*
"""
if batch_shape is None:
batch_shape = image.shape[:-3]
if image_dims is None:
image_dims = image.shape[-3:-1]
# shapes as list
batch_shape = list(batch_shape)
image_dims = list(image_dims)
# batch shape product
batch_shape_product = _reduce(_mul, batch_shape, 1)
# prod(BS) x H x W x D
uniform_values_flat = _ivy.reshape(image, [batch_shape_product] + image_dims + [-1])
# prod(BS) x H x W x 2
sampling_pixel_coords_flat = _ivy.reshape(sampling_pixel_coords, [batch_shape_product] + image_dims + [2])
# prod(BS) x H x W x D
interpolation_flat = _ivy.bilinear_resample(uniform_values_flat, sampling_pixel_coords_flat)
# BS x H x W x D
return _ivy.reshape(interpolation_flat, batch_shape + image_dims + [-1])
def get_reward(self):
"""
Get reward based on current state
:return: Reward array
"""
# Goal proximity.
rew = ivy.exp(-0.5 * ivy.reduce_sum((self.xy - self.goal_xy)**2, -1))
# Urchins proximity.
rew = rew * ivy.reduce_prod(
1 - ivy.exp(-30 * ivy.reduce_sum(
(self.xy - self.urchin_xys)**2, -1)), -1)
return ivy.reshape(rew, (1, ))
def main(interactive=True, try_use_sim=True, f=None):
f = choose_random_framework() if f is None else f
set_framework(f)
sim = Simulator(interactive, try_use_sim)
vis = Visualizer(ivy.to_numpy(sim.default_camera_ext_mat_homo))
xyzs = list()
rgbs = list()
iterations = 10 if sim.with_pyrep else 1
for _ in range(iterations):
for cam in sim.cams:
depth, rgb = cam.cap()
xyz = sim.depth_to_xyz(depth, cam.get_inv_ext_mat(),
cam.inv_calib_mat, [128] * 2)
xyzs.append(xyz)
rgbs.append(rgb)
xyz = ivy.reshape(ivy.concatenate(xyzs, 1), (-1, 3))
rgb = ivy.reshape(ivy.concatenate(rgbs, 1), (-1, 3))
voxels = [
ivy.to_numpy(item)
for item in ivy_vision.coords_to_voxel_grid(xyz, [100] * 3,
features=rgb)
]
cuboid_inv_ext_mats = [
ivy.to_numpy(
ivy_mech.make_transformation_homogeneous(
cam.get_inv_ext_mat())) for cam in sim.cams
]
with ivy.numpy.use:
vis.show_voxel_grid(voxels,
interactive,
cuboid_inv_ext_mats=cuboid_inv_ext_mats,
cuboid_dims=[
np.array([0.045, 0.045, 0.112])
for _ in sim.cams
])
xyzs.clear()
rgbs.clear()
sim.close()
unset_framework()
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:])
