def log(self, point, base_point): """ Compute the Riemannian logarithm at point base_point, of point wrt the metric defined in inner_product. This gives a tangent vector at point base_point. """ point = vectorization.expand_dims(point, to_ndim=3) n_points, _, _ = point.shape base_point = vectorization.expand_dims(base_point, to_ndim=3) n_base_points, mat_dim, _ = base_point.shape assert (n_points == n_base_points or n_points == 1 or n_base_points == 1) sqrt_base_point = np.zeros((n_base_points,) + (mat_dim,) * 2) for i in range(n_base_points): sqrt_base_point[i] = scipy.linalg.sqrtm(base_point[i]) inv_sqrt_base_point = np.linalg.inv(sqrt_base_point) point_near_id = np.matmul(inv_sqrt_base_point, point) point_near_id = np.matmul(point_near_id, inv_sqrt_base_point) log_at_id = group_log(point_near_id) log = np.matmul(sqrt_base_point, log_at_id) log = np.matmul(log, sqrt_base_point) return log
def rotation_vector_from_quaternion(self, quaternion): """ Convert a unit quaternion into a rotation vector. """ quaternion = vectorization.expand_dims(quaternion, to_ndim=2) n_quaternions, _ = quaternion.shape cos_half_angle = quaternion[:, 0] cos_half_angle = np.clip(cos_half_angle, -1, 1) half_angle = np.arccos(cos_half_angle) half_angle = vectorization.expand_dims(half_angle, to_ndim=2, axis=1) assert half_angle.shape == (n_quaternions, 1) rot_vec = np.zeros_like(quaternion[:, 1:]) mask_0 = np.isclose(half_angle, 0) mask_0 = np.squeeze(mask_0, axis=1) mask_not_0 = ~mask_0 rotation_axis = (quaternion[mask_not_0, 1:] / np.sin(half_angle[mask_not_0])) rot_vec[mask_not_0] = (2 * half_angle[mask_not_0] * rotation_axis) rot_vec = self.regularize(rot_vec) return rot_vec
def dist(self, point_a, point_b): """ Compute the Riemannian distance between points point_a and point_b. """ # TODO(nina): case np.dot(unit_vec, unit_vec) != 1 if np.all(point_a == point_b): return 0. point_a = vectorization.expand_dims(point_a, to_ndim=2) point_b = vectorization.expand_dims(point_b, to_ndim=2) n_points_a, _ = point_a.shape n_points_b, _ = point_b.shape assert (n_points_a == n_points_b or n_points_a == 1 or n_points_b == 1) n_dists = np.maximum(n_points_a, n_points_b) dist = np.zeros((n_dists, 1)) norm_a = self.embedding_metric.norm(point_a) norm_b = self.embedding_metric.norm(point_b) inner_prod = self.embedding_metric.inner_product(point_a, point_b) cos_angle = inner_prod / (norm_a * norm_b) mask_cos_greater_1 = np.greater_equal(cos_angle, 1.) mask_cos_less_minus_1 = np.less_equal(cos_angle, -1.) mask_else = ~mask_cos_greater_1 & ~mask_cos_less_minus_1 dist[mask_cos_greater_1] = 0. dist[mask_cos_less_minus_1] = np.pi dist[mask_else] = np.arccos(cos_angle[mask_else]) return dist
def dist(self, point_a, point_b): """ Compute the distance induced on the hyperbolic space, from its embedding in the Minkowski space. """ if np.all(point_a == point_b): return 0. point_a = vectorization.expand_dims(point_a, to_ndim=2) point_b = vectorization.expand_dims(point_b, to_ndim=2) n_points_a, _ = point_a.shape n_points_b, _ = point_b.shape assert (n_points_a == n_points_b or n_points_a == 1 or n_points_b == 1) n_dists = np.maximum(n_points_a, n_points_b) dist = np.zeros((n_dists, 1)) sq_norm_a = self.embedding_metric.squared_norm(point_a) sq_norm_b = self.embedding_metric.squared_norm(point_b) inner_prod = self.embedding_metric.inner_product(point_a, point_b) cosh_angle = -inner_prod / np.sqrt(sq_norm_a * sq_norm_b) mask_cosh_less_1 = np.less_equal(cosh_angle, 1.) mask_cosh_greater_1 = ~mask_cosh_less_1 dist[mask_cosh_less_1] = 0. dist[mask_cosh_greater_1] = np.arccosh(cosh_angle[mask_cosh_greater_1]) return dist
def exp(self, tangent_vec, base_point): """ Compute the Riemannian exponential at point base_point of tangent vector tangent_vec wrt the metric defined in inner_product. This gives a symmetric positive definite matrix. """ tangent_vec = vectorization.expand_dims(tangent_vec, to_ndim=3) n_tangent_vecs, _, _ = tangent_vec.shape base_point = vectorization.expand_dims(base_point, to_ndim=3) n_base_points, mat_dim, _ = base_point.shape assert (n_tangent_vecs == n_base_points or n_tangent_vecs == 1 or n_base_points == 1) sqrt_base_point = np.zeros((n_base_points, mat_dim, mat_dim)) for i in range(n_base_points): sqrt_base_point[i] = scipy.linalg.sqrtm(base_point[i]) inv_sqrt_base_point = np.linalg.inv(sqrt_base_point) tangent_vec_at_id = np.matmul(inv_sqrt_base_point, tangent_vec) tangent_vec_at_id = np.matmul(tangent_vec_at_id, inv_sqrt_base_point) exp_from_id = group_exp(tangent_vec_at_id) exp = np.matmul(exp_from_id, sqrt_base_point) exp = np.matmul(sqrt_base_point, exp) return exp
def regularize_tangent_vec(self, tangent_vec, base_point, metric=None): """ Regularize a tangent_vector by getting its norm at the identity, determined by the metric, to be less than pi, following the regularization convention """ if metric is None: metric = self.left_canonical_metric base_point = self.regularize(base_point) tangent_vec = vectorization.expand_dims(tangent_vec, to_ndim=2) jacobian = self.jacobian_translation( point=base_point, left_or_right=metric.left_or_right) inv_jacobian = np.linalg.inv(jacobian) tangent_vec_at_id = np.dot(tangent_vec, np.transpose(inv_jacobian, axes=(0, 2, 1))) tangent_vec_at_id = np.squeeze(tangent_vec_at_id, axis=1) tangent_vec_at_id = self.regularize_tangent_vec_at_identity( tangent_vec_at_id, metric) regularized_tangent_vec = np.dot(tangent_vec_at_id, np.transpose(jacobian, axes=(0, 2, 1))) regularized_tangent_vec = np.squeeze(regularized_tangent_vec, axis=1) return regularized_tangent_vec
def regularize_tangent_vec_at_identity(self, tangent_vec, metric=None): if metric is None: metric = self.left_canonical_metric tangent_vec = vectorization.expand_dims(tangent_vec, to_ndim=2) tangent_vec_metric_norm = metric.norm(tangent_vec) tangent_vec_canonical_norm = np.linalg.norm(tangent_vec, axis=1) if tangent_vec_canonical_norm.ndim == 1: tangent_vec_canonical_norm = np.expand_dims( tangent_vec_canonical_norm, axis=1) mask_norm_0 = np.isclose(tangent_vec_metric_norm, 0) mask_canonical_norm_0 = np.isclose(tangent_vec_canonical_norm, 0) mask_0 = mask_norm_0 | mask_canonical_norm_0 mask_else = ~mask_0 mask_0 = np.squeeze(mask_0, axis=1) mask_else = np.squeeze(mask_else, axis=1) coef = np.empty_like(tangent_vec_metric_norm) regularized_vec = tangent_vec regularized_vec[mask_0] = tangent_vec[mask_0] coef[mask_else] = (tangent_vec_metric_norm[mask_else] / tangent_vec_canonical_norm[mask_else]) regularized_vec[mask_else] = self.regularize( coef[mask_else] * tangent_vec[mask_else]) regularized_vec[mask_else] = (regularized_vec[mask_else] / coef[mask_else]) return regularized_vec
def matrix_from_quaternion(self, quaternion): """ Convert a unit quaternion into a rotation vector. """ quaternion = vectorization.expand_dims(quaternion, to_ndim=2) n_quaternions, _ = quaternion.shape a, b, c, d = np.hsplit(quaternion, 4) rot_mat = np.zeros((n_quaternions,) + (self.n,) * 2) for i in range(n_quaternions): # TODO(nina): vectorize by applying the composition of # quaternions to the identity matrix column_1 = [a[i] ** 2 + b[i] ** 2 - c[i] ** 2 - d[i] ** 2, 2 * b[i] * c[i] - 2 * a[i] * d[i], 2 * b[i] * d[i] + 2 * a[i] * c[i]] column_2 = [2 * b[i] * c[i] + 2 * a[i] * d[i], a[i] ** 2 - b[i] ** 2 + c[i] ** 2 - d[i] ** 2, 2 * c[i] * d[i] - 2 * a[i] * b[i]] column_3 = [2 * b[i] * d[i] - 2 * a[i] * c[i], 2 * c[i] * d[i] + 2 * a[i] * b[i], a[i] ** 2 - b[i] ** 2 - c[i] ** 2 + d[i] ** 2] rot_mat[i] = np.hstack([column_1, column_2, column_3]).transpose() assert rot_mat.ndim == 3 return rot_mat
def regularize(self, rot_vec): """ Regularize the norm of the rotation vector, to be between 0 and pi, following the axis-angle representation's convention. If the angle angle is between pi and 2pi, the function computes its complementary in 2pi and inverts the direction of the rotation axis. :param rot_vec: 3d vector :returns self.regularized_rot_vec: 3d vector with: 0 < norm < pi """ rot_vec = vectorization.expand_dims(rot_vec, to_ndim=2) assert self.belongs(rot_vec) n_rot_vecs, _ = rot_vec.shape angle = np.linalg.norm(rot_vec, axis=1) regularized_rot_vec = rot_vec.astype('float64') mask_not_0 = angle != 0 k = np.floor(angle / (2 * np.pi) + .5) norms_ratio = np.zeros_like(angle).astype('float64') norms_ratio[mask_not_0] = ( 1. - 2. * np.pi * k[mask_not_0] / angle[mask_not_0]) norms_ratio[angle == 0] = 1 for i in range(n_rot_vecs): regularized_rot_vec[i, :] = norms_ratio[i] * rot_vec[i] assert regularized_rot_vec.ndim == 2 return regularized_rot_vec
def belongs(self, point): """ Check if point belongs to the Minkowski space. """ point = vectorization.expand_dims(point, to_ndim=2) _, point_dim = point.shape return point_dim == self.dimension
def closest_rotation_matrix(mat): """ Compute the closest - in terms of the Frobenius norm - rotation matrix of a given matrix M. This avoids computational errors. :param mat: 3x3 matrix :returns rot_mat: 3x3 rotation matrix. """ mat = vectorization.expand_dims(mat, to_ndim=3) _, mat_dim_1, mat_dim_2 = mat.shape assert mat_dim_1 == mat_dim_2 == 3 mat_unitary_u, diag_s, mat_unitary_v = np.linalg.svd(mat) rot_mat = np.matmul(mat_unitary_u, mat_unitary_v) mask = np.where(np.linalg.det(rot_mat) < 0) new_mat_diag_s = np.tile(np.diag([1, 1, -1]), sum(mask)) rot_mat[mask] = np.matmul(np.matmul(mat_unitary_u[mask], new_mat_diag_s), mat_unitary_v[mask]) assert rot_mat.ndim == 3 return rot_mat
def random_tangent_vec_uniform(self, n_samples=1, base_point=None): if base_point is None: base_point = np.eye(self.dimension) base_point = vectorization.expand_dims(base_point, to_ndim=3) n_base_points, _, _ = base_point.shape assert n_base_points == n_samples or n_base_points == 1 sqrt_base_point = np.zeros_like(base_point) for i in range(n_base_points): sqrt_base_point[i] = scipy.linalg.sqrtm(base_point[i]) tangent_vec_at_id = (2 * np.random.rand(n_samples, self.dimension, self.dimension) - 1) tangent_vec_at_id = (tangent_vec_at_id + np.transpose(tangent_vec_at_id, axes=(0, 2, 1))) tangent_vec = np.matmul(sqrt_base_point, tangent_vec_at_id) tangent_vec = np.matmul(tangent_vec, sqrt_base_point) return tangent_vec
def belongs(self, point): """ Check that the transformation belongs to the special euclidean group. """ point = vectorization.expand_dims(point, to_ndim=2) _, point_dim = point.shape return point_dim == self.dimension
def belongs(self, rot_vec): """ Check that a vector belongs to the special orthogonal group. """ rot_vec = vectorization.expand_dims(rot_vec, to_ndim=2) _, vec_dim = rot_vec.shape return vec_dim == self.dimension
def point_on_geodesic(t): t = vectorization.expand_dims(t, to_ndim=1) t = vectorization.expand_dims(t, to_ndim=2, axis=1) new_initial_point = vectorization.expand_dims(initial_point, to_ndim=point_ndim + 1) new_initial_tangent_vec = vectorization.expand_dims( initial_tangent_vec, to_ndim=point_ndim + 1) n_times_t, _ = t.shape tangent_vec_shape = new_initial_tangent_vec.shape[1:] tangent_vecs = np.zeros((n_times_t, ) + tangent_vec_shape) for i in range(n_times_t): tangent_vecs[i] = t[i] * new_initial_tangent_vec point_at_time_t = self.exp(tangent_vec=tangent_vecs, base_point=new_initial_point) return point_at_time_t
def plot(points, ax=None, space=None, **point_draw_kwargs): """ Plot points in the 3D Special Euclidean Group, by showing them as trihedrons. """ if space not in IMPLEMENTED: raise NotImplementedError( 'The plot function is not implemented' ' for space {}. The spaces available for visualization' ' are: {}.'.format(space, IMPLEMENTED)) if points is None: raise ValueError("No points given for plotting.") points = vectorization.expand_dims(points, to_ndim=2) if ax is None: if space is 'SE3_GROUP': ax_s = AX_SCALE * np.amax(np.abs(points[:, 3:6])) elif space is 'SO3_GROUP': ax_s = AX_SCALE * np.amax(np.abs(points[:, :3])) else: ax_s = AX_SCALE if space is 'H2': ax = plt.subplot(aspect='equal') plt.setp(ax, xlim=(-ax_s, ax_s), ylim=(-ax_s, ax_s), xlabel='X', ylabel='Y') else: ax = plt.subplot(111, projection='3d', aspect='equal') plt.setp(ax, xlim=(-ax_s, ax_s), ylim=(-ax_s, ax_s), zlim=(-ax_s, ax_s), xlabel='X', ylabel='Y', zlabel='Z') if space in ('SO3_GROUP', 'SE3_GROUP'): trihedrons = convert_to_trihedron(points, space=space) for t in trihedrons: t.draw(ax, **point_draw_kwargs) elif space is 'S2': sphere = Sphere() sphere.add_points(points) sphere.draw(ax) elif space is 'H2': poincare_disk = PoincareDisk() poincare_disk.add_points(points) poincare_disk.draw(ax) return ax
def quaternion_from_matrix(self, rot_mat): """ Convert a rotation matrix into a unit quaternion. """ rot_mat = vectorization.expand_dims(rot_mat, to_ndim=3) rot_vec = self.rotation_vector_from_matrix(rot_mat) quaternion = self.quaternion_from_rotation_vector(rot_vec) assert quaternion.ndim == 2 return quaternion
def extrinsic_to_intrinsic_coords(self, point_extrinsic): """ From the extrinsic coordinates in Minkowski space, to the extrinsic coordinates in Hyperbolic space. """ point_extrinsic = vectorization.expand_dims(point_extrinsic, to_ndim=2) assert np.all(self.belongs(point_extrinsic)) point_intrinsic = point_extrinsic[:, 1:] assert point_intrinsic.ndim == 2 return point_intrinsic
def is_symmetric(mat, tolerance=TOLERANCE): """Check if a matrix is symmetric.""" mat = vectorization.expand_dims(mat, to_ndim=3) n_mats, _, _ = mat.shape mask = np.zeros(n_mats, dtype=bool) for i in range(n_mats): mask[i] = np.allclose(mat[i], np.transpose(mat[i]), atol=tolerance) return mask
def extrinsic_to_intrinsic_coords(self, point_extrinsic): """ From the extrinsic coordinates in Euclidean space, to some intrinsic coordinates in Hypersphere. """ point_extrinsic = vectorization.expand_dims(point_extrinsic, to_ndim=2) assert np.all(self.belongs(point_extrinsic)) point_intrinsic = point_extrinsic[:, 1:] assert point_intrinsic.ndim == 2 return point_intrinsic
def group_exp(self, tangent_vec, base_point=None): """ Compute the group exponential of vector tangent_vector. """ base_point = self.regularize(base_point) tangent_vec = vectorization.expand_dims(tangent_vec, to_ndim=2) point = super(SpecialOrthogonalGroup, self).group_exp( tangent_vec=tangent_vec, base_point=base_point) point = self.regularize(point) return point
def inner_product(self, tangent_vec_a, tangent_vec_b, base_point=None): """ Inner product defined by the Riemannian metric at point base_point between tangent vectors tangent_vec_a and tangent_vec_b. """ tangent_vec_a = vectorization.expand_dims(tangent_vec_a, to_ndim=2) tangent_vec_b = vectorization.expand_dims(tangent_vec_b, to_ndim=2) inner_prod_mat = self.inner_product_matrix(base_point) inner_prod_mat = vectorization.expand_dims(inner_prod_mat, to_ndim=3) n_tangent_vecs_a = tangent_vec_a.shape[0] n_tangent_vecs_b = tangent_vec_b.shape[0] n_inner_prod_mats = inner_prod_mat.shape[0] bool_all_same_n = (n_tangent_vecs_a == n_tangent_vecs_b == n_inner_prod_mats) bool_a = n_tangent_vecs_a == 1 bool_b = n_tangent_vecs_b == 1 bool_inner_prod = n_inner_prod_mats == 1 assert (bool_all_same_n or n_tangent_vecs_a == n_tangent_vecs_b and bool_inner_prod or n_tangent_vecs_a == n_inner_prod_mats and bool_b or n_tangent_vecs_b == n_inner_prod_mats and bool_a or bool_a and bool_b or bool_a and bool_inner_prod or bool_b and bool_inner_prod) n_inner_prods = np.amax( [n_tangent_vecs_a, n_tangent_vecs_b, n_inner_prod_mats], axis=0) inner_prod = np.zeros((n_inner_prods, 1)) for i in range(n_inner_prods): tangent_vec_a_i = (tangent_vec_a[0] if n_tangent_vecs_a == 1 else tangent_vec_a[i]) tangent_vec_b_i = (tangent_vec_b[0] if n_tangent_vecs_b == 1 else tangent_vec_b[i]) inner_prod_mat_i = (inner_prod_mat[0] if n_inner_prod_mats == 1 else inner_prod_mat[i]) inner_prod[i] = np.dot(np.dot(tangent_vec_a_i, inner_prod_mat_i), tangent_vec_b_i.transpose()) return inner_prod
def inner_product(self, tangent_vec_a, tangent_vec_b, base_point): """ Compute the inner product of tangent_vec_a and tangent_vec_b at point base_point using the affine invariant Riemannian metric. """ inv_base_point = np.linalg.inv(base_point) aux_a = np.matmul(inv_base_point, tangent_vec_a) aux_b = np.matmul(inv_base_point, tangent_vec_b) inner_product = np.trace(np.matmul(aux_a, aux_b), axis1=1, axis2=2) inner_product = vectorization.expand_dims(inner_product, to_ndim=2, axis=1) return inner_product
def exponential_matrix(self, rot_vec): """ Compute the exponential of the rotation matrix represented by rot_vec. :param rot_vec: 3D rotation vector :returns exponential_mat: 3x3 matrix """ rot_vec = self.rotations.regularize(rot_vec) n_rot_vecs, _ = rot_vec.shape angle = np.linalg.norm(rot_vec, axis=1) angle = vectorization.expand_dims(angle, to_ndim=2, axis=1) skew_rot_vec = so_group.skew_matrix_from_vector(rot_vec) coef_1 = np.empty_like(angle) coef_2 = np.empty_like(coef_1) mask_0 = np.equal(angle, 0) mask_0 = np.squeeze(mask_0, axis=1) mask_close_to_0 = np.isclose(angle, 0) mask_close_to_0 = np.squeeze(mask_close_to_0, axis=1) mask_else = ~mask_0 & ~mask_close_to_0 coef_1[mask_close_to_0] = (1. / 2. - angle[mask_close_to_0] ** 2 / 24.) coef_2[mask_close_to_0] = (1. / 6. - angle[mask_close_to_0] ** 3 / 120.) # TODO(nina): check if the discountinuity as 0 is expected. coef_1[mask_0] = 0 coef_2[mask_0] = 0 coef_1[mask_else] = (angle[mask_else] ** (-2) * (1. - np.cos(angle[mask_else]))) coef_2[mask_else] = (angle[mask_else] ** (-2) * (1. - (np.sin(angle[mask_else]) / angle[mask_else]))) term_1 = np.zeros((n_rot_vecs, self.n, self.n)) term_2 = np.zeros_like(term_1) for i in range(n_rot_vecs): term_1[i] = np.eye(self.n) + skew_rot_vec[i] * coef_1[i] term_2[i] = np.matmul(skew_rot_vec[i], skew_rot_vec[i]) * coef_2[i] exponential_mat = term_1 + term_2 assert exponential_mat.ndim == 3 return exponential_mat
def log(self, point, base_point=None): """ Riemannian logarithm at point base_point of tangent vector tangent_vec wrt the Riemannian metric. """ point = vectorization.expand_dims(point, to_ndim=2) base_point = vectorization.expand_dims(base_point, to_ndim=2) n_points, _ = point.shape n_base_points, point_dim = base_point.shape assert (n_points == n_base_points or n_points == 1 or n_base_points == 1) n_logs = np.maximum(n_points, n_base_points) log = np.zeros((n_logs, point_dim)) for i in range(n_logs): base_point_i = (base_point[0] if n_base_points == 1 else base_point[i]) point_i = (point[0] if n_points == 1 else point[i]) log[i] = self.log_basis(point_i, base_point_i) return log
def regularize_tangent_vec(self, tangent_vec, base_point, metric=None): """ Regularize an element of the group SE(3), by extracting the rotation vector r from the input [r t] and using self.rotations.regularize. :param point: 6d vector, element in SE(3) represented as [r t]. :returns self.regularized_point: 6d vector, element in SE(3) with self.regularized rotation. """ if metric is None: metric = self.left_canonical_metric tangent_vec = vectorization.expand_dims(tangent_vec, to_ndim=2) base_point = vectorization.expand_dims(base_point, to_ndim=2) rotations = self.rotations dim_rotations = rotations.dimension rot_tangent_vec = tangent_vec[:, :dim_rotations] rot_base_point = base_point[:, :dim_rotations] metric_mat = metric.inner_product_mat_at_identity rot_metric_mat = metric_mat[:dim_rotations, :dim_rotations] rot_metric = InvariantMetric( group=rotations, inner_product_mat_at_identity=rot_metric_mat, left_or_right=metric.left_or_right) regularized_vec = np.zeros_like(tangent_vec) regularized_vec[:, :dim_rotations] = rotations.regularize_tangent_vec( tangent_vec=rot_tangent_vec, base_point=rot_base_point, metric=rot_metric) regularized_vec[:, dim_rotations:] = tangent_vec[:, dim_rotations:] return regularized_vec
def belongs(self, mat, tolerance=TOLERANCE): """ Check if a matrix belongs to the manifold of symmetric positive definite matrices. """ mat = vectorization.expand_dims(mat, to_ndim=3) n_mats, mat_dim, _ = mat.shape mask_is_symmetric = is_symmetric(mat, tolerance=tolerance) eigenvalues = np.zeros((n_mats, mat_dim)) eigenvalues[mask_is_symmetric] = np.linalg.eigvalsh( mat[mask_is_symmetric]) mask_pos_eigenvalues = np.all(eigenvalues > 0) return mask_is_symmetric & mask_pos_eigenvalues
def exp_from_identity(self, tangent_vec): """ Compute the Riemannian exponential from the identity of the Lie group of tangent vector tangent_vec. """ tangent_vec = vectorization.expand_dims(tangent_vec, to_ndim=2) if self.left_or_right == 'left': exp = self.left_exp_from_identity(tangent_vec) else: opp_left_exp = self.left_exp_from_identity(-tangent_vec) exp = self.group.inverse(opp_left_exp) exp = self.group.regularize(exp) return exp
def intrinsic_to_extrinsic_coords(self, point_intrinsic): """ From the intrinsic coordinates in the hyperbolic space, to the extrinsic coordinates in Minkowski space. """ point_intrinsic = vectorization.expand_dims(point_intrinsic, to_ndim=2) n_points, _ = point_intrinsic.shape dimension = self.dimension point_extrinsic = np.zeros((n_points, dimension + 1)) point_extrinsic[:, 1:dimension + 1] = point_intrinsic[:, 0:dimension] point_extrinsic[:, 0] = np.sqrt( 1. + np.linalg.norm(point_intrinsic, axis=1)**2) assert np.all(self.belongs(point_extrinsic)) assert point_extrinsic.ndim == 2 return point_extrinsic
def belongs(self, mat): """ Check if mat belongs to the Matrix Lie group. Note: - By default, check that the matrix is invertible. - Need override for any matrix Lie group that is not GL(n). """ mat = vectorization.expand_dims(mat, to_ndim=3) n_mats, _, _ = mat.shape mat_rank = np.zeros((n_mats, 1)) for i in range(n_mats): mat_rank[i] = np.linalg.matrix_rank(mat[i]) return mat_rank == self.n