Lie Groups And On-Manifold Optimization

The Special Orthogonal group SO(2). Angles live on the manifold represented by a circle. Exponent maps from angles (flat world) into the manifold.

If you are a roboticist, you have certainly encountered issues when working with plain angles (e.g., θ in a 2D pose) and related rotations, such as:

The difference between an angle θ_1 = 179° and θ_2 = -179° should be 2°, but a plain subtraction yields 358°.
The average between θ_1 = 350° and θ_2 = 10° should be 0°, but regular averaging gives 180°.

This happens because standard calculations assume a flat vector space, but rotations actually live on a manifold (a smooth, curved space). Many aspiring and beginner roboticists do not realize this. When doing state estimation, SLAM, or Visual-Inertial Odometry, they often get puzzled by weird bugs resulting from a failure to handle the wrap-around boundary correctly.

In reality, the poses (position and orientation) of a robot moving on a plane or in 3D space exist natively on these manifolds. Implementations of state estimation, SLAM, and related algorithms should ideally be executed directly on the manifold rather than in a standard flat plane. Lie theory — specifically the Special Orthogonal groups (SO(2), SO(3)) and Special Euclidean groups (SE(2), SE(3)) — provides the correct mathematical representation for rigid-body motion (rotation and translation). These groups preserve the geometry of the space, unlike flat vectors (such as a naive [x, y, θ] vector) which suffer from coordinate singularities and discontinuities.

Under the hood, these Lie groups are represented using homogeneous transformation matrices. Operators like retract ([+] symbol) and local ([-] symbol) replace the standard addition and subtraction of the flat world, bringing the robust, error-free mathematics of Lie theory to the core of modern robotics.

As I continue my journey of creating the state estimation engine for the System 1 / System 2 robotics autonomy stack, let’s explore Lie theory and on-manifold optimization together through this notebook:

factor-graphs-from-scratch/modules/04-lie-groups/lesson.ipynb at main ·…