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 _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 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)
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)