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)