Introduction to SE3 Lie Groups in Python

After years of feeling daunted by?Lie groups?and?algebras, I finally took the plunge into exploring these fascinating smooth manifolds. This article offers an introduction to the widely-used 3D?Special Euclidean group(SE3).

What you will learn: ?How to calculate an element of the 3D Special Euclidean group (SE3) from a given rotation matrix and translation vector in the tangent space, including the implementation of operations for computing the inverse and composition of SE3 group elements.

Notes:?

• This post is a follow up on articles related to differential geometry and geometry learning [ref?1,?2,?3?and?4] and introduction to 3-dimension Special Orthogonal group [ref?5].
• Environments:?Python 3.11, ?Matplotlib 3.9, Geomstats 2.8.0, Numpy 2.1.2
• Source code is available at ?Github.com/patnicolas/Data_Exploration/Lie
• To enhance the readability of the algorithm implementations, we have omitted non-essential code elements like error checking, comments, exceptions, validation of class and method arguments, scoping qualifiers, and import statement.

Disclaimer?:?A thorough tutorial and explanation of Lie groups, Lie algebras, and geometric priors for deep learning models is beyond the scope of this article. Instead, the following sections concentrate on experiments involving key elements and operations on Lie groups using the Geomstats Python library [ref?6].

Overview

Lie manifolds

A?smooth manifold?is a topological space that locally resembles Euclidean space and allows for smooth (infinitely differentiable) transitions between local coordinate systems. This structure allows for the use of calculus on the manifold.?

The?tangent space?at a point on a manifold is the set of tangent vectors at that point, like a line tangent to a circle or a plane tangent to a surface.

Tangent vectors?can act as directional derivatives, where you can apply specific formulas to characterize these derivatives.

In differential geometry, a?Lie group?is a mathematical structure that combines the properties of both a group and a smooth manifold. It allows for the application of both algebraic and geometric techniques. As a group, it has an operation (like multiplication) that satisfies certain axioms (closure, associativity, identity, and invertibility) [ref?7].

A 'real' Lie group is a set?G?with two structures:?G?is a group and?G?is a (smooth, real) manifold. These structures agree in the following sense multiplication (a.k.a.?product or?composition) and?inversion?are smooth maps.

A morphism of Lie groups is a smooth map which also preserves the group operation:? f(gh) =?f(g)f(h)?and?f(1) = 1.

Special Euclidean Group

The?Euclidean group?is a subset of the broader?affine transformation group. It contains the translational and orthogonal groups as subgroups. Any element of SE(n) can be represented as a combination of a translation and an orthogonal transformation, where the translation?B?can either precede or follow the orthogonal transformation?A,

The 3-dimension Special Euclidean group (SE3) is described as a 4x4 matrix as?

A previous article,?Special Orthogonal Lie group SO3 ?introduced the 3-dimension Special Orthogonal Lie group (SO3). How SE3 group differs from SO3?

Geomstats

Geomstats?is a free, open-source Python library designed for conducting machine learning on data situated on nonlinear manifolds, an area known as Geometric Learning. This library offers object-oriented, thoroughly unit-tested features for fundamental manifolds, operations, and learning algorithms, compatible with various execution environments, including?NumPy,?PyTorch, and?TensorFlow (Overview Geomstats library ).

The library is structured into two principal components:

• geometry: This part provides an object-oriented framework for crucial concepts in differential geometry, such as exponential and logarithm maps, parallel transport, tangent vectors, geodesics, and Riemannian metrics.
• learning: This section includes statistics and machine learning algorithms tailored for manifold data, building upon the?scikit-learn?framework.

Evaluation

The purpose of this section is to demonstrate that the inverse of a SE3 element and the composition of two SE3 elements belong to SE3 manifold.?

Components

We adopt the same object-oriented approach as used with the Special Orthogonal Group to describe the components and operations on the?SE(3) manifold. The?LieSE3Group?class encapsulates the definition of the Special Euclidean group and its associated operations.

We specify three constructors:

• init: Default constructor that create a new element in the SE3 manifold,?group_element, using a 4x4 matrix as rotation+translation matrix on the tangent space.
• build_from_numpy:?Alternative constructor?with a 3x3 Numpy array list for the rotation matrix and 1x3 Numpy array for the translation vector on the tangent space?
• build_from_vec:?Alternative constructor?with a 9 elements list for rotation matrix and a 3 elements list as translation vector on the tangent space.

import geomstats.backend as gs
from geomstats.geometry.special_euclidean import SpecialEuclidean


class LieSE3Group(object):
     dim = 3
     # Lie group as defined in Geomstats library
     lie_group = SpecialEuclidean(n=dim, 
                                                        point_type='matrix', 
                                                        epsilon=0.15, 
                                                        equip=False)
    # Support conversion of rotation matrix and translation vector to 4x4 matrix
    extend_rotation = np.array([[0.0, 0.0, 0.0]])
    extend_translation = np.array([[1.0]])

   
    # Default constructor with 4x4 matrix on tangent space as argument
    def __init__(self, se3_element: np.array) -> None:
        self.se3_element = se3_element
        # Apply the exponential map to generate a point on the SE3 manifold
        self.group_element = LieSE3Group.lie_group.exp(self.se3_element)


   # Constructor with 3x3 numpy array for rotation matrix and  
   # 1x3 numpy array as translation vector on the tangent space 
   @classmethod
   def build_from_numpy(cls, rot_matrix: np.array, trans_matrix: np.array) -> Self:
        rotation_matrix = gs.array(rot_matrix)
        translation_matrix = gs.array(trans_matrix)
        se3_element = LieSE3Group.__build_se3_matrix(rotation_matrix, 
                                                                                              translation_matrix)
      
        return cls(se3_element)



   # Constructor with a 9 elements list for rotation matrix and  a
   # 3 elements list as translation vector on the tangent space 
   @classmethod
   def build_from_vec(cls, rot_matrix: List[float], trans_vector: List[float]) -> Self:
        np_rotation_matrix = np.reshape(rot_matrix, (3, 3))
        np_translation_matrix = LieSE3Group.__convert_to_matrix(trans_vector)
       
        return LieSE3Group.build_from_numpy(np_rotation_matrix, 
                                                                             np_translation_matrix)        

The method?build_from_numpy?invoked the private static method,?__build_se3_matrix?to build the 4x4 numpy array from the 3x3 rotation matrix and 1x3 translation vector. Its implemented is included in the appendix.

The generation of point on SE3 manifold uses a rotation around Z axis (rot_matrix) and a translation along each of the 3 axis (trans_vector).

rot_matrix = [1.0, 0.0, 0.0, 0.0, 0.0, -1.0, 0.0, 1.0, 0.0]
trans_vector = [0.5, 0.3, 0.4]
print(f'\nRotation matrix:\n{np.reshape(rot_matrix, (3, 3))}')
print(f'Translation vector: {trans_vector}')

lie_se3_group = LieSE3Group.build_from_vec(rot_matrix, trans_vector)
print(lie_se3_group)
lie_se3_group.visualize_all(rot_matrix, trans_vector)        

Output:

Rotation matrix: [[ 1. ?0. ?0.] ?[ 0. ?0. -1.] ?[ 0. ?1. ?0.]]

Translation vector:?[0.5, 0.8, 0.6]

SE3 tangent space:

[[ 1.0 ?0.0 ?0.0 ?0.5]

?[ 0.0 ?0.0 -1.0 ?0.8]

?[ 0.0 ?1.0 ?0.0 ?0.6]

?[ 0.0 ?0.0 ?0.0 ?1.0]]

SE3 point:

[[ 2.718 ? 0.000 ? 0.000 ? 1.359]

?[ 0.000 ? 0.540 ?-0.841 ? 0.806]

?[ 0.000 ? 0.841 ? 0.540 ? 1.440]

?[ 0.000 ? 0.000 ? 0.000 ? 2.718]]

The following plots illustrates the two inputs (rotation matrix and translation vector) on the tangent space and the resulting point on the SE3 manifold.

Inversion

Let's validate that the inverse of an element on SE3 Lie group belongs to a SE3 group. The?implementation,?LieSE3Group?method?inverse, relies on the SpecialEuclidean.inverse ?method of Geomstats library.

def inverse(self) -> Self:
    inverse_group_point = LieSE3Group.lie_group.inverse(self.group_element)
    return LieSE3Group(inverse_group_point)        

We reuse the 3x3 orthogonal?rotation around Z axis (rot_matrix) with a new translation vector [0.5, 0.8, 0.6] on the SE3 tangent space to generate point on the manifold and its inverse.

rot_matrix = [1.0, 0.0, 0.0, 0.0, 0.0, -1.0, 0.0, 1.0, 0.0]
trans_vector = [0.5, 0.8, 0.6]
lie_se3_group = LieSE3Group.build_from_vec(rot_matrix, trans_vector)
   
inv_lie_se3_group = lie_se3_group.inverse()
print(f'\nSE3 element\n{lie_se3_group}\nInverse\n{inv_lie_se3_group}')
lie_se3_group.visualize(inv_lie_se3_group.group_element, 'Inverse')        

Input

SE3 tangent space:

[[ 1.0 ?0.0 ?0.0 ?0.5]

?[ 0.0 ?0.0 -1.0 ?0.8]

?[ 0.0 ?1.0 ?0.0 ?0.6]

?[ 0.0 ?0.0 ?0.0 ?1.0]]

SE3 point:

[[ 2.718 ? 0.000 ? 0.000 ? 1.359]

?[ 0.000 ? 0.540 ?-0.841 ? 0.806]

?[ 0.000 ? 0.841 ? 0.540 ? 1.440]

?[ 0.000 ? 0.000 ? 0.000 ? 2.718]]

Inverse SE3 point

[[ 15.154 ? ?0.000 ? 0.000 ?-26.738]

?[ ? 0.000 ? ?1.143 ? 1.279 ? ?-3.307]

?[ ? 0.000 ? -1.279 ? 1.143 ? ? 1.125]

?[ ? 0.000 ? ?0.000 ? 0.000 ? ? 2.718]]

The inverse on the SE3 manifold is visualizes in the following heat map.

Composition

The second key property of a Lie group on a manifold is that the composition of two group elements also belongs to the group. The?product?method in the?LieSE3Group?class performs?this?operation by composing the current 4x4 SE(3) matrix with another SE(3) element (denoted as?lie_se3_group).

def product(self, lie_se3_group: Self) -> Self:
     composed_group_point = LieSE3Group.lie_group.compose(
                 self.group_element,  
                 lie_se3_group.group_element)
     return LieSE3Group(composed_group_element)        

First test

Let's compose this SE3 element with itself.

rot_matrix = [1.0, 0.0, 0.0, 0.0, 0.0, -1.0, 0.0, 1.0, 0.0]
trans_vector = [0.5, 0.8, 0.6]
se3_group = LieSE3Group.build_from_vec(rot_matrix, trans_vector)

# Composition of the same matrix
se3_group_product = se3_group.product(se3_group)
print(f'\nComposed SE3 point:\:{se3_group_product}')        

SE3 tangent space:

[[ 7.389 ? 0.000 ? ?0.000 ? 7.389 ]

?[ 0.000 ?-0.416 ? -0.909 ? 1.417]

?[ 0.000 ? 0.909 ? -0.416 ? ?5.372]

?[ 0.000 ? 0.000 ? ?0.000 ? ?7.389]]

Composed SE3 point:

[[ 1618.174 ? ?0.000 ? ? 0.000 ?119568.075]

?[ ? ? ? 0.000 ?40.518 ?-52.045 ? ? ? 162.141]

?[ ? ? ? 0.000 ?52.045 ? 40.518 ? ? ? ?113.238]

?[ ? ? ? 0.000 ? ?0.000 ? ? 0.000 ? ? ?1618.174 ]]

Second test:

We compose a combine 3x3 rotation matrix (rotation around z axis)?rot1_matrix?and translation vector?trans1_vector?= [0.5, 0.8. 0.6] with a 3x3 rotation matrix (x axis),?rot2_matrix?and translation vector?trans2_vector?= [0.1, -0.3, 0.3].

rot1_matrix = [1.0, 0.0, 0.0, 0.0, 0.0, -1.0, 0.0, 1.0, 0.0]
trans1_vector = [0.5, 0.8, 0.6]
se3_group1 = LieSE3Group.build_from_vec(rot1_matrix, trans1_vector)
print(f'\nFirst SE3 matrix:{se3_group1}')

rot2_matrix = [0.0, -1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0]
trans2_vector = [0.1, -0.3, 0.3]
se3_group2 = LieSE3Group.build_from_vec(rot2_matrix, trans2_vector)
print(f'\nSecond SE3 matrix:{se3_group2}')

se3_composed_group = se3_group1.product(se3_group1)
print(f'\nComposed SE3 matrix:{se3_composed_group}')        

First SE3 matrix:

SE3 tangent space:

[[ 1. ? 0. ? 0. ? 0.5]

?[ 0. ? 0. ?-1. ? 0.8]

?[ 0. ? 1. ? 0. ? 0.6]

?[ 0. ? 0. ? 0. ? 1. ]]

SE3 point:

[[ 2.718 ? 0.000 ? 0.000 ?1.359 ]

?[ 0.000 ? 0.540 ?-0.841 ?0.806 ]

?[ 0.000 ? 0.841 ? 0.540 ?1.440 ]

?[ 0.000 ? 0.000 ? 0.000 ? 2.718 ]]

Second SE3 matrix:

SE3 tangent space:

[[ 0. ?-1. ? 0. ? 0.1]

?[ 1. ? 0. ? 0. ?-0.3]

?[ 0. ? 0. ? 1. ? 0.3]

?[ 0. ? 0. ? 0. ? 1. ]]

SE3 point:

[[ 0.540 ?-0.841 ? 0.000 ? 0.351 ]

?[ 0.841 ? 0.540 ? 0.000 ?-0.386 ]

?[ 0.000 ? 0.000 ? 2.718 ? 0.815 ]

?[ 0.000 ? 0.000 ? 0.000 ? 2.718 ]]

Composed SE3 matrix:

SE3 tangent space:

[[ 7.389 ? 0.000 ? 0.000 ? 7.389 ]

?[ 0.000 ?-0.416 ?-0.909 ?1.417 ]

?[ 0.000 ? 0.909 ?-0.416 ? 5.372 ]

?[ 0.000 ? 0.000 ? 0.000 ? 7.389 ]]

SE3 point:

[[ 1618.177 ? ?0.000 ? 0.000 ?11956.808 ]

?[ ? ? ? 0.000 ? ?0.405 ?-0.520 ? ? 162.141 ]

?[ ? ? ? 0.000 ? ?0.520 ? 0.405 ? ?1132.388 ]

?[ ? ? ? 0.000 ? ?0.000 ? 0.000 ? ?1618.177 ]]

The following diagram visualizes the two input SE3 group elements used in the composition,?se3_group1,?se3_group2??and the resulting SE3 element se3_composed_group.

References

[1]?Foundation of Geometric Learning

[2]?Differentiable Manifolds for Geometric Learning

[3]?Intrinsic Representation in Geometric Learning

[4]?Riemann Metric and Connection

[5]?Operation on SO3 Lie Group in Python

[6]?Introduction to Geometric Learning in Python with Geomstats

[7]?Introduction to Lie Groups and Lie Algebras - A. Kirillov, Jr - SUNY at Stony Brook

----------------------------------

Patrick Nicolas has over 25 years of experience in software and data engineering, architecture design and end-to-end deployment and support with extensive knowledge in machine learning.? He has been director of data engineering at Aideo Technologies since 2017 and he is the?author of "Scala for Machine Learning", Packt Publishing ISBN 978-1-78712-238-3 and?Geometric Learning in Python ?Newsletter on LinkedIn.

Appendix

A simple class method to build a 4 x 4 matrix on SE3 tangent space, from a 3x3 rotation matrix and 1x3 translation vector.

@staticmethod
def __build_se3_matrix(rot_matrix: np.array, trans_matrix: np.array) -> np.array:
     extended_rot = np.concatenate([rot_matrix, LieSE3Group.extend_rotation], 
                                                             axis=0)
     extended_trans = np.concatenate(
                                      [trans_matrix.T, LieSE3Group.extend_translation]
     )
     return np.concatenate([extended_rot, extended_trans], axis=1)