def projection_matrix_pseudo_inverse(proj_mat, batch_shape=None): """ Given projection matrix :math:`\mathbf{P}\in\mathbb{R}^{3×4}`, compute it's pseudo-inverse :math:`\mathbf{P}^+\in\mathbb{R}^{4×3}`.\n `[reference] <localhost:63342/ivy/docs/source/references/mvg_textbook.pdf?_ijt=25ihpil89dmfo4da975v402ogc#page=179>`_ bottom of page 161, section 6.2.2 :param proj_mat: Projection matrix *[batch_shape,3,4]* :type proj_mat: array :param batch_shape: Shape of batch. Inferred from inputs if None. :type batch_shape: sequence of ints, optional :return: Projection matrix pseudo-inverse *[batch_shape,4,3]* """ if batch_shape is None: batch_shape = proj_mat.shape[:-2] # shapes as list batch_shape = list(batch_shape) # transpose idxs num_batch_dims = len(batch_shape) transpose_idxs = list( range(num_batch_dims)) + [num_batch_dims + 1, num_batch_dims] # BS x 4 x 3 matrix_transposed = _ivy.transpose(proj_mat, transpose_idxs) # BS x 4 x 3 return _ivy.matmul(matrix_transposed, _ivy.inv(_ivy.matmul(proj_mat, matrix_transposed)))
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 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 projection_matrix_inverse(proj_mat): """ Given projection matrix :math:`\mathbf{P}\in\mathbb{R}^{3×4}`, compute it's inverse :math:`\mathbf{P}^{-1}\in\mathbb{R}^{3×4}`.\n `[reference] <https://github.com/pranjals16/cs676/blob/master/Hartley%2C%20Zisserman%20-%20Multiple%20View%20Geometry%20in%20Computer%20Vision.pdf#page=174>`_ middle of page 156, section 6.1, eq 6.6 :param proj_mat: Projection matrix *[batch_shape,3,4]* :type proj_mat: array :return: Projection matrix inverse *[batch_shape,3,4]* """ # BS x 3 x 3 rotation_matrix = proj_mat[..., 0:3] # BS x 3 x 3 rotation_matrix_inverses = _ivy.inv(rotation_matrix) # BS x 3 x 1 translations = proj_mat[..., 3:4] # BS x 3 x 1 translation_inverses = -_ivy.matmul(rotation_matrix_inverses, translations) # BS x 3 x 4 return _ivy.concatenate((rotation_matrix_inverses, translation_inverses), -1)
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 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 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 _fit_spline(train_points, train_values, order): # shapes train_points_shape = train_points.shape batch_shape = list(train_points_shape[:-2]) num_batch_dims = len(batch_shape) n = train_points_shape[-2] pd = train_values.shape[-1] # BS x N x 1 c = train_points # BS x N x PD f_ = train_values # BS x N x N matrix_a = _phi(_pairwise_distance(c, c), order) # BS x N x 1 ones = _ivy.ones_like(c[..., :1]) # BS x N x 2 matrix_b = _ivy.concatenate([c, ones], -1) # BS x 2 x N matrix_b_trans = _ivy.transpose( matrix_b, list(range(num_batch_dims)) + [num_batch_dims + 1, num_batch_dims]) # BS x N+2 x N left_block = _ivy.concatenate([matrix_a, matrix_b_trans], -2) # BS x 2 x 2 lhs_zeros = _ivy.zeros(batch_shape + [2, 2]) # BS x N+2 x 2 right_block = _ivy.concatenate([matrix_b, lhs_zeros], -2) # BS x N+2 x N+2 lhs = _ivy.concatenate([left_block, right_block], -1) # BS x 2 x PD rhs_zeros = _ivy.zeros(batch_shape + [2, pd]) # BS x N+2 x PD rhs = _ivy.concatenate([f_, rhs_zeros], -2) # BS x N+2 x PD w_v = _ivy.matmul(_ivy.pinv(lhs), rhs) # BS x N x PD w = w_v[..., :n, :] # BS x 2 x PD v = w_v[..., n:, :] # BS x N x PD, BS x 2 x PD return w, v
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 calib_and_ext_to_full_mat(calib_mat, ext_mat): """ Compute full projection matrix :math:`\mathbf{P}\in\mathbb{R}^{3×4}` from calibration :math:`\mathbf{K}\in\mathbb{R}^{3×3}` and extrinsic matrix :math:`\mathbf{E}\in\mathbb{R}^{3×4}`.\n :param calib_mat: Calibration matrix *[batch_shape,3,3]* :type calib_mat: array :param ext_mat: Extrinsic matrix *[batch_shape,3,4]* :type ext_mat: array :return: Full projection matrix *[batch_shape,3,4]* """ # BS x 3 x 4 return _ivy.matmul(calib_mat, ext_mat)
def sample_spline_path(anchor_points, anchor_vals, sample_points, order=3): """ Sample spline path, given sample locations for path defined by the anchor locations and points. `[reference] <https://github.com/tensorflow/addons/blob/v0.11.2/tensorflow_addons/image/interpolate_spline.py>`_ :param anchor_points: Anchor locations between 0-1 (regular spacing not necessary) *[batch_shape,num_anchors,1]* :type anchor_points: array :param anchor_vals: Anchor points along the spline path, in path space *[batch_shape,num_anchors,path_dim]* :type anchor_vals: array :param sample_points: Sample locations between 0-1 *[batch_shape,num_samples,1]* :type sample_points: array :param order: Order of the spline path interpolation :type order: float :return: Spline path sampled at sample_locations, giving points in path space *[batch_shape,num_samples,path_dim]* """ # BS x N x PD, BS x 2 x PD w, v = _fit_spline(anchor_points, anchor_vals, order) # Kernel term # BS x NS x N pairwise_dists = _pairwise_distance(sample_points, anchor_points) phi_pairwise_dists = _phi(pairwise_dists, order) # BS x NS x PD rbf_term = _ivy.matmul(phi_pairwise_dists, w) # Polynomial / linear term. # BS x NS x 2 query_points_pad = _ivy.concatenate( [sample_points, _ivy.ones_like(sample_points[..., :1])], -1) # BS x NS x PD linear_term = _ivy.matmul(query_points_pad, v) return rbf_term + linear_term
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 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 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)
def _forward(self, x, prev_state): prev_read_vector_list = prev_state[1] controller_input = ivy.concatenate([x] + prev_read_vector_list, axis=1) controller_output, controller_state = self._controller(ivy.expand_dims(controller_input, -2), initial_state=prev_state[0]) controller_output = controller_output[..., -1, :] parameters = self._controller_proj(controller_output) parameters = ivy.clip(parameters, -self._clip_value, self._clip_value) head_parameter_list = \ ivy.split(parameters[:, :self._num_parameters_per_head * self._num_heads], self._num_heads, axis=1) erase_add_list = ivy.split(parameters[:, self._num_parameters_per_head * self._num_heads:], 2 * self._write_head_num, axis=1) prev_w_list = prev_state[2] prev_M = prev_state[4] w_list = [] for i, head_parameter in enumerate(head_parameter_list): k = ivy.tanh(head_parameter[:, 0:self._memory_vector_dim]) beta = ivy.softplus(head_parameter[:, self._memory_vector_dim]) g = ivy.sigmoid(head_parameter[:, self._memory_vector_dim + 1]) s = ivy.softmax( head_parameter[:, self._memory_vector_dim + 2:self._memory_vector_dim + 2 + (self._shift_range * 2 + 1)]) gamma = ivy.softplus(head_parameter[:, -1]) + 1 w = self._addressing(k, beta, g, s, gamma, prev_M, prev_w_list[i]) w_list.append(w) # Reading (Sec 3.1) read_w_list = w_list[:self._read_head_num] if self._step == 0: usage_indicator = ivy.zeros_like(w_list[0]) else: usage_indicator = prev_state[3] + ivy.reduce_sum(ivy.concatenate(read_w_list, 0)) read_vector_list = [] for i in range(self._read_head_num): read_vector = ivy.reduce_sum(ivy.expand_dims(read_w_list[i], axis=2) * prev_M, axis=1) read_vector_list.append(read_vector) # Writing (Sec 3.2) prev_wrtie_w_list = prev_w_list[self._read_head_num:] w_wr_size = math.ceil(self._memory_size / 2) if self._retroactive_updates else self._memory_size if self._sequential_writing: batch_size = ivy.shape(x)[0] if self._step < w_wr_size: w_wr_list = [ivy.tile(ivy.cast(ivy.one_hot( ivy.array([self._step]), w_wr_size), 'float32'), (batch_size, 1))] * self._write_head_num else: batch_idxs = ivy.expand_dims(ivy.arange(batch_size, 0), -1) mem_idxs = ivy.expand_dims(ivy.argmax(usage_indicator[..., :w_wr_size], -1), -1) total_idxs = ivy.concatenate((batch_idxs, mem_idxs), -1) w_wr_list = [ivy.scatter_nd(total_idxs, ivy.ones((batch_size,)), (batch_size, w_wr_size))] * self._write_head_num else: w_wr_list = w_list[self._read_head_num:] if self._retroactive_updates: w_ret_list = [self._retroactive_discount * prev_wrtie_w[..., w_wr_size:] + (1 - self._retroactive_discount) * prev_wrtie_w[..., :w_wr_size] for prev_wrtie_w in prev_wrtie_w_list] w_wrtie_list = [ivy.concatenate((w_wr, w_ret), -1) for w_wr, w_ret in zip(w_wr_list, w_ret_list)] else: w_wrtie_list = w_wr_list M = prev_M for i in range(self._write_head_num): w = ivy.expand_dims(w_wrtie_list[i], axis=2) if self._with_erase: erase_vector = ivy.expand_dims(ivy.sigmoid(erase_add_list[i * 2]), axis=1) M = M * ivy.ones(ivy.shape(M)) - ivy.matmul(w, erase_vector) add_vector = ivy.expand_dims(ivy.tanh(erase_add_list[i * 2 + 1]), axis=1) M = M + ivy.matmul(w, add_vector) NTM_output = self._output_proj(ivy.concatenate([controller_output] + read_vector_list, axis=1)) NTM_output = ivy.clip(NTM_output, -self._clip_value, self._clip_value) self._step += 1 return NTM_output, NTMControllerState( controller_state=controller_state, read_vector_list=read_vector_list, w_list=w_list, usage_indicator=usage_indicator, M=M)
def project_cam_coords_with_object_transformations(cam_coords_1, id_image, obj_ids, obj_trans, cam_1_to_2_ext_mat, batch_shape=None, image_dims=None): """ Compute velocity image from co-ordinate image, id image, and object transformations. :param cam_coords_1: Camera-centric homogeneous co-ordinates image in frame t *[batch_shape,h,w,4]* :type cam_coords_1: 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 cam_1_to_2_ext_mat: Camera 1 to camera 2 extrinsic projection matrix *[batch_shape,3,4]* :type cam_1_to_2_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 :return: Relative velocity image *[batch_shape,h,w,3]* """ if batch_shape is None: batch_shape = cam_coords_1.shape[:-3] if image_dims is None: image_dims = cam_coords_1.shape[-3:-1] # shapes as list batch_shape = list(batch_shape) image_dims = list(image_dims) num_batch_dims = len(batch_shape) # Transform the co-ordinate image by each transformation # BS x (num_obj x 3) x 4 obj_trans = _ivy.reshape(obj_trans, batch_shape + [-1, 4]) # BS x 4 x H x W cam_coords_1_ = _ivy.transpose( cam_coords_1, list(range(num_batch_dims)) + [i + num_batch_dims for i in [2, 0, 1]]) # BS x 4 x (HxW) cam_coords_1_ = _ivy.reshape(cam_coords_1_, batch_shape + [4, -1]) # BS x (num_obj x 3) x (HxW) cam_coords_2_all_obj_trans = _ivy.matmul(obj_trans, cam_coords_1_) # BS x (HxW) x (num_obj x 3) cam_coords_2_all_obj_trans = \ _ivy.transpose(cam_coords_2_all_obj_trans, list(range(num_batch_dims)) + [i + num_batch_dims for i in [1, 0]]) # BS x H x W x num_obj x 3 cam_coords_2_all_obj_trans = _ivy.reshape( cam_coords_2_all_obj_trans, batch_shape + image_dims + [-1, 3]) # Multiplier # BS x 1 x 1 x num_obj obj_ids = _ivy.reshape(obj_ids, batch_shape + [1, 1] + [-1]) # BS x H x W x num_obj x 1 multiplier = _ivy.cast(_ivy.expand_dims(obj_ids == id_image, -1), 'float32') # compute validity mask, for pixels which are on moving objects # BS x H x W x 1 motion_mask = _ivy.reduce_sum(multiplier, -2) > 0 # make invalid transformations equal to zero # BS x H x W x num_obj x 3 cam_coords_2_all_obj_trans_w_zeros = cam_coords_2_all_obj_trans * multiplier # reduce to get only valid transformations # BS x H x W x 3 cam_coords_2_all_obj_trans = _ivy.reduce_sum( cam_coords_2_all_obj_trans_w_zeros, -2) # find cam coords to for zero motion pixels # BS x H x W x 3 cam_coords_2_wo_motion = _ivy_tvg.cam_to_cam_coords( cam_coords_1, cam_1_to_2_ext_mat, batch_shape, image_dims) # BS x H x W x 4 cam_coords_2_all_trans_homo =\ _ivy_mech.make_coordinates_homogeneous(cam_coords_2_all_obj_trans, batch_shape + image_dims) cam_coords_2 = _ivy.where(motion_mask, cam_coords_2_all_trans_homo, cam_coords_2_wo_motion) # return # BS x H x W x 3, BS x H x W x 1 return cam_coords_2, motion_mask
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 main(interactive=True, f=None): global INTERACTIVE INTERACTIVE = interactive # Framework Setup # # ----------------# # choose random framework f = choose_random_framework() if f is None else f set_framework(f) # Camera Geometry # # ----------------# # intrinsics # common intrinsic params img_dims = [512, 512] pp_offsets = ivy.array([dim / 2 - 0.5 for dim in img_dims], 'float32') cam_persp_angles = ivy.array([60 * np.pi / 180] * 2, 'float32') # ivy cam intrinsics container intrinsics = ivy_vision.persp_angles_and_pp_offsets_to_intrinsics_object( cam_persp_angles, pp_offsets, img_dims) # extrinsics # 3 x 4 cam1_inv_ext_mat = ivy.array(np.load(data_dir + '/cam1_inv_ext_mat.npy'), 'float32') cam2_inv_ext_mat = ivy.array(np.load(data_dir + '/cam2_inv_ext_mat.npy'), 'float32') # full geometry # ivy cam geometry container cam1_geom = ivy_vision.inv_ext_mat_and_intrinsics_to_cam_geometry_object( cam1_inv_ext_mat, intrinsics) cam2_geom = ivy_vision.inv_ext_mat_and_intrinsics_to_cam_geometry_object( cam2_inv_ext_mat, intrinsics) cam_geoms = [cam1_geom, cam2_geom] # Camera Geometry Check # # ----------------------# # assert camera geometry shapes for cam_geom in cam_geoms: assert cam_geom.intrinsics.focal_lengths.shape == (2, ) assert cam_geom.intrinsics.persp_angles.shape == (2, ) assert cam_geom.intrinsics.pp_offsets.shape == (2, ) assert cam_geom.intrinsics.calib_mats.shape == (3, 3) assert cam_geom.intrinsics.inv_calib_mats.shape == (3, 3) assert cam_geom.extrinsics.cam_centers.shape == (3, 1) assert cam_geom.extrinsics.Rs.shape == (3, 3) assert cam_geom.extrinsics.inv_Rs.shape == (3, 3) assert cam_geom.extrinsics.ext_mats_homo.shape == (4, 4) assert cam_geom.extrinsics.inv_ext_mats_homo.shape == (4, 4) assert cam_geom.full_mats_homo.shape == (4, 4) assert cam_geom.inv_full_mats_homo.shape == (4, 4) # Image Data # # -----------# # load images # h x w x 3 color1 = ivy.array( cv2.imread(data_dir + '/rgb1.png').astype(np.float32) / 255) color2 = ivy.array( cv2.imread(data_dir + '/rgb2.png').astype(np.float32) / 255) # h x w x 1 depth1 = ivy.array( np.reshape( np.frombuffer( cv2.imread(data_dir + '/depth1.png', -1).tobytes(), np.float32), img_dims + [1])) depth2 = ivy.array( np.reshape( np.frombuffer( cv2.imread(data_dir + '/depth2.png', -1).tobytes(), np.float32), img_dims + [1])) # depth scaled pixel coords # h x w x 3 u_pix_coords = ivy_vision.create_uniform_pixel_coords_image(img_dims) ds_pixel_coords1 = u_pix_coords * depth1 ds_pixel_coords2 = u_pix_coords * depth2 # depth limits depth_min = ivy.reduce_min(ivy.concatenate((depth1, depth2), 0)) depth_max = ivy.reduce_max(ivy.concatenate((depth1, depth2), 0)) depth_limits = [depth_min, depth_max] # show images show_rgb_and_depth_images(color1, color2, depth1, depth2, depth_limits) # Flow and Depth Triangulation # # -----------------------------# # required mat formats cam1to2_full_mat_homo = ivy.matmul(cam2_geom.full_mats_homo, cam1_geom.inv_full_mats_homo) cam1to2_full_mat = cam1to2_full_mat_homo[..., 0:3, :] full_mats_homo = ivy.concatenate( (ivy.expand_dims(cam1_geom.full_mats_homo, 0), ivy.expand_dims(cam2_geom.full_mats_homo, 0)), 0) full_mats = full_mats_homo[..., 0:3, :] # flow flow1to2 = ivy_vision.flow_from_depth_and_cam_mats(ds_pixel_coords1, cam1to2_full_mat) # depth again depth1_from_flow = ivy_vision.depth_from_flow_and_cam_mats( flow1to2, full_mats) # show images show_flow_and_depth_images(depth1, flow1to2, depth1_from_flow, depth_limits) # Inverse Warping # # ----------------# # inverse warp rendering warp = u_pix_coords[..., 0:2] + flow1to2 color2_warp_to_f1 = ivy.reshape(ivy.bilinear_resample(color2, warp), color1.shape) # projected depth scaled pixel coords 2 ds_pixel_coords1_wrt_f2 = ivy_vision.ds_pixel_to_ds_pixel_coords( ds_pixel_coords1, cam1to2_full_mat) # projected depth 2 depth1_wrt_f2 = ds_pixel_coords1_wrt_f2[..., -1:] # inverse warp depth depth2_warp_to_f1 = ivy.reshape(ivy.bilinear_resample(depth2, warp), depth1.shape) # depth validity depth_validity = ivy.abs(depth1_wrt_f2 - depth2_warp_to_f1) < 0.01 # inverse warp rendering with mask color2_warp_to_f1_masked = ivy.where(depth_validity, color2_warp_to_f1, ivy.zeros_like(color2_warp_to_f1)) # show images show_inverse_warped_images(depth1_wrt_f2, depth2_warp_to_f1, depth_validity, color1, color2_warp_to_f1, color2_warp_to_f1_masked, depth_limits) # Forward Warping # # ----------------# # forward warp rendering ds_pixel_coords1_proj = ivy_vision.ds_pixel_to_ds_pixel_coords( ds_pixel_coords2, ivy.inv(cam1to2_full_mat_homo)[..., 0:3, :]) depth1_proj = ds_pixel_coords1_proj[..., -1:] ds_pixel_coords1_proj = ds_pixel_coords1_proj[..., 0:2] / depth1_proj features_to_render = ivy.concatenate((depth1_proj, color2), -1) # without depth buffer f1_forward_warp_no_db, _, _ = ivy_vision.quantize_to_image( ivy.reshape(ds_pixel_coords1_proj, (-1, 2)), img_dims, ivy.reshape(features_to_render, (-1, 4)), ivy.zeros_like(features_to_render), with_db=False) # with depth buffer f1_forward_warp_w_db, _, _ = ivy_vision.quantize_to_image( ivy.reshape(ds_pixel_coords1_proj, (-1, 2)), img_dims, ivy.reshape(features_to_render, (-1, 4)), ivy.zeros_like(features_to_render), with_db=False if ivy.get_framework() == 'mxnd' else True) # show images show_forward_warped_images(depth1, color1, f1_forward_warp_no_db, f1_forward_warp_w_db, depth_limits) # message print('End of Run Through Demo!')
def sample_links(self, joint_angles, link_num=None, samples_per_metre=25, batch_shape=None): """ Sample links of the robot at uniformly distributed cartesian positions. :param joint_angles: Joint angles of the robot *[batch_shape,num_joints]* :type joint_angles: array :param link_num: Link number for which to compute matrices up to. Default is the last link. :type link_num: int, optional :param samples_per_metre: Number of samples per metre of robot link :type samples_per_metre: int :param batch_shape: Shape of batch. Inferred from inputs if None. :type batch_shape: sequence of ints, optional :return: The sampled link cartesian positions *[batch_shape,total_sampling_chain_length,3]* """ if link_num is None: link_num = self._num_joints if batch_shape is None: batch_shape = joint_angles.shape[:-1] batch_shape = list(batch_shape) num_batch_dims = len(batch_shape) batch_dims_for_trans = list(range(num_batch_dims)) # BS x NJ x 4 x 4 link_matrices = self.compute_link_matrices(joint_angles, link_num, batch_shape) # BS x LN+1 x 3 link_positions = link_matrices[..., 0:3, -1] # BS x LN x 3 segment_starts = link_positions[..., :-1, :] segment_ends = link_positions[..., 1:, :] # LN segment_sizes = _ivy.cast(_ivy.ceil( self._link_lengths[0:link_num] * samples_per_metre), 'int32') # list of segments segments_list = list() for link_idx in range(link_num): segment_size = segment_sizes[link_idx] # BS x 1 x 3 segment_start = segment_starts[..., link_idx:link_idx + 1, :] segment_end = segment_ends[..., link_idx:link_idx + 1, :] # BS x segment_size x 3 segment = _ivy.linspace(segment_start, segment_end, segment_size, axis=-2)[..., 0, :, :] if link_idx == link_num - 1 or segment_size == 1: segments_list.append(segment) else: segments_list.append(segment[..., :-1, :]) # BS x total_robot_chain_length x 3 all_segments = _ivy.concatenate(segments_list, -2) # BS x total_robot_chain_length x 4 all_segments_homo = _ivy_mech.make_coordinates_homogeneous(all_segments) # 4 x BSxtotal_robot_chain_length all_segments_homo_trans = _ivy.reshape(_ivy.transpose( all_segments_homo, [num_batch_dims + 1] + batch_dims_for_trans + [num_batch_dims]), (4, -1)) # 3 x BSxtotal_robot_chain_length transformed_trans = _ivy.matmul(self._base_inv_ext_mat[..., 0:3, :], all_segments_homo_trans) # BS x total_robot_chain_length x 3 return _ivy.transpose(_ivy.reshape( transformed_trans, [3] + batch_shape + [-1]), [i+1 for i in batch_dims_for_trans] + [num_batch_dims+1] + [0])
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 compute_link_matrices(self, joint_angles, link_num, batch_shape=None): """ Compute homogeneous transformation matrices relative to base frame, up to link_num of links. :param joint_angles: Joint angles of the robot *[batch_shape,num_joints]* :type joint_angles: array :param link_num: Link number for which to compute matrices up to :type link_num: int :param batch_shape: Shape of batch. Inferred from inputs if None. :type batch_shape: sequence of ints, optional :return: The link_num matrices, up the link_num *[batch_shape,link_num,4,4]* """ if batch_shape is None: batch_shape = joint_angles.shape[:-1] batch_shape = list(batch_shape) num_batch_dims = len(batch_shape) # BS x 1 x NJ try: dh_joint_angles = _ivy.expand_dims(joint_angles * self._dh_joint_scales - self._dh_joint_offsets, -2) except: d = 0 # BS x 1 x 4 x 4 A00 = _ivy.identity(4, batch_shape=batch_shape + [1]) Aitoip1dashs = list() Aiip1s = list() A0is = [A00] # repeated blocks # BS x 1 x NJ dis = _ivy.tile(_ivy.reshape(self._dis, [1] * num_batch_dims + [1, self._num_joints]), batch_shape + [1, 1]) # BS x 1 x 4 bottom_row = _ivy.tile( _ivy.reshape(_ivy.array([0., 0., 0., 1.]), [1] * num_batch_dims + [1, 4]), batch_shape + [1, 1]) # BS x 1 x 3 start_of_bottom_middle = _ivy.tile( _ivy.reshape(_ivy.array([0., 0., 1.]), [1] * num_batch_dims + [1, 3]), batch_shape + [1, 1]) # BS x 1 x 2 zeros = _ivy.zeros(batch_shape + [1, 2]) for i in range(self._num_joints): # BS x 1 x 4 top_row = _ivy.concatenate((_ivy.cos(dh_joint_angles[..., i:i + 1]), -_ivy.sin(dh_joint_angles[..., i:i + 1]), zeros), -1) top_middle_row = _ivy.concatenate((_ivy.sin(dh_joint_angles[..., i:i + 1]), _ivy.cos(dh_joint_angles[..., i:i + 1]), zeros), -1) bottom_middle_row = _ivy.concatenate((start_of_bottom_middle, dis[..., i:i + 1]), -1) # BS x 4 x 4 Aitoip1dash = _ivy.concatenate((top_row, top_middle_row, bottom_middle_row, bottom_row), -2) # (BSx4) x 4 Aitoip1dash_flat = _ivy.reshape(Aitoip1dash, (-1, 4)) # (BSx4) x 4 Aiip1_flat = _ivy.matmul(Aitoip1dash_flat, self._AidashtoAis[i + 1]) # BS x 4 x 4 Aiip1 = _ivy.reshape(Aiip1_flat, batch_shape + [4, 4]) # BS x 4 x 4 A0ip1 = _ivy.matmul(A0is[-1][..., 0, :, :], Aiip1) # append term to lists Aitoip1dashs.append(Aitoip1dash) Aiip1s.append(Aiip1) A0is.append(_ivy.expand_dims(A0ip1, -3)) if i + 1 == link_num: # BS x LN x 4 x 4 return _ivy.concatenate(A0is, -3) raise Exception('wrong parameter entered for link_num, please enter integer from 1-' + str(self._num_joints))