Showing posts with label SO3. Show all posts
Showing posts with label SO3. Show all posts

Tuesday, December 10, 2024

Fréchet Centroid on Manifolds in Python

  Target audience: Intermediate
Estimated reading time: 5'


The Fréchet centroid (or intrinsic centroid) is a generalization of the concept of a mean to data points that lie on a manifold or in a non-Euclidean space. It minimizes a similar quantity defined using the intrinsic geometry of the manifold.


Follow me on LinkedIn

What you will learn: How to compute the Frechet centroid (or mean) of multiple point on a data manifold.

Notes

  • Environments: Python 3.12.5,  GeomStats 2.8.0, Matplotlib 3.9, Numpy 2.2.0
  • Source code is available on GitHub [ref 1]
  • 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.


Introduction

For readers unfamiliar with manifolds and basic differential geometry, I highly recommend starting with my two introductory posts:

  1. Foundation of Geometric Learning This post introduces differential geometry in the context of machine learning, outlining its foundational components.
  2. Differentiable Manifolds for Geometric Learning: This post explores manifold concepts such as tangent vectors and geodesics, with Python implementations for the hypersphere using the Geomstats library.
Alternatively, I strongly encourage readers to consult tutorials [ref 2], video series [ref 3], or publications [ref 4, 5] for more in-depth information.

The following image illustrates the difference between the Euclidean and  Fréchet  means given a manifold.


Fig. 1. Illustration of Euclidean and Frechet means on an arbitrary manifold


Let's consider n tensors x[i] with d values, the Euclidean mean is computed as
\[\mathbf{\mu} = \frac{1}{n}\sum_{i=1}^{n} \mathbf{x}_{i} \] with for each  coordinate/value of index j: \[\mu^{(j)}=\frac{1}{n}\sum_{i=1}^{n}x_{i}^{(j)} \] 

For a manifold M and a metric-distance d the  Fréchet centroid is defined as \[ m=arg\displaystyle \min_{p \in M}\sum_{i=1}^{n}d^{2}\left(p, \mathbf{x_{i}} \right) \] and the weighted Frechet mean as: \[m=arg\displaystyle \min_{p \in M}\sum_{i=1}^{n}w_{i}d^{2}\left(p, \mathbf{x_{i}} \right) \]


Implementation

Let's explore the computation of means from the perspective of extrinsic geometry, focusing on surfaces embedded within a three-dimensional Euclidean space.
To begin, we'll define a class `FrechetEstimator` to encapsulate the essential components of the estimator:
  • space: A smooth manifold (e.g., Sphere, Hyperbolic) or a Lie group (e.g., SO3, SE3).  
  • optimizer: An optimization algorithm used to iterate through the space and minimize the sum of squared distances.  
  • weights: Optional weights that can be applied during the mean estimation process.

Note: The constructor invoke the Geomstats class FrechetMean associated to the given manifold


class FrechetEstimator(object):
    def __init__(self, space: Manifold, optimizer: BaseGradientDescent, weights: Tensor = None) -> None:
        self.frechet_mean = FrechetMean(space)
        self.frechet_mean.optimizer = optimizer
        self.weights = weights
        self.space = space

    def estimate(self, X: List[np.array]) -> np.array:
    
    def rand(self, num_samples: int) -> List[np.array]:


The `rand` method generates random points on the manifold for evaluation purposes, utilizing the `random_uniform` function from the Geomstats library.

The implementation ensures that:
- The manifold type is supported.  
- All randomly generated points (as tensors) are verified to belong to the manifold.

def rand(self, num_samples: int) -> np.array:
    from geomstats.geometry.hypersphere import Hypersphere
    from geomstats.geometry.special_orthogonal import _SpecialOrthogonalMatrices

    # Test if this manifold is supported
    if not (isinstance(self.space, Hypersphere) or 
              isinstance(self.space, _SpecialOrthogonalMatrices)):
        raise GeometricException('Cannot generate random values on unsupported manifold')

    X = self.space.random_uniform(num_samples)
    # Validate the randomly generated belongs to the manifold 'self.space'
    are_points_valid = all([self.space.belongs(x) for x in X])
    if not are_points_valid:
        raise GeometricException('Some generated points do not belong to the manifold')
    return X

Our evaluation uses the Euclidean mean as a baseline, with a straightforward implementation using NumPy.


def euclidean_mean(manifold_points: List[Tensor]) -> np.array:
      return np.mean(manifold_points, axis=0)


The computation of the Fréchet centroid on a sequence of data points defined as stacked numpy arrays, is implemented in method, estimate. It relies on the Geomstats method FrechetMean.fit

def estimate(self, X: np.array) -> np.array:
    self.frechet_mean.fit(X=X, y=None, weights=self.weights)
    return self.frechet_mean.estimate_

Evaluation

Centroid on Sphere (S2)

Our initial test involves calculating the non-weighted Fréchet centroid for 7 points located on a hypersphere [ref 6]. The randomly generated points, `rand_points`, are visualized both on a 3D sphere and in Euclidean space using the `HyperspherPlot` and `EuclideanPlot` classes. While the code is not included here for clarity, it is available on GitHub [ref 1].


frechet_estimator = FrechetEstimator(space=Hypersphere(dim=2),
                                                            optimizer=GradientDescent(), 
                                                            weights=None)
# 1- Generate the random point on the hypersphere
rand_points = frechet_estimator.rand(8)

# 2- Estimate the Frechet centroid then test if belongs to the manifold
frechet_mean = frechet_estimator.estimate(rand_points)
assert frechet_estimator.space.belongs(frechet_mean)
# 3- Display the points on the Hypersphere on plot hypersphere_plot = HyperspherePlot(rand_points,  
frechet_mean)
hypersphere_plot.show()

# 4- Compute the euclidean or arithmetic centroid
euclidean_mean = FrechetEstimator.euclidean_mean(np_points)

# 5- Display the points in 3D Euclidean space
euclidean_plot = EuclideanPlot(rand_points, euclidean_mean
euclidean_plot.show()

print(f'\nFrechet mean:   {frechet_mean}\nEuclidean mean: {euclidean_mean}')


Output:

Frechet mean:     [ 0.0174    -0.9958   -0.0897]

Euclidean mean: [ 0.0220    -0.0792     0.0226]


The 8 random points are visualized using Matplotlib 3D scatter plot.


'

Fig 2. Visualization of points from a 2-dimension hypersphere on 3D Euclidean space


The next graph visualizes the 8 random points on a 3D Sphere.

Fig 3. Visualization of random points on a 2-dimension hypersphere



Centroid on Special Orthogonal Group (SO3)

A special Orthogonal Group is a Lie group. 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).
The Special Orthogonal Group in 3 dimensions, SO(3) is the group of all rotation matrices in 3 spatial dimensions.
It can be defined by 3 rotation elements for each of the axis of rotation x, y, and z.

Important note: The SO(3) manifold is described and evaluated with a Python source code in a previous post [ref 7].


We utilize the Geomstats `SpecialOrthogonal` class to generate 4 random matrices, then compute their Euclidean and Fréchet centroids.

manifold = SpecialOrthogonal(n=3, point_type="matrix")
frechet_estimator = FrechetEstimator(manifold, GradientDescent(), weights=None)

# Generate the random point on SO3
manifold_points = frechet_estimator.rand(4)

# Visualize the SO3 matrices on 3D plot
so3_plot = SO3Plot(manifold_points) so3_plot.show()

# Compute the Frechet and Euclidean centroids
frechet_mean = frechet_estimator.estimate(manifold_points) euclidean_mean = FrechetEstimator.euclidean_mean(manifold_points) print(f'\nFrechet mean:\n{frechet_mean}\nEuclidean mean:\n{euclidean_mean}')

Output:
Frechet mean:   
[[-0.74841 -0.40675 -0.52385]
 [-0.01445 -0.77966  0.62603]
 [-0.66306  0.47610  0.57763]]
Euclidean mean: 
[[-0.52282 -0.24432 -0.38322]
 [ 0.03642 -0.55841  0.37230]
 [-0.44746  0.26312  0.11775]]

In this scenario we represent 4 asymmetric 3x3 matrices representing the SO(3) rotations on a 3D plots.

Fig 4. Visualization of 4 random SO3 matrices on a 3D plot

References

[4Vector and Tensor Analysis with Applications -  A. I. Borisenko, I. E. Tarapov - Dover Books on MathenaticsPublications 1979 
[5A Student's Guide to Vectors and Tensors - D. Fleisch - Cambridge University Press - 2008





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



Thursday, September 26, 2024

Introduction to SE3 Lie Groups in Python

Target audience: Advanced
Estimated reading time: 5'

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
       Lie manifolds
       Geomstats
       Components
       Inversion
       Composition
References
Appendix
Follow me on LinkedIn

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, 23 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

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,
The 3-dimension Special Euclidean group (SE3) is described as a 4x4 matrix as 
equation

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 NumPyPyTorch, 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_numpyAlternative 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




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