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 get_fundamental_matrix(full_mat1,
full_mat2,
camera_center1=None,
pinv_full_mat1=None,
batch_shape=None,
dev_str=None):
"""
Compute fundamental matrix :math:`\mathbf{F}\in\mathbb{R}^{3×3}` between two cameras, given their extrinsic
matrices :math:`\mathbf{E}_1\in\mathbb{R}^{3×4}` and :math:`\mathbf{E}_2\in\mathbb{R}^{3×4}`.\n
`[reference] <localhost:63342/ivy/docs/source/references/mvg_textbook.pdf#page=262>`_
bottom of page 244, section 9.2.2, equation 9.1
:param full_mat1: Frame 1 full projection matrix *[batch_shape,3,4]*
:type full_mat1: array
:param full_mat2: Frame 2 full projection matrix *[batch_shape,3,4]*
:type full_mat2: array
:param camera_center1: Frame 1 camera center, inferred from full_mat1 if None *[batch_shape,3,1]*
:type camera_center1: array, optional
:param pinv_full_mat1: Frame 1 full projection matrix pseudo-inverse, inferred from full_mat1 if None *[batch_shape,4,3]*
:type pinv_full_mat1: array, optional
: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: Fundamental matrix connecting frames 1 and 2 *[batch_shape,3,3]*
"""
if batch_shape is None:
batch_shape = full_mat1.shape[:-2]
if dev_str is None:
dev_str = _ivy.dev_str(full_mat1)
# shapes as list
batch_shape = list(batch_shape)
if camera_center1 is None:
inv_full_mat1 = _ivy.inv(
_ivy_mech.make_transformation_homogeneous(full_mat1, batch_shape,
dev_str))[..., 0:3, :]
camera_center1 = _ivy_svg.inv_ext_mat_to_camera_center(inv_full_mat1)
if pinv_full_mat1 is None:
pinv_full_mat1 = _ivy.pinv(full_mat1)
# BS x 4 x 1
camera_center1_homo = _ivy.concatenate(
(camera_center1, _ivy.ones(batch_shape + [1, 1], dev_str=dev_str)), -2)
# BS x 3
e2 = _ivy.matmul(full_mat2, camera_center1_homo)[..., -1]
# BS x 3 x 3
e2_skew_symmetric = _ivy.linalg.vector_to_skew_symmetric_matrix(e2)
# BS x 3 x 3
return _ivy.matmul(e2_skew_symmetric, _ivy.matmul(full_mat2,
pinv_full_mat1))
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 ds_pixel_to_ds_pixel_coords(ds_pixel_coords1,
cam1to2_full_mat,
batch_shape=None,
image_dims=None,
dev_str=None):
"""
Transform depth scaled homogeneous pixel co-ordinates image in first camera frame
:math:`\mathbf{X}_{p1}\in\mathbb{R}^{h×w×3}` to depth scaled homogeneous pixel co-ordinates image in second camera
frame :math:`\mathbf{X}_{p2}\in\mathbb{R}^{h×w×3}`, given camera to camera projection matrix
:math:`\mathbf{P}_{1→2}\in\mathbb{R}^{3×4}`.\n
`[reference] <localhost:63342/ivy/docs/source/references/mvg_textbook.pdf#page=174>`_
:param ds_pixel_coords1: Depth scaled homogeneous pixel co-ordinates image in frame 1 *[batch_shape,h,w,3]*
:type ds_pixel_coords1: array
:param cam1to2_full_mat: Camera1-to-camera2 full projection matrix *[batch_shape,3,4]*
:type cam1to2_full_mat: 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
: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: Depth scaled homogeneous pixel co-ordinates image in frame 2 *[batch_shape,h,w,3]*
"""
if batch_shape is None:
batch_shape = ds_pixel_coords1.shape[:-3]
if image_dims is None:
image_dims = ds_pixel_coords1.shape[-3:-1]
if dev_str is None:
dev_str = _ivy.dev_str(ds_pixel_coords1)
# shapes as list
batch_shape = list(batch_shape)
image_dims = list(image_dims)
# BS x H x W x 4
pixel_coords_homo = _ivy.concatenate(
(ds_pixel_coords1,
_ivy.ones(batch_shape + image_dims + [1], dev_str=dev_str)), -1)
# BS x H x W x 3
return _ivy_pg.transform(pixel_coords_homo, cam1to2_full_mat, batch_shape,
image_dims)
def cam_to_cam_coords(cam_coords1,
cam1to2_ext_mat,
batch_shape=None,
image_dims=None,
dev_str=None):
"""
Transform camera-centric homogeneous co-ordinates image for camera 1 :math:`\mathbf{X}_{c1}\in\mathbb{R}^{h×w×4}` to
camera-centric homogeneous co-ordinates image for camera 2 :math:`\mathbf{X}_{c2}\in\mathbb{R}^{h×w×4}`.\n
`[reference] <localhost:63342/ivy/docs/source/references/mvg_textbook.pdf#page=174>`_
:param cam_coords1: Camera-centric homogeneous co-ordinates image in frame 1 *[batch_shape,h,w,4]*
:type cam_coords1: array
:param cam1to2_ext_mat: Camera1-to-camera2 extrinsic projection matrix *[batch_shape,3,4]*
:type cam1to2_ext_mat: 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
: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: Depth scaled homogeneous pixel co-ordinates image in frame 2 *[batch_shape,h,w,3]*
"""
if batch_shape is None:
batch_shape = cam_coords1.shape[:-3]
if image_dims is None:
image_dims = cam_coords1.shape[-3:-1]
if dev_str is None:
dev_str = _ivy.dev_str(cam_coords1)
# shapes as list
batch_shape = list(batch_shape)
image_dims = list(image_dims)
# BS x H x W x 3
cam_coords2 = _ivy_pg.transform(cam_coords1, cam1to2_ext_mat, batch_shape,
image_dims)
# BS x H x W x 4
return _ivy.concatenate(
(cam_coords2, _ivy.ones(batch_shape + image_dims + [1],
dev_str=dev_str)), -1)
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 ds_pixel_to_world_coords(ds_pixel_coords, inv_full_mat, batch_shape=None, image_dims=None, dev_str=None):
"""
Get world-centric homogeneous co-ordinates image :math:`\mathbf{X}_w\in\mathbb{R}^{h×w×4}` from depth scaled
homogeneous pixel co-ordinates image :math:`\mathbf{X}_p\in\mathbb{R}^{h×w×3}`.\n
`[reference] <localhost:63342/ivy/docs/source/references/mvg_textbook.pdf#page=173>`_
combination of page 155, matrix inverse of equation 6.3, and matrix inverse of page 156, equation 6.6
:param ds_pixel_coords: Depth scaled homogeneous pixel co-ordinates image: *[batch_shape,h,w,3]*
:type ds_pixel_coords: array
:param inv_full_mat: Inverse full projection matrix *[batch_shape,3,4]*
:type inv_full_mat: 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
: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: World-centric homogeneous co-ordinates image *[batch_shape,h,w,4]*
"""
if batch_shape is None:
batch_shape = ds_pixel_coords.shape[:-3]
if image_dims is None:
image_dims = ds_pixel_coords.shape[-3:-1]
if dev_str is None:
dev_str = _ivy.dev_str(ds_pixel_coords)
# shapes as list
batch_shape = list(batch_shape)
image_dims = list(image_dims)
# BS x H x W x 4
ds_pixel_coords = _ivy.concatenate((ds_pixel_coords, _ivy.ones(batch_shape + image_dims + [1], dev_str=dev_str)), -1)
# BS x H x W x 3
world_coords = _ivy_pg.transform(ds_pixel_coords, inv_full_mat, batch_shape, image_dims)
# BS x H x W x 4
return _ivy.concatenate((world_coords, _ivy.ones(batch_shape + image_dims + [1], dev_str=dev_str)), -1)
def sphere_to_cam_coords(sphere_coords, forward_facing_z=True, batch_shape=None, image_dims=None, dev_str=None):
"""
Convert camera-centric ego-sphere polar co-ordinates image :math:`\mathbf{S}_c\in\mathbb{R}^{h×w×3}` to
camera-centric homogeneous cartesian co-ordinates image :math:`\mathbf{X}_c\in\mathbb{R}^{h×w×4}`.\n
`[reference] <https://en.wikipedia.org/wiki/Spherical_coordinate_system#Cartesian_coordinates>`_
:param sphere_coords: Camera-centric ego-sphere polar co-ordinates image *[batch_shape,h,w,3]*
:type sphere_coords: array
:param forward_facing_z: Whether to use reference frame so z is forward facing. Default is False.
:type forward_facing_z: bool, optional
: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
: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: *Camera-centric homogeneous cartesian co-ordinates image *[batch_shape,h,w,4]*
"""
if batch_shape is None:
batch_shape = sphere_coords.shape[:-3]
if image_dims is None:
image_dims = sphere_coords.shape[-3:-1]
if dev_str is None:
dev_str = _ivy.dev_str(sphere_coords)
# shapes as list
batch_shape = list(batch_shape)
image_dims = list(image_dims)
# BS x H x W x 3
cam_coords = _ivy_mec.polar_to_cartesian_coords(sphere_coords)
if forward_facing_z:
cam_coords = _ivy.concatenate(
(cam_coords[..., 1:2], cam_coords[..., 2:3], cam_coords[..., 0:1]), -1)
# BS x H x W x 4
return _ivy.concatenate((cam_coords, _ivy.ones(batch_shape + image_dims + [1], dev_str=dev_str)), -1)
def cam_to_world_coords(coords_wrt_cam, inv_ext_mat, batch_shape=None, image_dims=None, dev_str=None):
"""
Get world-centric homogeneous co-ordinates image :math:`\mathbf{X}_w\in\mathbb{R}^{h×w×4}` from camera-centric
homogeneous co-ordinates image :math:`\mathbf{X}_c\in\mathbb{R}^{h×w×4}`.\n
`[reference] <localhost:63342/ivy/docs/source/references/mvg_textbook.pdf#page=174>`_
matrix inverse of page 156, equation 6.6
:param coords_wrt_cam: Camera-centric homogeneous co-ordinates image *[batch_shape,h,w,4]*
:type coords_wrt_cam: array
:param inv_ext_mat: Inverse extrinsic matrix *[batch_shape,3,4]*
:type inv_ext_mat: 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
: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: World-centric homogeneous co-ordinates image *[batch_shape,h,w,4]*
"""
if batch_shape is None:
batch_shape = coords_wrt_cam.shape[:-3]
if image_dims is None:
image_dims = coords_wrt_cam.shape[-3:-1]
if dev_str is None:
dev_str = _ivy.dev_str(coords_wrt_cam)
# shapes as list
batch_shape = list(batch_shape)
image_dims = list(image_dims)
# BS x H x W x 3
world_coords = _ivy_pg.transform(coords_wrt_cam, inv_ext_mat, batch_shape, image_dims)
# BS x H x W x 4
return _ivy.concatenate((world_coords, _ivy.ones(batch_shape + image_dims + [1], dev_str=dev_str)), -1)
def __init__(self,
img_meas: Dict[str, ESMCamMeasurement],
agent_rel_mat: ivy.Array,
control_mean: ivy.Array = None,
control_cov: ivy.Array = None,
dev_str: str = 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
:param dev_str: Device string to use, default is to use img_mean.
:type dev_str: str
"""
if dev_str is None:
dev_str = ivy.dev_str(agent_rel_mat)
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, dev_str=dev_str), -1), (1, 1, 1, 6))
else:
control_cov = _pad_to_batch_n_time_dims(control_cov, 4)
self['control_cov'] = control_cov
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 persp_angles_to_focal_lengths(persp_angles, image_dims, dev_str=None):
"""
Compute focal lengths :math:`f_x, f_y` from perspective angles :math:`θ_x, θ_y`.\n
`[reference] <localhost:63342/ivy/docs/source/references/mvg_textbook.pdf#page=172>`_
deduction from page 154, section 6.1, figure 6.1
:param persp_angles: Perspective angles *[batch_shape,2]*
:type persp_angles: array
: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. Same as x if None.
:type dev_str: str, optional
:return: Focal lengths *[batch_shape,2]*
"""
if dev_str is None:
dev_str = _ivy.dev_str(persp_angles)
# shapes as list
image_dims = list(image_dims)
# BS x 2
return -_ivy.flip(_ivy.cast(_ivy.array(image_dims, dev_str=dev_str), 'float32'), -1) /\
(2 * _ivy.tan(persp_angles / 2) + MIN_DENOMINATOR)
def variable(x):
with _tf.device('/' + ivy.dev_str(x).upper()):
return _tf.Variable(x, trainable=True)
def velocity_from_cam_coords_id_image_and_object_trans(cam_coords_t,
id_image,
obj_ids,
obj_trans,
delta_t,
batch_shape=None,
image_dims=None,
dev_str=None):
"""
Compute velocity image from co-ordinate image, id image, and object transformations.
:param cam_coords_t: Camera-centric homogeneous co-ordinates image in frame t *[batch_shape,h,w,4]*
:type cam_coords_t: array
:param id_image: Image containing per-pixel object ids *[batch_shape,h,w,1]*
:type id_image: array
:param obj_ids: Object ids *[batch_shape,num_obj,1]*
:type obj_ids: array
:param obj_trans: Object transformations for this frame over time *[batch_shape,num_obj,3,4]*
:type obj_trans: array
:param delta_t: Time difference between frame at timestep t-1 and t *[batch_shape,1]*
:type delta_t: 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
: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: Relative velocity image *[batch_shape,h,w,3]*
"""
if batch_shape is None:
batch_shape = cam_coords_t.shape[:-3]
if image_dims is None:
image_dims = cam_coords_t.shape[-3:-1]
if dev_str is None:
dev_str = _ivy.dev_str(cam_coords_t)
# shapes as list
batch_shape = list(batch_shape)
image_dims = list(image_dims)
# get co-ordinate re-projections
# BS x H x W x 4
cam_coords_t_all_trans, motion_mask =\
project_cam_coords_with_object_transformations(cam_coords_t, id_image, obj_ids, obj_trans,
_ivy.identity(4, batch_shape=batch_shape)[..., 0:3, :],
batch_shape, image_dims)
# BS x H x W x 4
cam_coords_t_all_trans = \
_ivy.where(motion_mask, cam_coords_t_all_trans, _ivy.zeros_like(cam_coords_t_all_trans, dev_str=dev_str))
# compute velocities
# BS x H x W x 3
vel = (cam_coords_t[..., 0:3] - cam_coords_t_all_trans[..., 0:3]) / delta_t
# prune velocities
# BS x H x W x 3
return _ivy.where(motion_mask, vel, _ivy.zeros_like(vel, dev_str=dev_str))
def velocity_from_flow_cam_coords_and_cam_mats(flow_t_to_tm1,
cam_coords_t,
cam_coords_tm1,
cam_tm1_to_t_ext_mat,
delta_t,
uniform_pixel_coords=None,
batch_shape=None,
image_dims=None,
dev_str=None):
"""
Compute relative cartesian velocity from optical flow, camera co-ordinates, and camera extrinsics.
:param flow_t_to_tm1: Optical flow from frame t to t-1 *[batch_shape,h,w,2]*
:type flow_t_to_tm1: array
:param cam_coords_t: Camera-centric homogeneous co-ordinates image in frame t *[batch_shape,h,w,4]*
:type cam_coords_t: array
:param cam_coords_tm1: Camera-centric homogeneous co-ordinates image in frame t-1 *[batch_shape,h,w,4]*
:type cam_coords_tm1: array
:param cam_tm1_to_t_ext_mat: Camera t-1 to camera t extrinsic projection matrix *[batch_shape,3,4]*
:type cam_tm1_to_t_ext_mat: array
:param delta_t: Time difference between frame at timestep t-1 and t *[batch_shape,1]*
:type delta_t: array
:param uniform_pixel_coords: Homogeneous uniform (integer) pixel co-ordinate images, inferred from image_dims if None *[batch_shape,h,w,3]*
:type uniform_pixel_coords: array, optional
: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
: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: Cartesian velocity measurements relative to the camera *[batch_shape,h,w,3]*
"""
if batch_shape is None:
batch_shape = flow_t_to_tm1.shape[:-3]
if image_dims is None:
image_dims = flow_t_to_tm1.shape[-3:-1]
# shapes as list
batch_shape = list(batch_shape)
image_dims = list(image_dims)
if dev_str is None:
dev_str = _ivy.dev_str(flow_t_to_tm1)
if uniform_pixel_coords is None:
uniform_pixel_coords = _ivy_svg.create_uniform_pixel_coords_image(
image_dims, batch_shape, dev_str)
# Interpolate cam coords from frame t-1
# BS x H x W x 2
warp = uniform_pixel_coords[..., 0:2] + flow_t_to_tm1
# BS x H x W x 4
cam_coords_tm1_interp = _ivy.image.bilinear_resample(cam_coords_tm1, warp)
# Project to frame t
# BS x H x W x 4
cam_coords_t_proj = _ivy_tvg.cam_to_cam_coords(cam_coords_tm1_interp,
cam_tm1_to_t_ext_mat,
batch_shape, image_dims)
# delta co-ordinates
# BS x H x W x 3
delta_cam_coords_t = (cam_coords_t - cam_coords_t_proj)[..., 0:3]
# velocity
# BS x H x W x 3
vel = delta_cam_coords_t / _ivy.reshape(delta_t, batch_shape + [1] * 3)
# Validity mask
# BS x H x W x 1
validity_mask = \
_ivy.reduce_sum(_ivy.cast(warp < _ivy.array([image_dims[1], image_dims[0]], 'float32', dev_str=dev_str),
'int32'), -1, keepdims=True) == 2
# pruned
# BS x H x W x 3, BS x H x W x 1
return _ivy.where(validity_mask, vel,
_ivy.zeros_like(vel, dev_str=dev_str)), validity_mask
def depth_from_flow_and_cam_mats(flow,
full_mats,
inv_full_mats=None,
camera_centers=None,
uniform_pixel_coords=None,
triangulation_method='cmp',
batch_shape=None,
image_dims=None,
dev_str=None):
"""
Compute depth map :math:`\mathbf{X}\in\mathbb{R}^{h×w×1}` in frame 1 using optical flow
:math:`\mathbf{U}_{1→2}\in\mathbb{R}^{h×w×2}` from frame 1 to 2, and the camera geometry.\n
:param flow: Optical flow from frame 1 to 2 *[batch_shape,h,w,2]*
:type flow: array
:param full_mats: Full projection matrices *[batch_shape,2,3,4]*
:type full_mats: array
:param inv_full_mats: Inverse full projection matrices, inferred from full_mats if None and 'cmp' triangulation method *[batch_shape,2,3,4]*
:type inv_full_mats: array, optional
:param camera_centers: Camera centers, inferred from inv_full_mats if None and 'cmp' triangulation method *[batch_shape,2,3,1]*
:type camera_centers: array, optional
:param uniform_pixel_coords: Homogeneous uniform (integer) pixel co-ordinate images, inferred from image_dims if None *[batch_shape,h,w,3]*
:type uniform_pixel_coords: array, optional
:param triangulation_method: Triangulation method, one of [cmp|dlt], for closest mutual points or homogeneous dlt approach, closest_mutual_points by default
:type triangulation_method: str, optional
: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
: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: Depth map in frame 1 *[batch_shape,h,w,1]*
"""
if batch_shape is None:
batch_shape = flow.shape[:-3]
if image_dims is None:
image_dims = flow.shape[-3:-1]
# shapes as list
batch_shape = list(batch_shape)
image_dims = list(image_dims)
if dev_str is None:
dev_str = _ivy.dev_str(flow)
if inv_full_mats is None:
inv_full_mats = _ivy.inv(
_ivy_mech.make_transformation_homogeneous(full_mats,
batch_shape + [2],
dev_str))[..., 0:3, :]
if camera_centers is None:
camera_centers = _ivy_svg.inv_ext_mat_to_camera_center(inv_full_mats)
if uniform_pixel_coords is None:
uniform_pixel_coords = _ivy_svg.create_uniform_pixel_coords_image(
image_dims, batch_shape, dev_str=dev_str)
# BS x H x W x 3
flow_homo = _ivy.concatenate(
(flow, _ivy.zeros(batch_shape + image_dims + [1], dev_str=dev_str)),
-1)
# BS x H x W x 3
transformed_pixel_coords = uniform_pixel_coords + flow_homo
# BS x 2 x H x W x 3
pixel_coords = _ivy.concatenate(
(_ivy.expand_dims(uniform_pixel_coords, -4),
_ivy.expand_dims(transformed_pixel_coords, -4)), -4)
# BS x H x W x 1
return _ivy_tvg.triangulate_depth(pixel_coords, full_mats, inv_full_mats,
camera_centers, triangulation_method,
batch_shape, image_dims)[..., -1:]
def coord_image_to_trimesh(coord_img, validity_mask=None, batch_shape=None, image_dims=None, dev_str=None):
"""
Create trimesh, with vertices and triangle indices, from co-ordinate image.
:param coord_img: Image of co-ordinates *[batch_shape,h,w,3]*
:type coord_img: array
:param validity_mask: Boolean mask of where the coord image contains valid values *[batch_shape,h,w,1]*
:type validity_mask: array, optional
: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
: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: Vertices *[batch_shape,(hxw),3]* amd Trimesh indices *[batch_shape,n,3]*
"""
if dev_str is None:
dev_str = _ivy.dev_str(coord_img)
if batch_shape is None:
batch_shape = _ivy.shape(coord_img)[:-3]
if image_dims is None:
image_dims = _ivy.shape(coord_img)[-3:-1]
# shapes as lists
batch_shape = list(batch_shape)
image_dims = list(image_dims)
# BS x (HxW) x 3
vertices = _ivy.reshape(coord_img, batch_shape + [image_dims[0] * image_dims[1], 3])
if validity_mask is not None:
# BS x H-1 x W-1 x 1
t00_validity = validity_mask[..., 0:image_dims[0] - 1, 0:image_dims[1] - 1, :]
t01_validity = validity_mask[..., 0:image_dims[0] - 1, 1:image_dims[1], :]
t02_validity = validity_mask[..., 1:image_dims[0], 0:image_dims[1] - 1, :]
t10_validity = validity_mask[..., 1:image_dims[0], 1:image_dims[1], :]
t11_validity = t01_validity
t12_validity = t02_validity
# BS x H-1 x W-1 x 1
t0_validity = _ivy.logical_and(t00_validity, _ivy.logical_and(t01_validity, t02_validity))
t1_validity = _ivy.logical_and(t10_validity, _ivy.logical_and(t11_validity, t12_validity))
# BS x (H-1xW-1)
t0_validity_flat = _ivy.reshape(t0_validity, batch_shape + [-1])
t1_validity_flat = _ivy.reshape(t1_validity, batch_shape + [-1])
# BS x 2x(H-1xW-1)
trimesh_index_validity = _ivy.concatenate((t0_validity_flat, t1_validity_flat), -1)
# BS x N
trimesh_valid_indices = _ivy.indices_where(trimesh_index_validity)
# BS x 2x(H-1xW-1) x 3
all_trimesh_indices = create_trimesh_indices_for_image(batch_shape, image_dims, dev_str)
# BS x N x 3
trimesh_indices = _ivy.gather_nd(all_trimesh_indices, trimesh_valid_indices)
else:
# BS x N=2x(H-1xW-1) x 3
trimesh_indices = create_trimesh_indices_for_image(batch_shape, image_dims)
# BS x (HxW) x 3, BS x N x 3
return vertices, trimesh_indices
def closest_mutual_points_along_two_skew_rays(camera_centers,
world_ray_vectors,
batch_shape=None,
image_dims=None,
dev_str=None):
"""
Compute closest mutual homogeneous co-ordinates :math:`\mathbf{x}_{1,i,j}\in\mathbb{R}^{4}` and
:math:`\mathbf{x}_{2,i,j}\in\mathbb{R}^{4}` along two world-centric rays
:math:`\overset{\sim}{\mathbf{C}_1} + λ_1\mathbf{rv}_{1,i,j}` and
:math:`\overset{\sim}{\mathbf{C}_2} + λ_2\mathbf{rv}_{2,i,j}`, for each index aligned pixel between two
world-centric ray vector images :math:`\mathbf{RV}_1\in\mathbb{R}^{h×w×3}` and
:math:`\mathbf{RV}_2\in\mathbb{R}^{h×w×3}`. The function returns two images of closest mutual homogeneous
co-ordinates :math:`\mathbf{X}_1\in\mathbb{R}^{h×w×4}` and :math:`\mathbf{X}_2\in\mathbb{R}^{h×w×4}`,
concatenated together into a single array.\n
`[reference] <https://math.stackexchange.com/questions/1414285/location-of-shortest-distance-between-two-skew-lines-in-3d>`_
second answer in forum
:param camera_centers: Camera center *[batch_shape,2,3,1]*
:type camera_centers: array
:param world_ray_vectors: World ray vectors *[batch_shape,2,h,w,3]*
:type world_ray_vectors: 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
: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: Closest mutual points image *[batch_shape,2,h,w,4]*
"""
if batch_shape is None:
batch_shape = world_ray_vectors.shape[:-4]
if image_dims is None:
image_dims = world_ray_vectors.shape[-3:-1]
if dev_str is None:
dev_str = _ivy.dev_str(camera_centers)
# shapes as list
batch_shape = list(batch_shape)
image_dims = list(image_dims)
# BS x 3 x 1
camera_center0 = camera_centers[..., 0, :, :]
camera_center1 = camera_centers[..., 1, :, :]
# BS x 1 x 1 x 3
cam1_to_cam2 = _ivy.reshape(camera_center1 - camera_center0,
batch_shape + [1, 1, 3])
cam2_to_cam1 = _ivy.reshape(camera_center0 - camera_center1,
batch_shape + [1, 1, 3])
# BS x 2 x H x W x 3
ds = world_ray_vectors
# BS x H x W x 3
ds0 = ds[..., 0, :, :, :]
ds1 = ds[..., 1, :, :, :]
n = _ivy.cross(ds0, ds1)
n1 = _ivy.cross(ds0, n)
n2 = _ivy.cross(ds1, n)
# BS x 1 x H x W
t1 = _ivy.expand_dims(
_ivy.reduce_sum(cam1_to_cam2 * n2, -1) /
(_ivy.reduce_sum(ds0 * n2, -1) + MIN_DENOMINATOR), -3)
t2 = _ivy.expand_dims(
_ivy.reduce_sum(cam2_to_cam1 * n1, -1) /
(_ivy.reduce_sum(ds1 * n1, -1) + MIN_DENOMINATOR), -3)
# BS x 2 x H x W
ts = _ivy.expand_dims(_ivy.concatenate((t1, t2), -3), -1)
# BS x 2 x H x W x 3
world_coords = _ivy.reshape(camera_centers[..., 0], batch_shape +
[2, 1, 1, 3]) + ts * world_ray_vectors
# BS x 2 x H x W x 4
return _ivy.concatenate(
(world_coords,
_ivy.ones(batch_shape + [2] + image_dims + [1], dev_str=dev_str)), -1)
def depth_to_ds_pixel_coords(depth, uniform_pixel_coords=None, batch_shape=None, image_dims=None):
"""
Get depth scaled homogeneous pixel co-ordinates image :math:`\mathbf{X}_p\in\mathbb{R}^{h×w×3}` from depth image
:math:`\mathbf{X}_d\in\mathbb{R}^{h×w×1}`.\n
:param depth: Depth image *[batch_shape,h,w,1]*
:type depth: array
:param uniform_pixel_coords: Image of homogeneous pixel co-ordinates. Created if None. *[batch_shape,h,w,3]*
:type uniform_pixel_coords: array, optional
: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: Depth scaled homogeneous pixel co-ordinates image *[batch_shape,h,w,3]*
"""
if batch_shape is None:
batch_shape = depth.shape[:-3]
if image_dims is None:
image_dims = depth.shape[-3:-1]
# shapes as list
batch_shape = list(batch_shape)
image_dims = list(image_dims)
# BS x H x W x 3
if uniform_pixel_coords is None:
uniform_pixel_coords = create_uniform_pixel_coords_image(image_dims, batch_shape, dev_str=_ivy.dev_str(depth))
# BS x H x W x 3
return uniform_pixel_coords * depth
def depth_to_radial_depth(depth, inv_calib_mat, uniform_pixel_coords=None, batch_shape=None, image_dims=None):
"""
Get radial depth image :math:`\mathbf{X}_{rd}\in\mathbb{R}^{h×w×1}` from depth image
:math:`\mathbf{X}_d\in\mathbb{R}^{h×w×1}`.\n
:param depth: Depth image *[batch_shape,h,w,1]*
:type depth: array
:param inv_calib_mat: Inverse calibration matrix *[batch_shape,3,3]*
:type inv_calib_mat: array
:param uniform_pixel_coords: Image of homogeneous pixel co-ordinates. Created if None. *[batch_shape,h,w,3]*
:type uniform_pixel_coords: array, optional
: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: Radial depth image *[batch_shape,h,w,1]*
"""
if batch_shape is None:
batch_shape = depth.shape[:-3]
if image_dims is None:
image_dims = depth.shape[-3:-1]
# shapes as list
batch_shape = list(batch_shape)
image_dims = list(image_dims)
# BS x H x W x 3
if uniform_pixel_coords is None:
uniform_pixel_coords = create_uniform_pixel_coords_image(image_dims, batch_shape, dev_str=_ivy.dev_str(depth))
# BS x H x W x 3
ds_pixel_coords = depth_to_ds_pixel_coords(depth, uniform_pixel_coords, batch_shape, image_dims)
# BS x H x W x 3
cam_coords = ds_pixel_to_cam_coords(ds_pixel_coords, inv_calib_mat, batch_shape, image_dims)[..., 0:3]
# BS x H x W x 1
return _ivy.reduce_sum(cam_coords**2, -1, keepdims=True)**0.5
def rasterize_triangles(pixel_coords_triangles, image_dims, batch_shape=None, dev_str=None):
"""
Rasterize image-projected triangles
based on: https://www.scratchapixel.com/lessons/3d-basic-rendering/rasterization-practical-implementation/rasterization-stage
and: https://www.scratchapixel.com/lessons/3d-basic-rendering/rasterization-practical-implementation/rasterization-practical-implementation
:param pixel_coords_triangles: Projected image-space triangles to be rasterized
*[batch_shape,input_size,3,3]*
:type pixel_coords_triangles: array
:param image_dims: Image dimensions.
:type image_dims: sequence of ints
: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: Rasterized triangles
"""
if batch_shape is None:
batch_shape = []
if dev_str is None:
dev_str = _ivy.dev_str(pixel_coords_triangles)
# shapes as list
batch_shape = list(batch_shape)
num_batch_dims = len(batch_shape)
image_dims = list(image_dims)
input_image_dims = pixel_coords_triangles.shape[num_batch_dims:-2]
input_image_dims_prod = _reduce(_mul, input_image_dims, 1)
# BS x 3 x 2
pixel_xy_coords = pixel_coords_triangles[..., 0:2]
# BS x 3 x 1
pixel_x_coords = pixel_coords_triangles[..., 0:1]
pixel_y_coords = pixel_coords_triangles[..., 1:2]
# 1
x_min = _ivy.reshape(_ivy.reduce_min(pixel_x_coords, keepdims=True), (-1,))
x_max = _ivy.reshape(_ivy.reduce_max(pixel_x_coords, keepdims=True), (-1,))
x_range = x_max - x_min
y_min = _ivy.reshape(_ivy.reduce_min(pixel_y_coords, keepdims=True), (-1,))
y_max = _ivy.reshape(_ivy.reduce_max(pixel_y_coords, keepdims=True), (-1,))
y_range = y_max - y_min
# 2
bbox = _ivy.concatenate((x_range, y_range), 0)
img_bbox_list = [int(item) for item in _ivy.to_list(_ivy.concatenate((y_range + 1, x_range + 1), 0))]
# BS x 2
v0 = pixel_xy_coords[..., 0, :]
v1 = pixel_xy_coords[..., 1, :]
v2 = pixel_xy_coords[..., 2, :]
tri_centres = (v0 + v1 + v2) / 3
# BS x 1
v0x = v0[..., 0:1]
v0y = v0[..., 1:2]
v1x = v1[..., 0:1]
v1y = v1[..., 1:2]
v2x = v2[..., 0:1]
v2y = v2[..., 1:2]
# BS x BBX x BBY x 2
uniform_sample_coords = _ivy_svg.create_uniform_pixel_coords_image(img_bbox_list, batch_shape)[..., 0:2]
P = _ivy.round(uniform_sample_coords + tri_centres - bbox / 2)
# BS x BBX x BBY x 1
Px = P[..., 0:1]
Py = P[..., 1:2]
v0v1_edge_func = ((Px - v0x) * (v1y - v0y) - (Py - v0y) * (v1x - v0x)) >= 0
v1v2_edge_func = ((Px - v1x) * (v2y - v1y) - (Py - v1y) * (v2x - v1x)) >= 0
v2v0_edge_func = ((Px - v2x) * (v0y - v2y) - (Py - v2y) * (v0x - v2x)) >= 0
edge_func = _ivy.logical_and(_ivy.logical_and(v0v1_edge_func, v1v2_edge_func), v2v0_edge_func)
batch_indices_list = list()
for i, batch_dim in enumerate(batch_shape):
# get batch shape
batch_dims_before = batch_shape[:i]
num_batch_dims_before = len(batch_dims_before)
batch_dims_after = batch_shape[i + 1:]
num_batch_dims_after = len(batch_dims_after)
# [batch_dim]
batch_indices = _ivy.arange(batch_dim, dtype_str='int32', dev_str=dev_str)
# [1]*num_batch_dims_before x batch_dim x [1]*num_batch_dims_after x 1 x 1
reshaped_batch_indices = _ivy.reshape(batch_indices, [1] * num_batch_dims_before + [batch_dim] +
[1] * num_batch_dims_after + [1, 1])
# BS x N x 1
tiled_batch_indices = _ivy.tile(reshaped_batch_indices, batch_dims_before + [1] + batch_dims_after +
[input_image_dims_prod * 9, 1])
batch_indices_list.append(tiled_batch_indices)
# BS x N x (num_batch_dims + 2)
all_indices = _ivy.concatenate(
batch_indices_list + [_ivy.cast(_ivy.flip(_ivy.reshape(P, batch_shape + [-1, 2]), -1),
'int32')], -1)
# offset uniform images
return _ivy.cast(_ivy.flip(_ivy.scatter_nd(_ivy.reshape(all_indices, [-1, num_batch_dims + 2]),
_ivy.reshape(_ivy.cast(edge_func, 'int32'), (-1, 1)),
batch_shape + image_dims + [1],
reduction='replace' if _ivy.backend == 'mxnd' else 'sum'), -3), 'bool')
def __init__(self,
img_mean: ivy.Array,
cam_rel_mat: ivy.Array = None,
img_var: ivy.Array = None,
validity_mask: ivy.Array = None,
pose_mean: ivy.Array = None,
pose_cov: ivy.Array = None,
dev_str: str = 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. Default is identity matrix
*[batch_size, timesteps, 3, 4]*
:type cam_rel_mat: array, optional
: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
:param dev_str: Device string to use, default is to use img_mean.
:type dev_str: str
"""
if dev_str is None:
dev_str = ivy.dev_str(img_mean)
img_mean = _pad_to_batch_n_time_dims(img_mean, 5)
self['img_mean'] = img_mean
if cam_rel_mat is None:
cam_rel_mat = ivy.identity(4, batch_shape=img_mean.shape[0:2], dev_str=dev_str)[..., 0:3, :]
else:
cam_rel_mat = _pad_to_batch_n_time_dims(cam_rel_mat, 4)
self['cam_rel_mat'] = cam_rel_mat
if img_var is None:
img_var = ivy.zeros_like(img_mean, dev_str=dev_str)
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], dev_str=dev_str)
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, dev_str=dev_str), -1), (1, 1, 1, 6))
else:
pose_cov = _pad_to_batch_n_time_dims(pose_cov, 4)
self['pose_cov'] = pose_cov
def coords_to_voxel_grid(coords,
voxel_shape_spec,
mode='DIMS',
coord_bounds=None,
features=None,
batch_shape=None,
dev_str=None):
"""
Create voxel grid :math:`\mathbf{X}_v\in\mathbb{R}^{x×y×z×(3+N+1)}` from homogeneous co-ordinates
:math:`\mathbf{X}_w\in\mathbb{R}^{num\_coords×4}`. Each voxel contains 3+N+1 values: the mean normalized
co-ordinate inside the voxel for the projected pixels with :math:`0 < x, y, z < 1`, N coordinte features (optional),
and also the number of projected pixels inside the voxel.
Grid resolutions and dimensions are also returned separately for each entry in the batch.
Note that the final batched voxel grid returned uses the maximum grid dimensions across the batch, this means
some returned grids may contain redundant space, with all but the single largest batched grid occupying a subset
of the grid space, originating from the corner of minimum :math:`x,y,z` values.\n
`[reference] <https://en.wikipedia.org/wiki/Voxel>`_
:param coords: Homogeneous co-ordinates *[batch_shape,c,4]*
:type coords: array
:param voxel_shape_spec: Either the number of voxels in x,y,z directions, or the resolutions (metres) in x,y,z
directions, depending on mode. Batched or unbatched. *[batch_shape,3]* or *[3]*
:type voxel_shape_spec: array
:param mode: Shape specification mode, either "DIMS" or "RES"
:type mode: str
:param coord_bounds: Co-ordinate x, y, z boundaries *[batch_shape,6]* or *[6]*
:type coord_bounds: array
:param features: Co-ordinate features *[batch_shape,c,4]*.
E.g. RGB values, low-dimensional features, etc.
Features mapping to the same voxel are averaged.
:type features: 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: Voxel grid *[batch_shape,x_max,v_max,z_max,3+feature_size+1]*, dimensions *[batch_shape,3]*, resolutions *[batch_shape,3]*, voxel_grid_lower_corners *[batch_shape,3]*
"""
if batch_shape is None:
batch_shape = coords.shape[:-2]
if dev_str is None:
dev_str = _ivy.dev_str(coords)
# shapes as list
batch_shape = list(batch_shape)
num_batch_dims = len(batch_shape)
num_coords_per_batch = coords.shape[-2]
# voxel shape spec as array
if len(voxel_shape_spec) is 3:
# BS x 1 x 3
voxel_shape_spec = _ivy.expand_dims(
_ivy.tile(
_ivy.reshape(_ivy.array(voxel_shape_spec),
[1] * num_batch_dims + [3]), batch_shape + [1]),
-2)
# coord bounds spec as array
if coord_bounds is not None and len(coord_bounds) is 6:
# BS x 1 x 6
coord_bounds = _ivy.expand_dims(
_ivy.tile(
_ivy.reshape(_ivy.array(coord_bounds, dtype_str='float32'),
[1] * num_batch_dims + [6]), batch_shape + [1]),
-2)
# BS x N x 3
coords = coords[..., 0:3]
if coord_bounds is not None:
# BS x 1
x_min = coord_bounds[..., 0:1]
y_min = coord_bounds[..., 1:2]
z_min = coord_bounds[..., 2:3]
x_max = coord_bounds[..., 3:4]
y_max = coord_bounds[..., 4:5]
z_max = coord_bounds[..., 5:6]
# BS x N x 1
x_coords = coords[..., 0:1]
y_coords = coords[..., 1:2]
z_coords = coords[..., 2:3]
x_validity_mask = _ivy.logical_and(x_coords > x_min, x_coords < x_max)
y_validity_mask = _ivy.logical_and(y_coords > y_min, y_coords < y_max)
z_validity_mask = _ivy.logical_and(z_coords > z_min, z_coords < z_max)
# BS x N
full_validity_mask = _ivy.logical_and(
_ivy.logical_and(x_validity_mask, y_validity_mask),
z_validity_mask)[..., 0]
# BS x 1 x 3
bb_mins = coord_bounds[..., 0:3]
bb_maxs = coord_bounds[..., 3:6]
bb_ranges = bb_maxs - bb_mins
else:
# BS x N
full_validity_mask = _ivy.cast(
_ivy.ones(batch_shape + [num_coords_per_batch]), 'bool')
# BS x 1 x 3
bb_mins = _ivy.reduce_min(coords, axis=-2, keepdims=True)
bb_maxs = _ivy.reduce_max(coords, axis=-2, keepdims=True)
bb_ranges = bb_maxs - bb_mins
# get voxel dimensions
if mode is 'DIMS':
# BS x 1 x 3
dims = _ivy.cast(voxel_shape_spec, 'int32')
elif mode is 'RES':
# BS x 1 x 3
res = _ivy.cast(voxel_shape_spec, 'float32')
dims = _ivy.cast(_ivy.ceil(bb_ranges / (res + MIN_DENOMINATOR)),
'int32')
else:
raise Exception(
'Invalid mode selection. Must be either "DIMS" or "RES"')
dims_m_one = _ivy.cast(dims - 1, 'int32')
# BS x 1 x 3
res = bb_ranges / (_ivy.cast(dims, 'float32') + MIN_DENOMINATOR)
# BS x NC x 3
voxel_indices = _ivy.minimum(
_ivy.cast(_ivy.floor((coords - bb_mins) / (res + MIN_DENOMINATOR)),
'int32'), dims_m_one)
# BS x NC x 3
voxel_values = ((coords - bb_mins) % res) / (res + MIN_DENOMINATOR)
feature_size = 0
if features is not None:
feature_size = features.shape[-1]
voxel_values = _ivy.concatenate([voxel_values, features], axis=-1)
# TNVC x len(BS)+1
valid_coord_indices = _ivy.cast(_ivy.indices_where(full_validity_mask),
'int32')
# scalar
total_num_valid_coords = valid_coord_indices.shape[0]
# TNVC x 3
voxel_values_pruned_flat = _ivy.gather_nd(voxel_values,
valid_coord_indices)
voxel_indices_pruned_flat = _ivy.gather_nd(voxel_indices,
valid_coord_indices)
# TNVC x len(BS)+2
if num_batch_dims == 0:
all_indices_pruned_flat = voxel_indices_pruned_flat
else:
batch_indices = valid_coord_indices[..., :-1]
all_indices_pruned_flat = _ivy.concatenate(
[batch_indices] + [voxel_indices_pruned_flat], -1)
# TNVC x 4
voxel_values_pruned_flat =\
_ivy.concatenate((voxel_values_pruned_flat, _ivy.ones([total_num_valid_coords, 1], dev_str=dev_str)), -1)
# get max dims list for scatter
if num_batch_dims > 0:
max_dims = _ivy.reduce_max(_ivy.reshape(dims, batch_shape + [3]),
axis=list(range(num_batch_dims)))
else:
max_dims = _ivy.reshape(dims, batch_shape + [3])
batch_shape_array_list = [_ivy.array(batch_shape, 'int32')
] if num_batch_dims != 0 else []
total_dims_list = _ivy.to_list(
_ivy.concatenate(
batch_shape_array_list +
[max_dims, _ivy.array([4 + feature_size], 'int32')], -1))
# BS x x_max x y_max x z_max x 4
scattered = _ivy.scatter_nd(
all_indices_pruned_flat,
voxel_values_pruned_flat,
total_dims_list,
reduction='replace' if _ivy.backend == 'mxnd' else 'sum')
# BS x x_max x y_max x z_max x 4 + feature_size, BS x 3, BS x 3, BS x 3
return _ivy.concatenate(
(scattered[..., :-1] /
(_ivy.maximum(scattered[..., -1:], 1.) + MIN_DENOMINATOR),
scattered[..., -1:]),
-1), dims[..., 0, :], res[..., 0, :], bb_mins[..., 0, :]
def ext_mat_and_intrinsics_to_cam_geometry_object(ext_mat, intrinsics, batch_shape=None, dev_str=None):
"""
Create camera geometry object from extrinsic matrix :math:`\mathbf{E}\in\mathbb{R}^{3×4}`, and camera intrinsics
object.
:param ext_mat: Extrinsic matrix *[batch_shape,3,4]*
:type ext_mat: array
:param intrinsics: camera intrinsics object
:type intrinsics: camera_intrinsics
: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: Camera geometry object
"""
if batch_shape is None:
batch_shape = ext_mat.shape[:-2]
if dev_str is None:
dev_str = _ivy.dev_str(ext_mat)
# shapes as list
batch_shape = list(batch_shape)
# num batch dims
num_batch_dims = len(batch_shape)
# BS x 4 x 4
ext_mat_homo = \
_ivy.concatenate(
(ext_mat, _ivy.tile(_ivy.reshape(_ivy.array([0., 0., 0., 1.], dev_str=dev_str),
[1] * (num_batch_dims + 1) + [4]),
batch_shape + [1, 1])), -2)
# BS x 4 x 4
inv_ext_mat_homo = _ivy.inv(ext_mat_homo)
# BS x 3 x 4
inv_ext_mat = inv_ext_mat_homo[..., 0:3, :]
# BS x 3 x 1
cam_center = inv_ext_mat_to_camera_center(inv_ext_mat)
# BS x 3 x 3
Rs = ext_mat[..., 0:3]
# BS x 3 x 3
inv_Rs = inv_ext_mat[..., 0:3]
# extrinsics object
extrinsics = _Extrinsics(cam_center, Rs, inv_Rs, ext_mat_homo, inv_ext_mat_homo)
# BS x 3 x 4
full_mat = calib_and_ext_to_full_mat(intrinsics.calib_mats, ext_mat)
# BS x 4 x 4
full_mat_homo = \
_ivy.concatenate((
full_mat, _ivy.tile(_ivy.reshape(_ivy.array([0., 0., 0., 1.], dev_str=dev_str),
[1] * (num_batch_dims + 1) + [4]),
batch_shape + [1, 1])), -2)
# BS x 4 x 4
inv_full_mat_homo = _ivy.inv(full_mat_homo)
# camera geometry object
cam_geometry = _CameraGeometry(intrinsics, extrinsics, full_mat_homo, inv_full_mat_homo)
return cam_geometry
def smooth_image_fom_var_image(mean,
var,
kernel_dim,
kernel_scale,
dev_str=None):
"""
Smooth an image using variance values from a variance image of the same size, and a spatial smoothing kernel.
:param mean: Image to smooth *[batch_shape,h,w,d]*
:type mean: array
:param var: Variance image, with the variance values of each pixel in the image *[batch_shape,h,w,d]*
:type var: array
:param kernel_dim: The dimension of the kernel
:type kernel_dim: int
:param kernel_scale: The scale of the kernel along the channel dimension *[d]*
:type kernel_scale: array
: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: Image smoothed based on variance image and smoothing kernel.
"""
if dev_str is None:
dev_str = _ivy.dev_str(mean)
# shapes as list
kernel_shape = [kernel_dim, kernel_dim]
kernel_size = kernel_dim**2
dims = mean.shape[-1]
# KH x KW x 2
uniform_pixel_coords = _ivy_svg.create_uniform_pixel_coords_image(
kernel_shape, dev_str=dev_str)[..., 0:2]
# 2
kernel_central_pixel_coord = _ivy.array([
float(_math.floor(kernel_shape[0] / 2)),
float(_math.floor(kernel_shape[1] / 2))
],
dev_str=dev_str)
# KH x KW x 2
kernel_xy_dists = kernel_central_pixel_coord - uniform_pixel_coords
kernel_xy_dists_sqrd = kernel_xy_dists**2
# KW x KW x D x D
unit_kernel = _ivy.tile(
_ivy.reduce_sum(kernel_xy_dists_sqrd, -1, keepdims=True)**0.5,
(1, 1, dims))
kernel = 1 + unit_kernel * kernel_scale
recip_kernel = 1 / (kernel + MIN_DENOMINATOR)
# D
kernel_sum = _ivy.reduce_sum(kernel, [0, 1])[0]
recip_kernel_sum = _ivy.reduce_sum(recip_kernel, [0, 1])
# BS x H x W x D
recip_var = 1 / (var + MIN_DENOMINATOR)
recip_var_scaled = recip_var + 1
recip_new_var_scaled = _ivy.depthwise_conv2d(recip_var_scaled,
recip_kernel, 1, "VALID")
# This 0.99 prevents float32 rounding errors leading to -ve variances, the true equation would use 1.0
recip_new_var = recip_new_var_scaled - recip_kernel_sum * 0.99
new_var = 1 / (recip_new_var + MIN_DENOMINATOR)
mean_x_recip_var = mean * recip_var
mean_x_recip_var_sum = _ivy.abs(
_ivy.depthwise_conv2d(mean_x_recip_var, recip_kernel, 1, "VALID"))
new_mean = new_var * mean_x_recip_var_sum
new_var = new_var * kernel_size**2 / (kernel_sum + MIN_DENOMINATOR)
# prevent overconfidence from false meas independence assumption
# BS x H x W x D, # BS x H x W x D
return new_mean, new_var
def quantize_to_image(pixel_coords,
final_image_dims,
feat=None,
feat_prior=None,
with_db=False,
pixel_coords_var=1e-3,
feat_var=1e-3,
pixel_coords_prior_var=1e12,
feat_prior_var=1e12,
var_threshold=(1e-3, 1e12),
uniform_pixel_coords=None,
batch_shape=None,
dev_str=None):
"""
Quantize pixel co-ordinates with d feature channels (for depth, rgb, normals etc.), from
images :math:`\mathbf{X}\in\mathbb{R}^{input\_images\_shape×(2+d)}`, which may have been reprojected from a host of
different cameras (leading to non-integer pixel values), to a new quantized pixel co-ordinate image with the same
feature channels :math:`\mathbf{X}\in\mathbb{R}^{h×w×(2+d)}`, and with integer pixel co-ordinates.
Duplicates during the quantization are either probabilistically fused based on variance, or the minimum depth is
chosen when using depth buffer mode.
:param pixel_coords: Pixel co-ordinates *[batch_shape,input_size,2]*
:type pixel_coords: array
:param final_image_dims: Image dimensions of the final image.
:type final_image_dims: sequence of ints
:param feat: Features (i.e. depth, rgb, encoded), default is None. *[batch_shape,input_size,d]*
:type feat: array, optional
:param feat_prior: Prior feature image mean, default is None. *[batch_shape,input_size,d]*
:type feat_prior: array or float to fill with
:param with_db: Whether or not to use depth buffer in rendering, default is false
:type with_db: bool, optional
:param pixel_coords_var: Pixel coords variance *[batch_shape,input_size,2]*
:type pixel_coords_var: array or float to fill with
:param feat_var: Feature variance *[batch_shape,input_size,d]*
:type feat_var: array or float to fill with
:param pixel_coords_prior_var: Pixel coords prior variance *[batch_shape,h,w,2]*
:type pixel_coords_prior_var: array or float to fill with
:param feat_prior_var: Features prior variance *[batch_shape,h,w,3]*
:type feat_prior_var: array or float to fill with
:param var_threshold: Variance threshold, for projecting valid coords and clipping *[batch_shape,2+d,2]*
:type var_threshold: array or sequence of floats to fill with
:param uniform_pixel_coords: Homogeneous uniform (integer) pixel co-ordinate images, inferred from final_image_dims
if None *[batch_shape,h,w,3]*
:type uniform_pixel_coords: array, optional
:param batch_shape: Shape of batch. Assumed no batches 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: Quantized pixel co-ordinates image with d feature channels (for depth, rgb, normals etc.) *[batch_shape,h,w,2+d]*,
maybe the quantized variance, *[batch_shape,h,w,2+d]*, and scatter counter image *[batch_shape,h,w,1]*
"""
# ToDo: make variance fully optional. If not specified,
# then do not compute and scatter during function call for better efficiency.
# config
if batch_shape is None:
batch_shape = pixel_coords.shape[:-2]
if dev_str is None:
dev_str = _ivy.dev_str(pixel_coords)
if feat is None:
d = 0
else:
d = feat.shape[-1]
min_depth_diff = _ivy.array([MIN_DEPTH_DIFF], dev_str=dev_str)
red = 'min' if with_db else 'sum'
# shapes as list
batch_shape = list(batch_shape)
final_image_dims = list(final_image_dims)
num_batch_dims = len(batch_shape)
# variance threshold
if isinstance(var_threshold, tuple) or isinstance(var_threshold, list):
ones = _ivy.ones(batch_shape + [1, 2 + d, 1])
var_threshold = _ivy.concatenate(
(ones * var_threshold[0], ones * var_threshold[1]), -1)
else:
var_threshold = _ivy.reshape(var_threshold,
batch_shape + [1, 2 + d, 2])
# uniform pixel coords
if uniform_pixel_coords is None:
uniform_pixel_coords =\
_ivy_svg.create_uniform_pixel_coords_image(final_image_dims, batch_shape, dev_str=dev_str)
uniform_pixel_coords = uniform_pixel_coords[..., 0:2]
# Extract Values #
feat_prior = _ivy.ones_like(feat) * feat_prior if isinstance(
feat_prior, float) else feat_prior
pixel_coords_var = _ivy.ones_like(pixel_coords) * pixel_coords_var\
if isinstance(pixel_coords_var, float) else pixel_coords_var
feat_var = _ivy.ones_like(feat) * feat_var if isinstance(
feat_var, float) else feat_var
pixel_coords_prior_var = _ivy.ones(batch_shape + final_image_dims + [2]) * pixel_coords_prior_var\
if isinstance(pixel_coords_prior_var, float) else pixel_coords_prior_var
feat_prior_var = _ivy.ones(batch_shape + final_image_dims + [d]) * feat_prior_var\
if isinstance(feat_prior_var, float) else feat_prior_var
# Quantize #
# BS x N x 2
quantized_pixel_coords = _ivy.reshape(
_ivy.cast(_ivy.round(pixel_coords), 'int32'), batch_shape + [-1, 2])
# Combine #
# BS x N x (2+D)
pc_n_feat = _ivy.reshape(_ivy.concatenate((pixel_coords, feat), -1),
batch_shape + [-1, 2 + d])
pc_n_feat_var = _ivy.reshape(
_ivy.concatenate((pixel_coords_var, feat_var), -1),
batch_shape + [-1, 2 + d])
# BS x H x W x (2+D)
prior = _ivy.concatenate((uniform_pixel_coords, feat_prior), -1)
prior_var = _ivy.concatenate((pixel_coords_prior_var, feat_prior_var), -1)
# Validity Mask #
# BS x N x 1
var_validity_mask = \
_ivy.reduce_sum(_ivy.cast(pc_n_feat_var < var_threshold[..., 1], 'int32'), -1, keepdims=True) == 2+d
bounds_validity_mask = _ivy.logical_and(
_ivy.logical_and(quantized_pixel_coords[..., 0:1] >= 0,
quantized_pixel_coords[..., 1:2] >= 0),
_ivy.logical_and(
quantized_pixel_coords[..., 0:1] <= final_image_dims[1] - 1,
quantized_pixel_coords[..., 1:2] <= final_image_dims[0] - 1))
validity_mask = _ivy.logical_and(var_validity_mask, bounds_validity_mask)
# num_valid_indices x len(BS)+2
validity_indices = _ivy.reshape(
_ivy.cast(_ivy.indices_where(validity_mask), 'int32'),
[-1, num_batch_dims + 2])
num_valid_indices = validity_indices.shape[-2]
if num_valid_indices == 0:
return _ivy.concatenate((uniform_pixel_coords[..., 0:2], feat_prior), -1), \
_ivy.concatenate((pixel_coords_prior_var, feat_prior_var), -1),\
_ivy.zeros_like(feat[..., 0:1], dev_str=dev_str)
# Depth Based Scaling #
mean_depth_min = None
mean_depth_range = None
pc_n_feat_wo_depth_range = None
pc_n_feat_wo_depth_min = None
var_vals_range = None
var_vals_min = None
if with_db:
# BS x N x 1
mean_depth = pc_n_feat[..., 2:3]
# BS x 1 x 1
mean_depth_min = _ivy.reduce_min(mean_depth, -2, keepdims=True)
mean_depth_max = _ivy.reduce_max(mean_depth, -2, keepdims=True)
mean_depth_range = mean_depth_max - mean_depth_min
# BS x N x 1
scaled_depth = (mean_depth - mean_depth_min) / (
mean_depth_range * min_depth_diff + MIN_DENOMINATOR)
if d == 1:
# BS x 1 x 1+D
pc_n_feat_wo_depth_min = _ivy.zeros(batch_shape + [1, 0],
dev_str=dev_str)
pc_n_feat_wo_depth_range = _ivy.ones(batch_shape + [1, 0],
dev_str=dev_str)
else:
# feat without depth
# BS x N x 1+D
pc_n_feat_wo_depth = _ivy.concatenate(
(pc_n_feat[..., 0:2], pc_n_feat[..., 3:]), -1)
# find the min and max of each value
# BS x 1 x 1+D
pc_n_feat_wo_depth_max = _ivy.reduce_max(
pc_n_feat_wo_depth, -2, keepdims=True) + 1
pc_n_feat_wo_depth_min = _ivy.reduce_min(
pc_n_feat_wo_depth, -2, keepdims=True) - 1
pc_n_feat_wo_depth_range = pc_n_feat_wo_depth_max - pc_n_feat_wo_depth_min
# BS x N x 1+D
normed_pc_n_feat_wo_depth = (pc_n_feat_wo_depth - pc_n_feat_wo_depth_min) / \
(pc_n_feat_wo_depth_range + MIN_DENOMINATOR)
# combine with scaled depth
# BS x N x 1+D
pc_n_feat_wo_depth_scaled = normed_pc_n_feat_wo_depth + scaled_depth
# BS x N x (2+D)
pc_n_feat = _ivy.concatenate(
(pc_n_feat_wo_depth_scaled[..., 0:2], mean_depth,
pc_n_feat_wo_depth_scaled[..., 2:]), -1)
# scale variance
# BS x 1 x (2+D)
var_vals_max = _ivy.reduce_max(pc_n_feat_var, -2, keepdims=True) + 1
var_vals_min = _ivy.reduce_min(pc_n_feat_var, -2, keepdims=True) - 1
var_vals_range = var_vals_max - var_vals_min
# BS x N x (2+D)
normed_var_vals = (pc_n_feat_var - var_vals_min) / (var_vals_range +
MIN_DENOMINATOR)
pc_n_feat_var = normed_var_vals + scaled_depth
# ready for later reversal with full image dimensions
# BS x 1 x 1 x D
var_vals_min = _ivy.expand_dims(var_vals_min, -2)
var_vals_range = _ivy.expand_dims(var_vals_range, -2)
# Validity Pruning #
# num_valid_indices x (2+D)
pc_n_feat = _ivy.gather_nd(pc_n_feat,
validity_indices[..., 0:num_batch_dims + 1])
pc_n_feat_var = _ivy.gather_nd(pc_n_feat_var,
validity_indices[..., 0:num_batch_dims + 1])
# num_valid_indices x 2
quantized_pixel_coords = _ivy.gather_nd(
quantized_pixel_coords, validity_indices[..., 0:num_batch_dims + 1])
if with_db:
means_to_scatter = pc_n_feat
vars_to_scatter = pc_n_feat_var
else:
# num_valid_indices x (2+D)
vars_to_scatter = 1 / (pc_n_feat_var + MIN_DENOMINATOR)
means_to_scatter = pc_n_feat * vars_to_scatter
# Scatter #
# num_valid_indices x 1
counter = _ivy.ones_like(pc_n_feat[..., 0:1], dev_str=dev_str)
if with_db:
counter *= -1
# num_valid_indices x 2(2+D)+1
values_to_scatter = _ivy.concatenate(
(means_to_scatter, vars_to_scatter, counter), -1)
# num_valid_indices x (num_batch_dims + 2)
all_indices = _ivy.flip(quantized_pixel_coords, -1)
if num_batch_dims > 0:
all_indices = _ivy.concatenate(
(validity_indices[..., :-2], all_indices), -1)
# BS x H x W x (2(2+D) + 1)
quantized_img = _ivy.scatter_nd(
_ivy.reshape(all_indices, [-1, num_batch_dims + 2]),
_ivy.reshape(values_to_scatter, [-1, 2 * (2 + d) + 1]),
batch_shape + final_image_dims + [2 * (2 + d) + 1],
reduction='replace' if _ivy.backend == 'mxnd' else red)
# BS x H x W x 1
quantized_counter = quantized_img[..., -1:]
if with_db:
invalidity_mask = quantized_counter != -1
else:
invalidity_mask = quantized_counter == 0
if with_db:
# BS x H x W x (2+D)
quantized_mean_scaled = quantized_img[..., 0:2 + d]
quantized_var_scaled = quantized_img[..., 2 + d:2 * (2 + d)]
# BS x H x W x 1
quantized_depth_mean = quantized_mean_scaled[..., 2:3]
# BS x 1 x 1 x 1
mean_depth_min = _ivy.expand_dims(mean_depth_min, -2)
mean_depth_range = _ivy.expand_dims(mean_depth_range, -2)
# BS x 1 x 1 x (1+D)
pc_n_feat_wo_depth_min = _ivy.expand_dims(pc_n_feat_wo_depth_min, -2)
pc_n_feat_wo_depth_range = _ivy.expand_dims(pc_n_feat_wo_depth_range,
-2)
# BS x 1 x 1 x (2+D) x 2
var_threshold = _ivy.expand_dims(var_threshold, -3)
# BS x H x W x (1+D)
quantized_mean_wo_depth_scaled = _ivy.concatenate(
(quantized_mean_scaled[..., 0:2], quantized_mean_scaled[..., 3:]),
-1)
quantized_mean_wo_depth_normed = quantized_mean_wo_depth_scaled - (quantized_depth_mean - mean_depth_min) / \
(mean_depth_range * min_depth_diff + MIN_DENOMINATOR)
quantized_mean_wo_depth = quantized_mean_wo_depth_normed * pc_n_feat_wo_depth_range + pc_n_feat_wo_depth_min
prior_wo_depth = _ivy.concatenate((prior[..., 0:2], prior[..., 3:]),
-1)
quantized_mean_wo_depth = _ivy.where(invalidity_mask, prior_wo_depth,
quantized_mean_wo_depth)
# BS x H x W x (2+D)
quantized_mean = _ivy.concatenate(
(quantized_mean_wo_depth[..., 0:2], quantized_depth_mean,
quantized_mean_wo_depth[..., 2:]), -1)
# BS x H x W x (2+D)
quantized_var_normed = quantized_var_scaled - (quantized_depth_mean - mean_depth_min) / \
(mean_depth_range * min_depth_diff + MIN_DENOMINATOR)
quantized_var = _ivy.maximum(
quantized_var_normed * var_vals_range + var_vals_min,
var_threshold[..., 0])
quantized_var = _ivy.where(invalidity_mask, prior_var, quantized_var)
else:
# BS x H x W x (2+D)
quantized_sum_mean_x_recip_var = quantized_img[..., 0:2 + d]
quantized_var_wo_increase = _ivy.where(
invalidity_mask, prior_var,
(1 / (quantized_img[..., 2 + d:2 * (2 + d)] + MIN_DENOMINATOR)))
quantized_var = _ivy.maximum(
quantized_var_wo_increase * quantized_counter,
_ivy.expand_dims(var_threshold[..., 0], -2))
quantized_var = _ivy.where(invalidity_mask, prior_var, quantized_var)
quantized_mean = _ivy.where(
invalidity_mask, prior,
quantized_var_wo_increase * quantized_sum_mean_x_recip_var)
# BS x H x W x (2+D) BS x H x W x (2+D) BS x H x W x 1
return quantized_mean, quantized_var, quantized_counter
