PaperlayerView exact revision on arXiv

Source-linked AI summary

A micro Lie theory for state estimation in robotics

Joan Solà, Jeremie Deray, Dinesh Atchuthan

arXiv:1812.01537v9cs.RO

TL;DR

Robotics state estimation needs precise handling of states, measurements, derivatives, and uncertainties on Lie-group manifolds, but Lie theory is difficult and broader than many applications require. The paper develops a deliberately simplified, didactic micro Lie theory, adds robotics formulas, application examples, and a C++ library, and concludes that this presentation can help researchers enter the field without introducing new theoretical material.

  • Problem

    Lie groups are increasingly used in robotics estimation, but their abstract literature and full theoretical scope make them difficult for many roboticians to understand and apply.

  • Method

    The paper selects and explains a small subset of Lie theory for state-estimation practitioners, emphasizing Jacobians, robotics examples, formulas, and an accompanying header-only C++ library.

  • Results

    The paper presents a useful robotics-oriented form of Lie theory, with application examples, extensive formulas for common groups, and implementations supporting analytic Jacobians.

  • Takeaways & Limitations

    The authors conclude that this simplified presentation can help researchers enter Lie theory for future robotics developments, despite adding no new theoretical material.

Abstract

from arXiv · show

A Lie group is an old mathematical abstract object dating back to the XIX century, when mathematician Sophus Lie laid the foundations of the theory of continuous transformation groups. As it often happens, its usage has spread over diverse areas of science and technology many years later. In robotics, we are recently experiencing an important trend in its usage, at least in the fields of estimation, and particularly in motion estimation for navigation. Yet for a vast majority of roboticians, Lie groups are highly abstract constructions and therefore difficult to understand and to use. This may be due to the fact that most of the literature on Lie theory is written by and for mathematicians and physicists, who might be more used than us to the deep abstractions this theory deals with. In estimation for robotics it is often not necessary to exploit the full capacity of the theory, and therefore an effort of selection of materials is required. In this paper, we will walk through the most basic principles of the Lie theory, with the aim of conveying clear and useful ideas, and leave a significant corpus of the Lie theory behind. Even with this mutilation, the material included here has proven to be extremely useful in modern estimation algorithms for robotics, especially in the fields of SLAM, visual odometry, and the like. Alongside this micro Lie theory, we provide a chapter with a few application examples, and a vast reference of formulas for the major Lie groups used in robotics, including most jacobian matrices and the way to easily manipulate them. We also present a new C++ template-only library implementing all the functionality described here.

I. INTRODUCTION

The paper presents a deliberately simplified, robotics-oriented introduction to Lie theory for state estimation, emphasizing intuitive connections among manifolds, tangent spaces, group operations, and vector-space representations. It supplements this micro theory with application examples, Jacobian formulas, references for common motion groups, and a header-only C++ implementation.

  • Core concepts: The exponential and logarithm maps connect tangent-space or Lie-algebra elements with manifold elements, translating between linear representations and curved group structure.Figure 1 illustrates exponential-map paths following geodesics, while the paper describes log as the inverse unwrapping operation.
  • Scope and audience: The paper selects a small subset of Lie theory and explains it didactically to reduce the entry gap for roboticists familiar with state estimation.Examples, verbose figure captions, and familiar notation are used to connect abstract concepts with rotations, motions, quaternions, and estimation practice.
  • Applications and tools: The paper focuses particular attention on Jacobians and provides localization and mapping examples using EKF and graph-based nonlinear optimization.Its accompanying manif library implements SO(2), SO(3), SE(2), and SE(3), including analytic Jacobians.
  • Scope and audience: The Lie algebra is treated primarily through its equivalent Cartesian vector space R^n, while the Lie bracket and deeper algebraic connection are omitted as unnecessary for the target applications.This is the paper’s principal scope simplification rather than a claim to present the full power of Lie theory.
  • Motivation: Lie groups combine smooth manifolds with group operations, allowing constrained robotics states and nonlinear composition to be handled through calculus.The paper frames manifolds as smooth surfaces on which state vectors evolve and Lie groups as manifolds whose elements satisfy group axioms.
  • Robotics examples: Examples include unit complex numbers, unit quaternions, rotation and rigid-motion groups, and translation groups used to represent 2D and 3D robotic motion.The paper identifies these groups as common robotics examples and provides an extensive formula compendium for them.

D. The exponential map

The exponential map transfers tangent or Lie-algebra elements onto a Lie group, following manifold geodesics. For SO(3) and unit quaternions, algebraic power-series structure yields closed forms.

  • D. The exponential map: The exponential map retracts Lie-algebra elements onto the group manifold, while the logarithmic map performs the inverse unwrapping.The exponential follows the great arc or geodesic on the manifold.
  • D. The exponential map: For constant velocity, the manifold trajectory is obtained by integrating the velocity through X(t) = X0 ◦ Exp(vt).Non-constant velocities are handled by piecewise-constant segments, as developed later in the paper.
  • D. The exponential map: SO(3) exponentiation expands the matrix exponential into the Rodrigues rotation formula, R = Exp(uθ) = exp([uθ]×).The derivation uses powers of [u]× and identifies the resulting sine and cosine series.
  • D. The exponential map: Unit-quaternion exponentiation similarly reduces its power series to cosine and sine terms, with the Lie-algebra rotation vector mapped through hat and vee.The quaternion exponential is equivalent to the corresponding rotation-matrix exponential.
  • D. The exponential map: The capitalized Exp and Log provide direct shortcuts between vector elements in R^m and group elements on the manifold.These shortcuts are used for generic Lie-group operations and are implemented for different manifolds in the appendices.

1) The capitalized exponential map:

Capitalized Exp and Log maps connect vector-valued tangent increments with manifold elements, while plus and minus operators express group increments in local or global frames.

  • 1) The capitalized exponential map:: Capitalized Exp and Log map vector elements τ ∈ R^m directly to manifold elements X ∈ M and back.They provide convenient shortcuts for the corresponding exponential and logarithmic maps.
  • 1) The capitalized exponential map:: Non-commutativity makes local and global increments unequal, relating their tangent vectors through Eτ = AdX Xτ.The adjoint action provides the linear transformation between the two frame representations.
  • 1) The capitalized exponential map:: Right and left plus/minus operators combine composition with Exp/Log to express increments between manifold elements in tangent vector spaces.Their distinction follows the order of composition, which is non-commutative.
  • 1) The capitalized exponential map:: Local perturbations use right operators by default, whereas global perturbations use left operators and the tangent space at the origin.The paper explicitly adopts local perturbations and right-form operators as its default convention.

2) The adjoint matrix:

The adjoint matrix is the linear representation of the adjoint action, transforming tangent vectors between local and origin tangent spaces. The section then develops Jacobians that map tangent perturbations through functions.

  • 2) The adjoint matrix:: The adjoint matrix is obtained by representing the linear adjoint action on Cartesian tangent vectors in R^m.It maps tangent vectors expressed at a group element to vectors expressed at the origin.
  • 2) Right Jacobians on Lie groups:: These Jacobians satisfy the chain rule and can be constructed from partial Jacobians of inversion, composition, exponentiation, and action.This makes composite-function derivatives accessible through reusable Jacobian blocks.
  • 2) The adjoint matrix:: For SE(3), the adjoint matrix is derived by expanding the group-level adjoint relation and applying rotation and cross-product identities.The derivation uses [Rθ]× = R[θ]×R⊤ and [a]×b = −[b]×a.
  • 1) Reminder: Jacobians on vector spaces:: Vector-space Jacobians stack partial derivatives into an n × m matrix whose columns describe responses to canonical basis directions.Each column is obtained from the variation caused by perturbing one input coordinate.
  • 2) Right Jacobians on Lie groups:: Manifold right Jacobians replace vector addition and subtraction with plus and minus operators, linearly mapping local tangent spaces TXM to Tf(X)N.The construction expresses infinitesimal variations in the local tangent spaces of the input and output.

3) Left Jacobians on Lie groups:

Left Jacobians map perturbations in global tangent spaces, while covariance and motion integration use tangent-space representations to manage uncertainty and dynamics on manifolds.

  • 3) Left Jacobians on Lie groups:: Left Jacobians map global tangent spaces, or Lie algebras, from the domain to the codomain.They use left-plus and left-minus operators, in contrast to right Jacobians’ local tangent spaces.
  • 3) Left Jacobians on Lie groups:: Left and right Jacobians are related through the adjoint matrices of the input and output manifolds.Mixed plus/minus conventions can also map local tangents to global tangents through adjoint transformations.
  • H. Uncertainty in manifolds, covariance propagation: Manifold uncertainty is represented by covariance of a tangent perturbation, ΣX = E[(X ⊖ X̄)(X ⊖ X̄)⊤], rather than by subtracting group elements directly.This yields well-defined m × m covariances because tangent-space dimension matches the manifold’s degrees of freedom.
  • H. Uncertainty in manifolds, covariance propagation: Global and local perturbation covariances are transformed using the adjoint, with reference-frame choice determining how uncertainty is specified.A globally defined covariance must be expressed in the global reference frame.
  • I. Motion integration on manifolds: Motion integration discretizes velocity into tangent steps τk and recursively updates the state as Xk = Xk−1 ⊕ τk = Xk−1 ◦ Exp(vkδtk).The formulation applies to piecewise-constant velocity segments, including angular-rate integration into rotations or quaternions.

III. DIFFERENTIATION RULES ON MANIFOLDS

The paper builds manifold differentiation around elementary Jacobian blocks and the chain rule, primarily using right-Jacobians that map tangent-space variations.

  • Differentiation rules on manifolds: Elementary Jacobians for inversion, composition, exponentiation, and action provide reusable blocks for differentiating typical manifolds.Other Jacobians, including those for Log, ⊕, and ⊖, can be derived from these blocks.
  • Differentiation rules on manifolds: Right-Jacobians are the paper’s default convention, while left-Jacobians are presented separately and can be related through the adjoint.Equation (46) provides a way to obtain left-Jacobians without deriving them independently.
  • Differentiation rules on manifolds: The chain rule composes Jacobians for nested functions, while mixed derivative conventions require chaining the associated reference frames.The paper proves the right-Jacobian version and notes analogous left and crossed forms.
  • Differentiation rules on manifolds: The Jacobian of Exp maps variations in its vector argument into local tangent-space variations at Exp(τ).For small δτ, the resulting expressions yield the paper’s local approximations.

4) Group action:

Composite manifolds represent heterogeneous states as blockwise combinations of manifolds, enabling per-block derivatives and uncertainty propagation with a vector-like structure.

  • Group action: Closed-form group-action expressions depend on the acted-on set V and therefore cannot be generalized across all actions.The paper directs readers to the appendices for reference formulas.
  • Composite manifolds: Composite manifolds concatenate non-interacting manifold blocks, with identity, inverse, composition, and retraction defined separately for each block.This construction preserves the group axioms and provides unified notation for the composite state.
  • Composite manifolds: The composite notation supports new derivatives that can be used to evaluate errors, uncertainty, derivatives, and covariances.The paper assumes right derivatives, while stating that the same construction applies to left derivatives.
  • Group action: SE(n) and T(n)×SO(n) encode rigid motions with different tangent parametrizations: simultaneous translation-rotation versus chained translation plus rotation.Both share the rotational component θ, but their translational parameters ρ and p differ.
  • Composite manifolds: Composite-state Jacobians can be computed in a per-block basis, requiring only the Jacobians of the individual manifold blocks.The resulting Jacobian has a block structure whose entries are computed using the manifold derivative definition.

V. LANDMARK-BASED LOCALIZATION AND MAPPING

The paper applies Lie-theoretic state representations and Jacobians to landmark localization, error-state filtering, and graph-based smoothing and mapping.

  • Applications: Three application examples cover landmark-based Kalman filtering, graph-based smoothing for simultaneous localization and mapping, and sensor self-calibration.All examples use a common robot-and-beacon setup.
  • Setup: The planar setup models the robot pose in SE(2), beacon positions in R2, noisy controls, and Cartesian beacon observations.Control noise includes possible lateral wheel slippage, while measurements are zero-mean Gaussian.
  • Setup: Beacon observations are modeled by y_k = R^T(b_k − t) + n, expressing a beacon in the robot’s reference frame with measurement noise.This is the rigid-motion action X^-1·b_k written in Cartesian form.
  • Localization with an error-state Kalman filter: In the manifold EKF, only the state-update operators differ from a regular EKF; the Kalman gain retains its standard form.The Jacobians are computed using Lie theory but used as in a standard EKF.
  • Smoothing and mapping: The SAM problem estimates three robot poses and three beacon locations with a graph-based iterative least-squares optimizer.Each prior or measurement contributes a factor, and the procedure linearizes residuals, solves by least squares, and iterates to convergence.

C. Smoothing and mapping with self-calibration

The paper extends the smoothing-and-mapping example to jointly estimate motion-sensor bias and illustrates a broader micro Lie theory through applications, formulas, notation, and implementation resources.

  • Smoothing and mapping with self-calibration: The self-calibration example augments the state with an unknown motion-sensor bias and modifies the total Jacobian with an additional bias block.The control incorporates velocity and angular-velocity bias corrections.
  • Smoothing and mapping with self-calibration: The optimized state includes an estimate of the calibration bias, providing sensor self-calibration within the iterative smoothing procedure.The bias Jacobian is a 3×2 block associated with the augmented state.
  • Three-dimensional extension: The 2D algorithms transfer to 3D by replacing the state and measurement spaces with SE(3), R6 ≅ se(3), and R3 while preserving the algorithmic mathematics.Only the variable spaces and corresponding matrix sizes change.
  • Conclusion: The paper simplifies Lie theory through selective coverage, grounded examples, verbose figures, practical operators, and emphasis on Jacobian computation.These initiatives target readers skilled in state estimation but unfamiliar with Lie-theory abstractions.
  • Conclusion: The authors state that the paper introduces no new theoretical material but may help researchers enter Lie theory for future developments.Its claimed contribution is the accessible form of exposition rather than new theory.

APPENDIX A THE 2D ROTATION GROUPS S1 AND SO(2)

S1 and SO(2) provide equivalent representations of planar rotations, with matching tangent spaces and representation-independent Jacobians. Their exponential and logarithm maps support rotation composition, inversion, and action on 2-vectors.

  • Group representations: S1 consists of unit complex numbers, while SO(2) consists of planar rotation matrices under multiplication.Both groups use conjugation or transposition for inversion and multiplication for composition.
  • Shared structure: Both groups rotate 2-vectors and have isomorphic tangent spaces, so the paper studies them together.
  • Exponential and logarithm maps: The exponential map represents an S1 element as z = Exp(θ) = cos θ + i sin θ.The paper identifies this expression with Euler's formula.
  • Jacobians: Planar rotations are commutative, and θ = Log(z) = Log(R) makes their Jacobians independent of the chosen representation.
  • 3D rotation analogy: S3 and SO(3) similarly represent 3D rotations, but quaternions form a double cover in which q and −q denote the same rotation.The positive-real-part cover uses w > 0; replacing q by −q before Log avoids double-cover problems.

C. Elementary Jacobian blocks

The paper develops elementary Jacobian blocks by exploiting shared tangent-space structure and simple group properties. It also introduces SE(2) as planar rigid motions combining rotation and translation.

  • 3D rotation Jacobians: For S3 and SO(3), θ = Log(q) = Log(R), so Jacobians are independent of whether rotations use quaternions or matrices.Generic 3D rotations are subsequently denoted by R.
  • Adjoint: The adjoint representation acts on a rotation vector as AdRθ = Rθ, with quaternion adjoints corresponding to their rotation matrices.
  • Jacobian derivations: The appendix derives closed-form Jacobian expressions and uses Exp(θ) ≈ I + [θ]× together with [a]×b = −[b]×a.
  • SE(2): SE(2) represents planar rigid motions with R ∈ SO(2) and t ∈ R2, using matrix inversion and multiplication for group operations.Its tangent space is identified with R3 ∼= se(2) = TSE(2).
  • SE(2) Jacobian blocks: The SE(2) section derives the adjoint, Jacobians, and point action, using commutativity of planar rotations and the limit Exp(τ) → I + τ∧ as τ → 0.

APPENDIX D THE 3D RIGID MOTION GROUP SE(3)

SE(3) extends rigid-motion representations to 3D by combining rotations and translations, while translation groups provide a commutative and trivial special case. The appendix gives exponential, logarithmic, adjoint, Jacobian, and point-action constructions.

  • SE(3) representation: SE(3) combines R ∈ SO(3) with t ∈ R3, and its tangent space is R6 ∼= se(3) = TSE(3).
  • Group operations: SE(3) composition and inversion use matrix multiplication and inversion, with the translation composition term t_a + R_a t_b.
  • Maps and Jacobians: The SE(3) appendix provides exponential and logarithmic maps, closed-form left Jacobians and inverses, and right Jacobians obtained from J_r(ρ, θ) = J_l(−ρ, −θ).The cited closed forms include results attributed to Barfoot.
  • Translation groups: The group (R^n, +) is a translation group whose group elements, Lie algebra, and tangent spaces coincide, making t = t∧ = Exp(t).
  • Translation groups: T(n) is a commutative subgroup of SE(n) with R = I, and its exponential follows by specializing the SE(n) expression to identity rotation.Its exponential also follows from (t∧)^2 = 0, while its Jacobians are trivial.
Loading 1812.01537v9…