Target audience: Advanced

Estimated reading time: 5'

Posts history

Newsletter: Geometric Learning 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).

Contents

Overview

Lie manifolds

Special Euclidean group

Geomstats

Evaluation

Components

Inversion

Composition

References

Appendix

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.

Fig. 1 Manifold with tangent space and exponential/logarithm maps

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.

Fig. 2 Manifold with tangent space and identity and group element

(Courtesy A. Kirillov Jr Department of Mathematics SUNY at Stony Brook)

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,

x\mapsto A(x+b)

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

$SE(3)=\left\{\begin{bmatrix}R&t\\0&1\\\end{bmatrix}\in\mathbb{R}^{4x4}\;|\;R\in SO(3),t\in\mathbb{R}^3\right\}$

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

Table 1. Comparison SO3 and SE3 groups invariance

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.

Fig 3. Visualization of 3x3 rotation matrix and 1x3 translation vector on SE3

Fig 4 Visualization of 4x4 matrix (point) on 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 heatmap.

Fig 5. Visualization of inverse of a 4x4 matrix (point) on SE3 manifold

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

Fig 6. Visualization of the composition of a 4x4 matrix (SE3 manifold point) with itself

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.

Fig 7. Visualization of two SE3 4x4 matrices input to composition

Fig 8. Visualization of the composition of two SE3 4x4 matrices

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)

Thursday, September 26, 2024

Introduction to SE3 Lie Groups in Python

Overview

Lie manifolds

Special Euclidean Group

Geomstats

Evaluation

Components

Inversion

Composition

References

Appendix

Contact Form

Equation Editor

Popular Posts