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)
