def tangent_submersion(vector, point): """Define the tangent space of SE(n) as the kernel of this method. Parameters ---------- vector : array-like, shape=[..., n + 1, n + 1] Point. point : array-like, shape=[..., n + 1, n + 1] Point. Returns ------- submersed_vector : array-like, shape=[..., n + 1, n + 1] Submersed Vector. """ n = point.shape[-1] - 1 rot = point[..., :n, :n] skew = vector[..., :n, :n] vec = vector[..., n, :n] scalar = vector[..., n, n] submersed_rot = Matrices.mul(Matrices.transpose(skew), rot) submersed_rot = Matrices.to_symmetric(submersed_rot) return homogeneous_representation(submersed_rot, vec, point.shape, constant=scalar)
def belongs(self, point, atol=gs.atol): """Check whether point is of the form rotation, translation. Parameters ---------- point : array-like, shape=[..., n, n]. Point to be checked. atol : float Tolerance threshold. Returns ------- belongs : array-like, shape=[...,] Boolean denoting if point belongs to the group. """ n = self.n belongs = Matrices(n + 1, n + 1).belongs(point) if gs.all(belongs): rotation = point[..., :n, :n] belongs = self.rotations.belongs(rotation, atol=atol) last_line_except_last_term = point[..., n:, :-1] all_but_last_zeros = ~gs.any(last_line_except_last_term, axis=(-2, -1)) belongs = gs.logical_and(belongs, all_but_last_zeros) last_term = point[..., n, n] belongs = gs.logical_and(belongs, gs.isclose(last_term, 1., atol=atol)) return belongs
def _log_translation_transform(self, rot_vec): exp_transform = self._exp_translation_transform(rot_vec) inv_determinant = 0.5 / utils.taylor_exp_even_func( rot_vec**2, utils.cosc_close_0, order=4) transform = gs.einsum("...l, ...jk -> ...jk", inv_determinant, Matrices.transpose(exp_transform)) return transform
def submersion(point): """Define SE(n) as the pre-image of identity. Parameters ---------- point : array-like, shape=[..., n + 1, n + 1] Point. Returns ------- submersed_point : array-like, shape=[..., n + 1, n + 1] Submersed Point. """ n = point.shape[-1] - 1 rot = point[..., :n, :n] vec = point[..., n, :n] scalar = point[..., n, n] submersed_rot = Matrices.mul(rot, Matrices.transpose(rot)) return homogeneous_representation( submersed_rot, vec, point.shape, constant=scalar)
def inner_product(self, tangent_vec_a, tangent_vec_b, base_point=None): """Compute inner product of two vectors in tangent space at base point. Parameters ---------- tangent_vec_a : array-like, shape=[..., n, n] First tangent vector at base_point. tangent_vec_b : array-like, shape=[..., n, n] Second tangent vector at base_point. base_point : array-like, shape=[..., n, n] Point in the group. Optional, defaults to identity if None. Returns ------- inner_prod : array-like, shape=[...,] Inner-product of the two tangent vectors. """ return Matrices.frobenius_product(tangent_vec_a, tangent_vec_b)
def inverse(cls, point): """Return the inverse of a point. Parameters ---------- point : array-like, shape=[..., n + 1, n + 1] Point to be inverted. Returns ------- inverse : array-like, shape=[..., n + 1, n + 1] Inverse of point. """ n = point.shape[-1] - 1 transposed_rot = Matrices.transpose(point[..., :n, :n]) translation = point[..., :n, -1] translation = gs.einsum("...ij,...j->...i", transposed_rot, translation) return homogeneous_representation(transposed_rot, -translation, point.shape)