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