Tutorial: Understanding SE(3) Manifold Geometry

Category: Manifolds & Geometry

---

Overview

This tutorial introduces the SE(3) manifold, the mathematical space underlying robotics, 3D mapping, SLAM, and all pose‑graph optimization in DSG‑JIT.

The goal is to deeply understand: - What SE(3) is
- How rotations + translations combine
- Why SE(3) is not a vector space - How tangent vectors, exponential maps, and geodesics work
- How SE(3) is implemented inside DSG‑JIT

Under the hood, SE(3) operations in DSG‑JIT are used inside a WorldModel‑backed factor graph, where pose variables live in the WorldModel and residuals are built on top of SE(3) manifold geometry.

---

1. What is SE(3)?

The Special Euclidean group in 3D, SE(3), represents 3D poses consisting of: - A rotation in SO(3) - A translation in ℝ³

Mathematically:

[ T = \begin{bmatrix} R & t \ 0 & 1 \end{bmatrix}, \quad R \in SO(3),\ t \in \mathbb{R}^3 ]

SE(3) is a Lie Group, meaning: - It is smooth and differentiable - It has an associated Lie algebra, se(3) - The exponential and logarithm maps connect the two

This structure enables gradient‑based optimization on poses.

---

2. Why SE(3) Is Not a Vector Space

Unlike ℝ⁶: - You cannot add two poses. - A geodesic (shortest path) between two poses is not a straight line. - Interpolating rotations requires spherical geometry (SO(3)).

Therefore, DSG‑JIT (and modern robotics libraries such as GTSAM, Kimera, Sophus, etc.) treat SE(3) as a manifold.

---

3. The Lie Algebra se(3)

A tangent vector in se(3) is a 6D twist:

[ \xi = (\omega_x, \omega_y, \omega_z, v_x, v_y, v_z) ]

• First 3 = angular velocity (axis‑angle)
• Last 3 = linear velocity

We map twists → poses using the exponential map:

T = exp_se3(xi)

And poses → twists with the logarithm map:

xi = log_se3(T)

DSG‑JIT’s SE(3) class implements both.

---

4. Exponential and Logarithm Maps

These are essential for: - Pose integration - Backpropagation through motion models - Gauss‑Newton / Levenberg‑Marquardt - Creating geodesics (used heavily in SLAM)

The exponential map integrates a twist:

T = exp_se3([wx, wy, wz, vx, vy, vz])

The log map extracts the tangent displacement from one pose to another:

xi = log_se3(inv(T1) * T2)

---

5. SE(3) Geodesics

A geodesic between two poses is a smooth curve obtained by:

T(s) = T0 * exp_se3(s * log_se3(inv(T0) * T1))

Where: - s ∈ [0, 1] - T(0) = T0
- T(1) = T1

This is implemented in exp02_mini_world_se3_geodesic.py.

---

6. How DSG‑JIT Implements SE(3)

Key functions:

Operation	Location	Description
exp_se3	slam/se3_ops.py	Twist → Pose
log_se3	slam/se3_ops.py	Pose → Twist
se3_mul	slam/se3_ops.py	Pose composition
se3_inv	slam/se3_ops.py	Pose inverse
se3_geodesic	experiments/exp02_mini_world_se3_geodesic.py	Generates geodesic path

Internally, JAX is used for differentiability and vectorization. These SE(3) primitives are then consumed by the WorldModel and its residual functions to perform manifold‑aware SLAM and scene‑graph optimization.

---

7. Example – Computing a Geodesic

from dsg_jit.slam.se3_ops import SE3, se3_geodesic
import jax.numpy as jnp

T0 = SE3.from_xyz_rpy(0,0,0, 0,0,0)
T1 = SE3.from_xyz_rpy(1,0,0, 0,0,jnp.pi/2)

path = se3_geodesic(T0, T1, steps=20)

This yields 20 poses transitioning smoothly from T0 → T1.

---

8. Visualizing the Result

Experiment exp02_mini_world_se3_geodesic shows: - Trajectories of SE(3) poses - Smooth rotational interpolation - How DSG‑JIT handles manifold navigation

---

9. Summary

SE(3) is the mathematical backbone of: - SLAM - Motion planning - Robot localization - Scene‑graph optimization in a WorldModel‑backed factor graph - Multisensor fusion

Understanding manifold geometry ensures: - Stable solvers
- Correct interpolation
- Proper WorldModel‑backed factor graph construction

This tutorial prepares you for upcoming lessons involving: - Odometry factors
- Range & bearing factors
- Full SLAM back‑ends
- Dynamic scene graphs

---