1

A micro Lie theory for state estimation in robotics Joan Sola,` Jeremie Deray, Dinesh Atchuthan

Abstract—A Lie group is an old mathematical abstract object ⌧1 3 X log( ) dating back to the XIX century, when mathematician Sophus Lie X3 laid the foundations of the theory of continuous transformation groups. Its influence has spread over diverse areas of science and vt technology many years later. In robotics, we are recently expe- T exp(⌧1) E M riencing an important trend in its usage, at least in the fields of ⌧2 estimation, and particularly in motion estimation for navigation. E Yet for a vast majority of roboticians, Lie groups are highly exp(vt) abstract constructions and therefore difficult to understand and

to use. exp(⌧2) In estimation for robotics it is often not necessary to exploit M 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 Figure 1. Representation of the relation between the Lie group and the Lie clear and useful ideas, and leave a significant corpus of the Lie algebra. The Lie algebra TE M (red plane) is the tangent space to the Lie group’s manifold M (here represented as a blue sphere) at the identity E. theory behind. Even with this mutilation, the material included Through the exponential map, each straight path vt through the origin on here has proven to be extremely useful in modern estimation the Lie algebra produces a path exp(vt) around the manifold which runs algorithms for robotics, especially in the fields of SLAM, visual along the respective geodesic. Conversely, each element of the group has odometry, and the like. an equivalent in the Lie algebra. This relation is so profound that (nearly) Alongside this micro Lie theory, we provide a chapter with all operations in the group, which is curved and nonlinear, have an exact a few application examples, and a vast reference of formulas equivalent in the Lie algebra, which is a linear vector space. Though the 3 for the major Lie groups used in robotics, including most sphere in R is not a Lie group (we just use it as a representation that can be 4 Jacobian matrices and the way to easily manipulate them. We drawn on paper), that in R is, and describes the group of unit quaternions also present a new C++ template-only library implementing all —see Fig.4 and Ex.5. the functionality described here.

and/or other modifications of bodies or reference frames. The I.INTRODUCTION origin frame may be identified with the group’s identity, and There has been a remarkable effort in the last years in any other point on the manifold represents a certain ‘local’ the robotics community to formulate estimation problems frame. By resorting to these analogies, many mathematical properly. This is motivated by an increasing demand for abstractions of the LT can be brought closer to intuitive notions precision, consistency and stability of the solutions. Indeed, a in vector spaces, geometry, kinematics and other more classical proper modeling of the states and measurements, the functions fields. relating them, and their uncertainties, is crucial to achieve Lie theory is by no means simple. To grasp a minimum these goals. This has led to designs involving what has been idea of what LT can be, we may consider the following known as ‘manifolds’, which in this context are no less three references. First, Abbaspour’s “Basic Lie theory” [1] arXiv:1812.01537v8 [cs.RO] 12 Nov 2020 than the smooth topologic surfaces of the Lie groups where comprises more than 400 pages. With a similar title, Howe’s the state representations evolve. Relying on the Lie theory “Very basic Lie theory” [2] comprises 24 (dense) pages, and (LT) we are able to construct a rigorous calculus corpus to is sometimes considered a must-read introduction. Finally, handle uncertainties, derivatives and integrals with precision the more modern and often celebrated Stillwell’s “Naive and ease. Typically, these works have focused on the well Lie theory” [3] comprises more than 200 pages. With such known manifolds of rotation SO(3) and rigid motion SE(3). precedents labeled as ‘basic’, ‘very basic’ and ‘naive’, the aim When being introduced to Lie groups for the first time, it is of this paper at merely 17 pages is to simplify Lie theory even important to try to regard them from different points of view. more (thus our adjective ‘micro’ in the title). This we do in The topological viewpoint, see Fig.1, involves the shape of two ways. First, we select a small subset of material from the manifold and conveys powerful intuitions of its relation the LT. This subset is so small that it merely explores the to the tangent space and the exponential map. The algebraic potential of LT. However, it appears very useful for uncertainty viewpoint involves the group operations and their concrete management in the kind of estimation problems we deal with realization, allowing the exploitation of algebraic properties in robotics (e.g. inertial pre-integration, odometry and SLAM, to develop closed-form formulas or to simplify them. The visual servoing, and the like), thus enabling elegant and geometrical viewpoint, particularly useful in robotics, asso- rigorous designs of optimal optimizers. Second, we explain ciates group elements to the position, velocity, orientation, it in a didactical way, with plenty of redundancy so as to 2 reduce the entry gap to LT even more, which we believe is still needed. That is, we insist on the efforts in this direction TX M ˙ X of, to name a paradigmatic title, Stillwell’s [3], and provide TX M yet a more simplified version. The main text body is generic, X X though we try to keep the abstraction level to a minimum. M M Inserted examples serve as grounding base for the general concepts when applied to known groups (rotation and motion ∼ 2 matrices, quaternions, etc.). Also, plenty of figures with very Figure 2. A manifold M and the vector space TX M (in this case = R ) ˙ verbose captions re-explain the same concepts once again. We tangent at the point X , and a convenient side-cut. The velocity element, X = ∂X /∂t, does not belong to the manifold M but to the tangent space TX M. put special attention to the computation of Jacobians (a topic that is not treated in [3]), which are essential for most optimal optimizers and the source of much trouble when designing and we believe they make the understanding of the LT and the new algorithms. We provide a chapter with some applicative manipulation of its tools easier. examples for robot localization and mapping, implementing This paper is accompanied by a new open-source C++ EKF and nonlinear optimization algorithms based on LT. And header-only library, called manif [7], which can be found finally, several appendices contain ample reference for the at https://github.com/artivis/manif. manif implements the most relevant details of the most commonly used groups widely used groups SO(2), SO(3), SE(2) and SE(3), with in robotics: unit complex numbers, quaternions, 2D and 3D support for the creation of analytic Jacobians. The library is rotation matrices, 2D and 3D rigid motion matrices, and the designed for ease of use, flexibility and performance. trivial translation groups. Yet our most important simplification to Lie theory is in II.A MICRO LIE THEORY terms of scope. The following passage from Howe [2] may serve us to illustrate what we leave behind: “The essential A. The Lie group phenomenon of Lie theory is that one may associate in a The Lie group encompasses the concepts of group and natural way to a Lie group its Lie algebra g. The Lie algebra smooth manifold in a unique body: a Lie group is a smooth G g is first of all a vector spaceG and secondly is endowed with manifold whose elements satisfy the group axioms. We briefly a bilinear nonassociative product called the Lie bracket [...]. present these two concepts before joining them together. Amazingly, the group is almost completely determined by g On one hand, a differentiable or smooth manifold is a and its Lie bracket. ThusG for many purposes one can replace topological space that locally resembles linear space. The with g. Since is a complicated nonlinear object and g reader should be able to visualize the idea of manifold (Fig.2): isG just a vector space,G it is usually vastly simpler to work it is like a curved, smooth (hyper)-surface, with no edges or with g. [...] This is one source of the power of Lie theory.” spikes, embedded in a space of higher dimension. In robotics, In [3], Stillwell even speaks of “the miracle of Lie theory”. we say that our state vector evolves on this surface, that is, the In this work we will effectively relegate the Lie algebra to manifold describes or is defined by the constraints imposed on a second plane in favor of its equivalent vector space Rn, the state. For example, vectors with the unit norm constraint and will not introduce the Lie bracket at all. Therefore, the define a spherical manifold of radius one. The smoothness of connection between the Lie group and its Lie algebra will not the manifold implies the existence of a unique tangent space be made here as profound as it should. Our position is that, at each point. This space is a linear or vector space on which given the target application areas that we foresee, this material we are allowed to do calculus. is often not necessary. Moreover, if included, then we would On the other hand, a group ( , ) is a set, , with a fail in the objective of being clear and useful, because the composition operation, , that, forG elements◦ , G, , reader would have to go into mathematical concepts that, by satisfies the following axioms,◦ X Y Z ∈ G their abstraction or subtleness, are unnecessarily complicated. Closure under ‘ ’ : (1) Our effort is in line with other recent works on the sub- ◦ X ◦ Y ∈ G ject [4], [5], [6], which have also identified this need of Identity : = = (2) E1 E ◦1 X X ◦ E X 1 bringing the LT closer to the robotician. Our approach aims Inverse − : − = − = (3) X X ◦ X X ◦ X E at appearing familiar to the target audience of this paper: an Associativity :( ) = ( ) . (4) audience that is skilled in state estimation (Kalman filtering, X ◦ Y ◦ Z X ◦ Y ◦ Z graph-based optimization, and the like), but not yet familiar In a Lie group, the manifold looks the same at every point with the theoretical corpus of the Lie theory. We have for this (like e.g. in the surface of a sphere, see Exs.1 and2), and taken some initiatives with regard to notation, especially in the therefore all tangent spaces at any point are alike. The group definition of the derivative, bringing it close to the vectorial structure imposes that the composition of elements of the counterparts, thus making the chain rule clearly visible. As manifold remains on the manifold, (1), and that each element said, we opted to practically avoid the material proper to the has an inverse also in the manifold, (3). A special one of Lie algebra, and prefer instead to work on its isomorphic tan- these elements is the identity, (2), and thus a special one gent vector space Rn, which is where we ultimately represent of the tangent spaces is the tangent at the identity, which uncertainty or (small) state increments. All these steps are we call the Lie algebra of the Lie group. Lie groups join undertaken with absolutely no loss in precision or exactness, the local properties of smooth manifolds, allowing us to 3

exp 3 3 3 1 1 z S q S q S q ✓ S i z log S ✓ z = x exp(i✓) ✓ ✓ ✓ x i✓ = log(x⇤z) p 1 1 q =exp(u✓) q = p ✓ z =exp(i✓) 1 u✓ = log(q) ✓ = q p i✓ = log(z) TxS ⇠= R 3 1 ⇠= R T1S = iR ⇠= R 3 3 Hp ⇠= R Hp ⇠= R 1 Figure 3. The S manifold is a unit circle (blue) in the plane C, where the unit ∗ 1 1 Figure 4. The S3 manifold is a unit 3-sphere (blue) in the 4-space of complex numbers z z = 1 live. The Lie algebra s = TE S is the line of 1 quaternions , where the unit quaternions q∗ q = 1 live. The Lie algebra imaginary numbers iR (red), and any tangent space TS is isomorphic to the H is the space of pure imaginary quaternions ix + jy + kz ∈ Hp, isomorphic line R (red). Tangent vectors (red segment) wrap the manifold creating the arc 3 3 of circle (blue arc). Mappings exp and log (arrows) map (wrap and unwrap) to the hyperplane R (red grid), and any other tangent space TS is also 1 isomorphic to 3. Tangent vectors (red segment) wrap the manifold over the elements of iR to/from elements of S (blue arc). Increments between unit R great arc or geodesic (dashed). The centre and right figures show a side-cut complex numbers are expressed in the tangent space via composition and the 1 exponential map (and we will define special operators ⊕, for this). See the through this geodesic (notice how it resembles S in Fig.3). Mappings exp text for explanations, and Fig.4 for a similar group. and log (arrows) map (wrap and unwrap) elements of Hp to/from elements of S3 (blue arc). Increments between quaternions are expressed in the tangent space via the operators ⊕, (see text). Example 1: The unit complex numbers group S1 Example 2: The unit quaternions group S3 Our first example of Lie group, which is the easiest to visualize, is the group of unit complex numbers under A second example of Lie group, which is also relatively complex multiplication (Fig.3). Unit complex numbers easy to visualize, is the group of unit quaternions under take the form z = cos θ + i sin θ. quaternion multiplication (Fig.4). Unit quaternions take – Action: Vectors x = x + iy rotate in the plane by an the form q = cos(θ/2) + u sin(θ/2), with u = iux + angle θ, through complex multiplication, x0 = z x. juy + kuz a unitary axis and θ a rotation angle. – Group facts: The product of unit complex numbers is – Action: Vectors x = ix + jy + kz rotate in 3D space a unit complex number, the identity is 1, and the inverse by an angle θ around the unit axis u through the double is the conjugate z∗. quaternion product x0 = q x q∗. – Manifold facts: The unit norm constraint defines the – Group facts: The product of unit quaternions is a unit circle in the complex plane (which can be viewed as unit quaternion, the identity is 1, and the inverse is the 1 the 1-sphere, and hence the name S ). This is a 1-DoF conjugate q∗. curve in 2-dimensional space. Unit complex numbers – Manifold facts: The unit norm constraint defines the 3- evolve with time on this circle. The group (the circle) sphere S3, a spherical 3-dimensional surface or manifold ressembles the linear space (the tangent line) locally, but in 4-dimensional space. Unit quaternions evolve with not globally. time on this surface. The group (the sphere) ressembles the linear space (the tangent hyperplane R3 R4) locally, but not globally. ⊂ do calculus, with the global properties of groups, enabling nonlinear composition of distant objects. Common examples are the groups of rotation matrices In this work, for simplicity and as it has been common in SO(n), the group of unit quaternions, and the groups of rigid robotics works, we will oftentimes refer to Lie groups as just motion SE(n). Their respective actions on vectors satisfy ‘manifolds’. SO(n): rotation matrix R x Rx · , SE(n): Euclidean matrix H x Rx + t · , B. The group actions S1 : unit complex z x z x · , 3 Importantly, Lie groups come with the power to transform S : unit quaternion q x q x q∗ · , elements of other sets, producing e.g. rotations, translations, See TableI for a more detailed exposition, and the appendices. scalings, and combinations of them. These are extensively used The group composition (1) may be viewed as an action of in robotics, both in 2D and 3D. the group on itself, : . Another interesting Given a Lie group and a set , we note v the action action is the adjoint action◦ M, which× M→ we M will see in Section II-F. of on v ,M V X· X ∈ M ∈ V C. The tangent spaces and the Lie algebra : ;( , v) v . (5) · M × V → V X 7→ X · Given (t) a point moving on a Lie group’s manifold , X ˙ M For to be a group action, it must satisfy the axioms, its velocity = ∂ /∂t belongs to the space tangent to · at (Fig.2),X whichX we note T . The smoothness of theM manifold,X i.e., the absence of edgesX M or spikes, implies the Identity : v = v (6) E· existence of a unique tangent space at each point. The structure Compatibility :( ) v = ( v) . (7) X ◦ Y · X· Y· of such tangent spaces is the same everywhere. 4

Table I n TYPICAL LIEGROUPSUSEDIN 2D AND 3D MOTION, INCLUDINGTHETRIVIAL R .SEETHEAPPENDICESFORFULLREFERENCE

∧ m Lie group M, ◦ size dim X ∈ M Constraint τ ∈ m τ ∈ R Exp(τ ) Comp. Action n n n n n-D vector R , + n n v ∈ R v − v = 0 v ∈ R v ∈ R v = exp(v) v1 +v2 v + x 1 ∗ circle S , · 2 1 z ∈ C z z = 1 iθ ∈ iR θ ∈ R z = exp(iθ) z1 z2 z x > Rotation SO(2), · 4 1 R R R = I [θ]× ∈ so(2) θ ∈ R R = exp([θ]×) R1 R2 R x  R t  > h [θ] ρ i  ρ  3 h [θ] ρ i Rigid motion SE(2), · 9 3 M = R R = I × ∈se(2) ∈ exp × M1 M2 R x+t 0 1 0 0 θ R 0 0 3 ∗ 3 ∗ 3-sphere S , · 4 3 q ∈ H q q = 1 θ/2 ∈ Hp θ ∈ R q = exp(uθ/2) q1 q2 q x q > 3 Rotation SO(3), · 9 3 R R R = I [θ]× ∈ so(3) θ ∈ R R = exp([θ]×) R1 R2 R x  R t  > h [θ] ρ i  ρ  6 h [θ] ρ i Rigid motion SE(3), · 16 6 M = R R = I × ∈se(3) ∈ exp × M1 M2 R x+t 0 1 0 0 θ R 0 0

z 1 v^ = i! iR v^ = i! iR Tangent T Manifold 2 2 E M exp z˙ = z i! / iR z(t) z˙ = i! iR Lie algebra · 2 2 ⌧ ^ m !t 2 log 1 ( )^ ( )_ · · Exp X 2 M m Vector ⌧ R 1 1 2 Log TzS T1S = iR S1 Figure 6. Mappings between the manifold M and the representations of its tangent space at the origin T M (Lie algebra and Cartesian m). Maps Figure 5. Let a point z ∈ S1 move at constant rotation rate ω, z(t) = E m R hat (·)∧ and vee (·)∨ are the linear invertible maps or isomorphisms (10–11), cos ωt + i sin ωt. Its velocities when passing through 1 and z are in the exp(·) and log(·) map the Lie algebra to/from the manifold, and Exp(·) and respective tangent spaces, T S1 and T S1. In the case of T S1, the velocity 1 z z Log(·) are shortcuts to map directly the vector space m to/from M. is z˙ = z iω = −ω sin ωt + iω cos ωt when expressed in the global R coordinates, and zv∧ = iω when expressed locally. Their relation is given z ∧ −1 ∗ 1 by v = z z˙ = z z˙. In the case of T1S , this relation is the identity 1 ∧ v = z˙ = iω. Clearly, the structure of all tangent spaces is iR, which is For multiplicative groups this yields the new constraint the Lie algebra. This is also the structure of z˙ at the identity, and this is why 1 ˙ ˙ 1 the Lie algebra is defined as the tangent space at the identity. − + − = 0, which applies to the elements tangent at X X X X˙ 1 (the term − is the derivative of the inverse). The elements ofX the Lie algebraX are therefore of the form,2 1) The Lie algebra m: The tangent space at the identity, 1 ˙ ˙ 1 T , is called the Lie algebra of , and noted m, v∧ = − = − . (9) E M M X X −X X 2) The Cartesian vector space m: The elements of Lie algebra : m , T . (8) R τ ∧ E M the Lie algebra have non-trivial structures (skew-symmetric Every Lie group has an associated Lie algebra. We relate the matrices, imaginary numbers, pure quaternions, see TableI) Lie group with its Lie algebra through the following facts [5] but the key aspect for us is that they can be expressed as linear (see Figs.1 and6): combinations of some base elements Ei, where Ei are called The Lie algebra m is a vector space.1 As such, its the generators of m (they are the derivatives of around the • X elements can be identified with vectors in Rm, whose origin in the i-th direction). It is then handy to manipulate dimension m is the number of degrees of freedom of just the coordinates as vectors in Rm, which we shall note . simply τ . We may pass from m to Rm and vice versa through TheM exponential map, exp : m , exactly converts two mutually inverse linear maps or isomorphisms, commonly • elements of the Lie algebra into→ elements M of the group. called hat and vee (see Fig.6), The log map is the inverse operation. m Vectors of the tangent space at can be transformed m X • Hat : R m ; τ τ ∧ = τi Ei (10) to the tangent space at the identityX through a linear → 7→ E i=1 transform. This transform is called the adjoint. m m X Vee : m R ; τ ∧ (τ ∧)∨ = τ = τi ei , (11) Lie algebras can be defined locally to a tangent point , → 7→ establishing local coordinates for T (Fig.5). We shallX i=1 X denote elements of the Lie algebras withM a ‘hat’ decorator, m with ei the vectors of the base of R (we have ei∧ = Ei). such as v∧ for velocities or τ ∧ = (vt)∧ = v∧t for general This means that m is isomorphic to the vector space Rm — m m elements. A left superscript may also be added to specify the one writes m = R , or τ ∧ = τ . Vectors τ R are handier ∼ ∼ ∈ precise tangent space, e.g., Xv∧ T and Ev∧ T . for our purposes than their isomorphics τ ∧ m, since they ∈ X M ∈ E M The structure of the Lie algebra can be found (see Exam- can be stacked in larger state vectors, and more∈ importantly, ples3 and5) by time-differentiating the group constraint (3). 2For additive Lie groups the constraint X −X = 0 differentiates to X˙ = X˙ , 1In a Lie algebra, the vector space is endowed with a non-associative that is, no constraint affects the tangent space. This means that the tangent product called the Lie bracket. In this work, we will not make use of it. space is the same as the group space. See App.E for more details. 5

Example 3: The rotation group SO(3), its Lie algebra Example 4: The exponential map of SO(3) so(3), and the vector space R3 We have seen in Ex.3 that R˙ = R [ω] TRSO(3). In the rotation group SO(3), of 3 3 rotation matrices For ω constant, this is an ordinary differential× ∈ equation R, we have the orthogonality condition× R R = I. The > (ODE), whose solution is R(t) = R0 exp([ω] t). At the tangent space may be found by taking the time derivative × origin R0 = I we have the exponential map, of this constraint, that is R>R˙ + R˙ >R = 0, which we rearrange as R(t) = exp([ω] t) SO(3) . × ∈ R>R˙ = (R>R˙ )>. − We now define the vector θ uθ ωt R3 as , , ∈ This expression reveals that R>R˙ is a skew-symmetric the integrated rotation in angle-axis form, with angle θ matrix (the negative of its transpose). Skew-symmetric and unit axis u. Thus [θ] so(3) is the total rotation ∈ matrices are often noted [ω] and have the form expressed in the Lie algebra.× We substitute it above. Then × write the exponential as a power series,  0 ωz ωy  − [ω] = ωz 0 ωx . θk ωy ωx −0 X k × − R = exp([θ] ) = ([u] ) . × k! × k This gives R>R˙ = [ω] . When R = I we have × In order to find a closed-form expression, we write down ˙ R = [ω] , a few powers of [u] , × × that is, [ω] is in the Lie algebra of SO(3), which 0 1 × [u] = I, [u] = [u] , we name so(3). Since [ω] so(3) has 3 DoF, the 2× 3× × dimension of SO(3) is m ×=∈ 3. The Lie algebra is a [u] = uu> I, [u] = [u] , × − × − × vector space whose elements can be decomposed into [u]4 = [u]2 , × − × ··· [ω] = ωxEx + ωyEy + ωzEz and realize that all can be expressed as multiples of I, × 2 h 0 0 0 i h 0 0 1 i h 0 1 0 i [u] or [u] . We thus rewrite the series as, with Ex = 0 0 1 , Ey = 0 0 0 , Ez = 1− 0 0 the × × 0 1− 0 1 0 0 0 0 0 1 3 1 5
− 3 R = I + [u] θ 3! θ + 5! θ generators of so(3), and where ω = (ωx, ωy, ωz) R × − − · · · ∈ 2 1 2 1 4 1 6
is the vector of angular velocities. The one-to-one linear + [u] 2 θ 4! θ + 6! θ , × − − · · · relation above allows us to identify so(3) with R3 — where we identify the series of sin θ and cos θ, yielding we write so(3) = 3. We pass from so(3) to 3 and ∼ R R the closed form, viceversa using the linear operators hat and vee, 2 3 R = exp([uθ] ) = I + [u] sin θ + [u] (1 cos θ) . Hat : R so(3); ω ω∧ = [ω] × × × − → 7→ × 3 Vee : so(3) R ;[ω] [ω]∨ = ω . This expression is the well known Rodrigues rotation → × 7→ × formula. It can be used as the capitalized exponential just by doing R = Exp(uθ) = exp([uθ] ). manipulated with linear algebra using matrix operators. In × this work, we enforce this preference of Rm over m, to the point that most of the operators and objects that we define Since (t) and (0) are elements of the group, then X X1 (specifically: the adjoint, the Jacobians, the perturbations and exp(v∧t) = (0)− (t) must be in the group too, and so m X X their covariances matrices, as we will see soon) are on R . exp(v∧t) maps elements v∧t of the Lie algebra to the group. This is known as the exponential map. D. The exponential map In order to provide a more generic definition of the expo- The exponential map exp() allows us to exactly transfer nential map, let us define the tangent increment τ vt Rm , ∈ elements of the Lie algebra to the group (Fig.1), an operation as velocity per time, so that we have τ ∧ = v∧t m a point generically known as retraction. Intuitively, exp() wraps the in the Lie algebra. The exponential map, and its∈ inverse the tangent element around the manifold following the great arc logarithmic map, can be now written as, or geodesic (as when wrapping a string around a ball, Figs.1, 3 and4). The inverse map is the log(), i.e., the unwrapping exp : m ; τ ∧ = exp(τ ∧) (14) → M 7→ X operation. The exp() map arises naturally by considering the log : m ; τ ∧ = log( ) . (15) time-derivatives of over the manifold, as follows. M → X 7→ X From (9) we have, X ∈ M Closed forms of the exponential in multiplicative groups are obtained by writing the absolutely convergent Taylor series, ˙ = v∧ . (12) X X 1 2 1 3 For v constant, this is an ordinary differential equation (ODE) exp(τ ∧) = + τ ∧ + τ ∧ + τ ∧ + , (16) E 2 3! ··· whose solution is and taking advantage of the algebraic properties of the powers (t) = (0) exp(v∧t) . (13) X X of τ ∧ (see Ex.4 and5 for developments of the exponential 6

Example 5: The unit quaternions group S3 (cont.) X X⌧ 3 X = E = X In the group S (recall Ex.2 and see e.g. [8]), the time X Y X X Y derivative of the unit norm condition q∗q = 1 yields E⌧ = E⌧ = X⌧ X Y X X q∗q˙ = (q∗q˙ )∗. E E E⌧ = Ad X⌧ − X This reveals that q∗q˙ is a pure quaternion (its real part M is zero). Pure quaternions uv Hp have the form X E ∈ Figure 7. Two paths, X ◦ δ and δ ◦ X , join the origin E with the point Y. They both compose the element X with increments or ‘deltas’ expressed uv = (iux + juy + kuz)v = ivx + jvy + kvz, either in the local frame, Xδ, or in the origin, Eδ. Due to non-commutativity, the elements Xδ and Eδ are not equal. Their associated tangent vectors Xτ = where u , iux + juy + kuz is pure and unitary, v is Log(Xδ) and Eτ = Log(Eδ) are therefore unequal too. They are related by E X the norm, and i, j, k are the generators of the Lie algebra the linear transform τ = AdX τ where AdX is the adjoint of M at X . 3 s = Hp. Re-writing the condition above we have, q˙ = q uv T S3, ∈ q Clearly from Fig.6, which integrates to q = q0 exp(uvt). Letting q0 = 1 = Exp(τ ) , exp(τ ∧) (23) and defining φ , uφ , uvt we get the exponential map, X τ = Log( ) , log( )∨ . (24) X φk X X q = exp(uφ) uk S3 . , k! ∈ See the Appendices for details on the implementation of these The powers of u follow the pattern 1, u, 1, u, 1, . maps for different manifolds. Thus we group the terms in 1 and u and− identify− ··· the series of cos φ and sin φ. We get the closed form, E. Plus and minus operators q = exp(uφ) = cos(φ) + u sin(φ) , Plus and minus allow us to introduce increments between elements of a (curved) manifold, and express them in its (flat) which is a beautiful extension of the Euler formula, tangent vector space. Denoted by and , they combine one . The elements of the Lie algebra exp(iφ) = cos φ+i sin φ Exp/Log operation with one composition.⊕ Because of the non- φ = uφ s3 can be identified with the rotation vector ∈ commutativity of the composition, they are defined in right- θ R3 trough the mappings hat and vee, ∈ and left- versions depending on the order of the operands. The 3 3 Hat : R s ; θ θ∧ = 2φ right operators are (see Fig.4- right), → 7→ Vee 3 3 : s R ; φ φ∨ = θ/2 , right- : = Xτ Exp(Xτ ) (25) → 7→ ⊕ Y X ⊕ , X ◦ ∈ M 1 where the factor 2 accounts for the double effect of the right- : Xτ = , Log( − ) T . (26) Y X X ◦Y ∈ X M quaternion in the rotation action, x0 = q x q∗. With this choice of Hat and Vee, the quaternion exponential Because in (25) Exp(Xτ ) appears at the right hand side of the composition, Xτ belongs to the tangent space at (see (26)): 3 X q = Exp(uθ) = cos(θ/2) + u sin(θ/2) we say by convention that Xτ is expressed in the local frame at — we note reference frames with a left superscript. is equivalent to the rotation matrix R = Exp(uθ). TheX left operators are,

left- : = Eτ , Exp(Eτ ) (27) 3 ⊕ Y ⊕ X ◦ X ∈ M map in SO(3) and S ). These are then inverted to find the 1 left- : Eτ = , Log( − ) T . (28) logarithmic map. Key properties of the exponential map are Y X Y ◦X ∈ E M Now, in (27) Exp(Eτ ) is on the left and we have Eτ T : exp((t + s)τ ∧) = exp(tτ ∧) exp(sτ ∧) (17) ∈ E M t we say that Eτ is expressed in the global frame. exp(tτ ∧) = exp(τ ∧) (18) Notice that while left- and right- are distinguished by the 1 ⊕ exp( τ ∧) = exp(τ ∧)− (19) operands order, the notation in (26) and (28) is ambiguous. − 1 1 exp( τ ∧ − ) = exp(τ ∧) − , (20) In this work we express perturbations locally by default and X X X X therefore we use the right- forms of and by default. where (20), a surprising and powerful statement, can be proved ⊕ easily by expanding the Taylor series and simplifying the many 1 F. The adjoint, and the adjoint matrix terms − . X X 1) The capitalized exponential map: The capitalized Exp If we identify in (25, 27), we arrive at Eτ = Xτ , and Log maps are convenient shortcuts to map vector elements which determinesY a relation between the local⊕ X andX global⊕ τ Rm (= T ) directly with elements . We have, ∼ E ∈ M X ∈ M 3The convention sticks to that of frame transformation, e.g. Gx = RLx, m Exp : R ; τ = Exp(τ ) (21) where the matrix R ∈ SO(3) transforms local vectors into global. Notice → Mm 7→ X that this convention is not shared by all authors, and for example [9] uses the Log : R ; τ = Log( ) . (22) L G M → X 7→ X opposite, x = R x. 7 tangent elements (Fig.7). We develop it with (20, 25, 27) as Example 6: The adjoint matrix of SE(3)

Exp(Eτ ) = Exp(Xτ ) The SE(3) group of rigid body motions (see App.D) has X X 1 1 exp(Eτ ∧) = exp(Xτ ∧) − = exp( Xτ ∧ − ) group, Lie algebra and vector elements, X X X X       τ = τ 1 R t [θ] ρ ρ E ∧ X ∧ − M = , τ = , τ = . X X 0 1 ∧ 0× 0 θ 1) The adjoint: We thus define the adjoint of at , The adjoint matrix is identified by developing (31) as noted Ad , to be M X X 1 AdM τ = (Mτ ∧M− )∨ = = 1 ··· Ad : m m; τ ∧ Ad (τ ∧) , τ ∧ − , (29)   X → 7→ X X X R [θ] R> R [θ] R>t + Rρ ∨ = × − × so that Eτ ∧ = Ad (Xτ ∧). This defines the adjoint action 0 0 X of the group on its own Lie algebra. The adjoint has two [Rθ] [t] Rθ + Rρ∨ = interesting (and easy to prove) properties, 0× × 0 [t] Rθ + Rρ R [t] R ρ Linear : Ad (aτ ∧ + bσ∧) = aAd (τ ∧) = = X X × Rθ 0 R× θ + bAd (σ∧) X where we used [Rθ] = R [θ] R and [a] b = Homomorphism : Ad (Ad (τ ∧)) = Ad (τ ∧) . > X Y XY [b] a. So the adjoint× matrix is × × − × 2) The adjoint matrix: Since Ad () is linear, we can find   X R [t] R 6 6 an equivalent matrix operator Ad that maps the Cartesian AdM = × R × . X 0 R ∈ tangent vectors Eτ ∼= Eτ ∧ and Xτ ∼= Xτ ∧, m m Ad : R R ; Xτ Eτ = Ad Xτ , (30) X → 7→ X 1) Reminder: Jacobians on vector spaces: For a multivari- which we call the adjoint matrix. This can be computed by ate function f : Rm Rn, the Jacobian matrix is defined as → applying to (29), thus writing the n m matrix stacking all partial derivatives, ∨ × ∂f ∂f 1  1 1  Ad τ = ( τ ∧ − )∨ , (31) ∂x1 ··· ∂xm X X X ∂f(x) . . n m J =  . .  R × . (35) ∂x ,  . .  ∈ then developing the right hand side to identify the adjoint ∂fn ∂fn matrix (see Ex.6 and the appendices). Additional properties ∂x1 ··· ∂xm of the adjoint matrix are, It is handy to define this matrix in the following form. Let us ∂f1 ∂fn partition J = [j1 jm], and let ji = [ ]> be its i-th τ = (Ad τ ) (32) ··· ∂xi ··· ∂xi X column vector. This column vector responds to X ⊕ 1 ⊕ X Ad −1 = Ad − (33) X X ∂f(x) f(x + hei) f(x) n ji = , lim − R , (36) Ad = Ad Ad . (34) ∂x h 0 h ∈ XY X Y i → m Notice in (33, 34) that the left parts of the equality are usually where ei is the i-th vector of the natural basis of R . cheaper to compute than the right ones. We will use the adjoint Regarding the numerator, notice that the vector matrix often as a way to linearly transform vectors of the n vi(h) , f(x + hei) f(x) R (37) tangent space at onto vectors of the tangent space at the − ∈ X origin, with Eτ = Ad Xτ ,(30). In this work, the adjoint is the variation of f(x) when x is perturbed in the direction X matrix will be referred to as simply the adjoint. of ei, and that the respective Jacobian column is just ji = ∂vi(h)/∂h h=0 = limh 0 vi(h)/h. In this work, for the sake of convenience,| we introduce→ the compact form, G. Derivatives on Lie groups ∂f(x) f(x + h) f(x) n m J = , lim − R × , (38) Among the different ways to define derivatives in the ∂x h 0 h ∈ context of Lie groups, we concentrate in those in the form → with h m, which aglutinates all columns (36) to form of Jacobian matrices mapping vector tangent spaces. This R the definition∈ of (35). We remark that (38) is just a notation is sufficient here, since in these spaces uncertainties and convenience (just as (35) is), since division by the vector h increments can be properly and easily defined. Using these is undefined and proper computation requires (36). However, Jacobians, the formulas for uncertainty management in Lie this form may be used to calculate Jacobians by developing groups will largely resemble those in vector spaces. the numerator into a form linear in h, and identifying the left The Jacobians described hereafter fulfill the chain rule, so hand side as the Jacobian, that is, that we can easily compute any Jacobian from the partial Jacobian blocks of inversion, composition, exponentiation and f(x+h) f(x) Jh ∂Jh lim − = = lim , = J. (39) action h 0 h ··· h 0 h ∂h . See Section III-A for details and proofs. → → 8

⌧2 = he2 XD T Tf( ) ED Y = f( ) X M j1 X N X Y D Y X f( ) XD X ⌧1 = he1 Ad X X X Ad Y X X⌧ Y ⌧1 E⌧ YD X 1(h) 2(h) E Y E ED X E M f( ) j M ED N · 2 Y f( ⌧1) D X X N Figure 9. Linear maps between all tangent spaces involved in a function Y = E X E Y Figure 8. Right Jacobian of a function f : M → N . The perturbation vectors f(X ), from M to N . The linear maps τ = AdX τ , σ = AdY σ, E DY X DY in the canonical directions, τi = hei ∈ T M, are propagated to perturbation E E Y X X σ = DX τ , and σ = DX τ , form a loop (solid) that leads to (46). vectors σi ∈ Tf(X )N through the processes of plus, apply f(), and minus The crossed Jacobians (dashed) form more mapping loops leading to (47,48). (green arrows), obtaining σi(h) = f(X ⊕hei) f(X ). For varying values of h, notice that in M the perturbations τi(h) = hei (thick red) produce paths in M (blue) along the geodesic (recall Fig.1). Notice also that in N , due to the non-linearity of f(·), the image paths (solid blue) are generally not in the (see Fig.8 again, and compare σi in (42) with vi in (37)) is geodesic (dashed blue). These image paths are lifted onto the tangent space the variation of f( ) when varies in the direction of e . X X i Tf(X )N , producing smooth curved paths (thin solid red). The column vectors Its respective Jacobian column is j = ∂σ (h)/∂h . j of J (thick red) are the derivatives of the lifted paths evaluated at f(X ), i.e., i i h=0 i As before, we use (41a) to actually find Jacobians| by ji = limh→0 σi(h)/h. Each hei ∈ TX M gives place to a ji ∈ Tf(X )N , n×m and thus the resulting Jacobian matrix J = [ j1 ··· jm ] ∈ R linearly resorting to the same mechanism (39). For example, for a 3D ∼ m ∼ n maps vectors from TX M = R to Tf(X )N = R . rotation f : SO(3) R3; f(R) = Rp, we have = SO(3) → M and = R3 and so (see App. B-C5), N Notice finally that for small values of h we have the linear RDRp (R θ)p Rp R Exp(θ)p Rp = lim ⊕ = lim − approximation, DR θ 0 θ θ 0 θ → → R(I + [θ] )p Rp R [θ] p ∂f(x) = lim × − = lim × f(x + h) f(x) + h . (40) θ θ −−−→h 0 ∂x 0 θ 0 θ → → → R [p] θ 3 3 2) Right Jacobians on Lie goups: Inspired by the standard = lim − × = R [p] R × . θ 0 θ − × ∈ derivative definition (38) above, we can now use our and → operators to define Jacobians of functions f : ⊕ acting Many examples of this mechanism can be observed in Sec- on manifolds (see Fig.8). Using the right- M, →in N place of tion III and in the appendices. Remark that whenever the +, we obtain a form akin to the standard{⊕ } derivative,4 function f passes from one manifold to another, the plus and { −} minus operators in (41a) must be selected appropriately: for X Df( ) f( τ ) f( ) n m ⊕ X , lim X ⊕ X R × (41a) the domain , and for the codomain or image . D τ 0 τ ∈ M N X → For small values of τ , the following approximation holds, which develops as, X Df( ) 1
Log f( )− f( Exp(τ )) f( Xτ ) f( ) X Xτ . (43) = lim X ◦ X ◦ (41b) X ⊕ −−−−→Xτ 0 X ⊕ D ∈ N τ 0 τ → X → 3) Left Jacobians on Lie groups: 1
Derivatives can also be ∂ Log f( )− f( Exp(τ )) = X ◦ X ◦ . (41c) defined from the left- plus and minus operators, leading to, ∂τ τ =0 E Df( ) f(τ ) f( ) n m X lim ⊕ X X R × (44) We call this Jacobian the right Jacobian of f. Notice that (41c) , τ 0 D → τ ∈ X 1 is just the standard derivative (38) of the rather complicated Log(f(Exp(τ ) ) f( )− ) 1
= lim ◦ X ◦ X function g(τ ) = Log f( )− f( Exp(τ )) . Writing it τ 0 τ X ◦ X ◦ → as in (41a) conveys much more intuition: it is the derivative 1
∂ Log f(Exp(τ ) ) f( )− of f( ) with respect to , only that we expressed the = ◦ X ◦ X , X X ∂τ infinitesimal variations in the tangent spaces! Indeed, thanks τ =0 to the way right- and operate, variations in and f( ) ⊕ X X which we call the left Jacobian of f. Notice that now are now expressed as vectors in the local tangent spaces, i.e., τ T , and the numerator belongs to T , thus the left tangent respectively at and f( ) . This derivative Jacobian∈ E M is a n m matrix mapping the globalE Ntangent spaces, X ∈ M n Xm ∈ N × is then a proper Jacobian matrix R × linearly mapping the T T , which are the Lie algebras of and (and E M → E N M N local tangent spaces T Tf( ) (and we mark the we mark the derivative with a global or origin ‘ ’ superscript). X M → X N derivative with a local ‘ ’ superscript). Just as in vector For small values of τ the following holds, E spaces, the columns of thisX matrix correspond to directional derivatives. That is, the vector E Df( ) f(Eτ ) X Eτ f( ) . (45) ⊕ X −−−−→Eτ 0 D ⊕ X ∈ N n → σi(h) = f( hei) f( ) R (42) X X ⊕ X ∈ We can show from (32, 43, 45) (see Fig.9) that left and 4The notation DY = Df(X ) is chosen in front of other alternatives in right Jacobians are related by the adjoints of and , DX DX M N order to make the chain rule readable, i.e., DZ = DZ DY . We will later DX DY DX Df( ) Df( ) Y DY E X introduce the lighter notation J , . X Ad = Adf( ) X . (46) X DX D X X D X X 9

T 0 3 ⌧4 T ¯ X M ⌧1 X ¯ X M ⌧3 3 4 X 1 ⌧2 X4 ⌧1 2 2 1 X 1 X 0 M X M

M Figure 11. Motion integration on a manifold. Each motion data produces

a step τk ∈ TXk−1 M, which is wrapped to a local motion increment or Figure 10. Uncertainty around a point X¯ ∈ M is properly expressed as a ‘delta’ δk = Exp(τk) ∈ M, and then composed with Xk−1 to yield Xk = covariance on the vector space tangent at the point (red). Using ⊕ (51), the Xk−1 ◦ δk = Xk−1 ◦ Exp(τk) = Xk−1 ⊕ τk ∈ M. probability ellipses in the tangent space are wrapped over the manifold (blue), thus illustrating the probability concentration region on the group. 2 2 a covariance E Σ = diag(σ , σ , ). Since “horizontal” is φ θ ∞ a global specification, Σ must be specified in the global 4) Crossed right–left Jacobians: One can also define Jaco- E reference. bians using right-plus but left-minus, or vice versa. Though Notice finally that there is no consensus on this last point. improbable, these are sometimes useful, since they map local While [6] and us use local perturbations = ¯ τ ,[5], to global tangents or vice versa. To keep it short, we will just X [10] use perturbations around the origin, X= τ X ⊕¯, yielding relate them to the other Jacobians through the adjoints, E global covariance specifications. Since globalX and⊕ localX pertur- E D E D Y D bations are related by the adjoint (30), their covariances can Y = Y Ad = Ad Y (47) D D X Y D be transformed according to X X E X X X Y D Y D 1 1 E D Y = Y Ad − = Ad − Y , (48) E Σ = Ad X Σ Ad > . (54) D D X Y D X X X X E X X X E X where = f( ). Now, the upper and lower super-scripts Covariance propagation through a function f : M → indicateY the referenceX frames where the differentials are ex- ; = f( ) just requires the linearization (43) with N X 7→ Y X pressed. Respective small-tau approximations read, Jacobian matrices (41a) to yield the familiar formula,

E Df( ) Df Df > n n f( X τ ) X X τ f( ) (49) Σ Σ R × . (55) X ⊕ −−−−→Xτ 0 D ⊕ X Y ≈ D X D ∈ → X X f( ) X X X Df( ) f(E τ ) f( ) X E τ . (50) I. Discrete integration on manifolds ⊕ X −−−−→Eτ 0 X ⊕ D → E X The exponential map (t) = 0 Exp(vt) performs the H. Uncertainty in manifolds, covariance propagation continuous-time integralX of constantX ◦ velocities v T ∈ X0 M We define local perturbations τ around a point ¯ in onto the manifold. Non-constant velocities v(t) are typically X ∈ M the tangent vector space T ¯ , using right- and , handled by segmenting them into piecewise constant bits X M ⊕ vk T k− , of (short) duration δtk, and writing the ¯ ¯ X 1 = τ , τ = T ¯ . (51) discrete∈ integralM X X ⊕ X X ∈ X M Covariances matrices can be properly defined on this tangent k = 0 Exp(v1δt1) Exp(v1δt2) Exp(vkδtk) space at ¯ through the standard expectation operator E[ ], X X ◦ ◦ ◦ · · · ◦ X · = 0 v1δt1 v1δt2 vkδtk . ¯ ¯ m m X ⊕ ⊕ ⊕ · · · ⊕ Σ , E[τ τ >] = E[( )( )>] R × , (52) X X X X X ∈ Equivalently (Fig. 11), we can define τk = vkδtk and allowing us to define Gaussian variables on manifolds, construct the integral as a “sum” of (small) discrete tangent ¯ X ∼ ( , Σ ), see Fig. 10. Notice that although we write Σ , the steps τk T k−1 , i.e., k , 0 τ1 τ2 τk. We X X ∈ X M X X ⊕ ⊕ ⊕ · · · ⊕ NcovarianceX is rather that of the tangent perturbation τ . Since write all these variants in recursive form, the dimension m of T matches the degrees of freedom of , these covariances areM well defined.5 k = k 1 τk = k 1 Exp(τk) = k 1 Exp(vkδtk) . X X − ⊕ X − ◦ X − ◦ (56) MPerturbations can also be expressed in the global reference, that is, in the tangent space at the origin T , using left- Common examples are the integration of 3D angular rates E M ⊕ and , ω into the rotation matrix, Rk = Rk 1 Exp(ωkδt), or into − the quaternion, qk = qk 1 Exp(ωkδt). = τ ¯ , τ = ¯ T . (53) − X ⊕ X X X ∈ E M This allows global specification of covariance matrices using III.DIFFERENTIATION RULES ON MANIFOLDS left-minus in (52). For example, a 3D orientation that is known For all the typical manifolds that we use, we can deter- up to rotations in the horizontal plane can be associated to mine closed forms for the elementaryM Jacobians of inversion, 5 ¯ ¯ > composition, exponentiation and action. Moreover, some of A naive definition ΣX , E[(X − X )(X − X ) ] is always ill-defined if size(X ) > dim(M), which is the case for most non-trivial manifolds. these forms can be related to the adjoint Ad , which becomes X 10 a central block of the differentiation process. Other forms for 2) Composition: We define with (41a) Log, and can be easily derived from them. Once these ⊕ X D m m forms or ‘blocks’ are found, all other Jacobians follow by JX ◦Y , X ◦ Y R × (63) X D ∈ the chain rule. Except for the so called left Jacobian, which X we also present below, all Jacobians developed here are right- Y D m m JX ◦Y , X ◦ Y R × , (64) Jacobians, i.e., defined by (41a). By following the hints here, Y D ∈ Y the interested reader should find no particular difficulties in and using (31) as above and (33), developing the left-Jacobians. For the reader not willing to do 1 this effort, equation (46) can be used to this end, since JX ◦Y = Ad − (65) X Y JX ◦Y = I (66) E Df( ) X Df( ) 1 X = Adf( ) X Ad − . (57) Y D X D X X X 3) Jacobians of : We define the right Jacobian of as f( ) Df( ) D X M m M We use the notations J , D X and JY , DY . the right Jacobian of = Exp(τ ), i.e., for τ R , X1 X X X X ∈ We notice also that Ad − should rather be implemented X τ D Exp(τ ) by −1 —see (33, 34) and the comment below them. m m Ad J (τ ) R × , (67) X r , Dτ ∈ A. The chain rule which is defined with (41a). The right Jacobian maps vari- For = f( ) and = g( ) we have = g(f( )). The ations of the argument τ into variations in the local tangent chain ruleY simplyX states,Z Y Z X space at Exp(τ ). From (41a) it is easy to prove that, for small δτ , the following approximations hold, D D D Z = Z Y or JZ = JZ JY . (58) D D D X Y X Exp(τ + δτ ) Exp(τ ) Exp(J (τ )δτ ) (68) X Y X ≈ r We prove it here for the right Jacobian using (43) thrice, 1 Exp(τ ) Exp(δτ ) Exp(τ + Jr− (τ ) δτ ) (69) ≈ 1 g(f( )) JZ τ g(f( τ )) g(f( ) JY τ ) Log(Exp(τ ) Exp(δτ )) τ + Jr− (τ ) δτ . (70) X ⊕ X ← X ⊕ → X ⊕ X ≈ g(f( )) JZ JY τ → X ⊕ Y X Complementarily, the left Jacobian of is defined by, M with the arrows indicating limit as τ 0, and so JZ = → X E D Exp(τ ) J JY . The proof for the left and crossed Jacobians is akin, m m Z Jl(τ ) , R × , (71) usingY X respectively (45, 49, 50). Notice that when mixing right, Dτ ∈ left and crossed Jacobians, we need to chain also the reference using the left Jacobian (44), leading to the approximations frames, as in e.g. Exp(τ + δτ ) Exp(J (τ )δτ ) Exp(τ ) (72) D D D D D ≈ l Z Z Y Z E (59) 1 Z = Z Y = Z Y Exp(δτ ) Exp(τ ) Exp(τ + Jl− (τ ) δτ ) (73) E D Y D E D E D E D ≈ X Y X Y X 1 (74) E D E D Y D E D E D Log(Exp(δτ ) Exp(τ )) τ + Jl− (τ ) δτ . Z = Z Y = Z Y , (60) ≈ D D D D D X X Y Y X X E Y X X The left Jacobian maps variations of the argument τ into vari- where the first identity of (59) is proven by writing, ations in the global tangent space or Lie algebra. From (68, 72)

(50) Z D we can relate left- and right- Jacobians with the adjoint, g(f(Eτ )) g(f( )) Z Eτ ; E ⊕ X −−−−→τ 0 X ⊕ E D 1 →  X  AdExp(τ ) = Jl(τ ) Jr− (τ ) . (75) (50) Y D g(f(Eτ )) g f( ) Y Eτ ⊕ X −−−−→Eτ 0 X ⊕ D → Also, the chain rule allows us to relate J and J , → E X r l (43) Z D Y D Exp( τ ) Exp( τ ) τ Exp(τ )−1 g(f( )) Z Y Eτ , Jr( τ ) , J τ − = Jτ − J τ = Jτ ( I) Eτ 0 D D −−−−→ X ⊕ Y E − − −1 − − → Y X Exp(τ ) Exp(τ ) and identifying (59) in the first and third rows. = J Jτ = AdExp(τ )J (τ ) − Exp(τ ) r = Jl(τ ) . (76) B. Elementary Jacobian blocks 1 1 1) Inverse: We define with (41a) Closed forms of Jr, Jr− , Jl and Jl− exist for the typical 1 manifolds in use. See the appendices for reference. −1 X D − m m JX , X R × . (61) 4) Group action: For and v , we define with X D ∈ X ∈ M ∈ V X (41a) This can be determined from the adjoint using (20) and (31), 1 1 1 v X D v −1 Log(( − )− ( Exp(τ ))− ) JX· , X· (77) JX = lim X X X D X τ 0 v → τ D X v 1 v Log( Exp( τ ) − ) JvX· , X· . (78) = lim X − X Dv τ 0 → τ 1 Since group actions depend on the set , these expressions ( ( τ )∧ − )∨ = lim X − X = Ad . (62) cannot be generalized. See the appendicesV for reference. τ 0 X → τ − 11

C. Useful, but deduced, Jacobian blocks Example 7: SE(n) vs. T (n) SO(n) vs. Rn, SO(n) × h i 1) Log map: For τ = Log( ), and from (70), X We consider the space of translations t Rn and rota- Log( ) 1 SO ∈ SE J X = Jr− (τ ) . (79) tions R (n). We have for this the well-known (n) X ∈ R t manifold of rigid motions M = [ 0 1 ] (see Apps.C and 2) Plus and minus: We have D), which can also be constructed as T (n) SO(n) (see × τ (Exp(τ )) 1 Apps.A,B andE). These two are very similar, but have JX ⊕ = JX ◦ = AdExp(τ )− (80) X X different tangent parametrizations: while SE(n) uses τ = τ (Exp(τ )) Exp(τ ) JX ⊕ = JX ◦ J = J (τ ) (81) τ Exp(τ ) τ r (θ, ρ) with M = exp(τ ∧), T (n) SO(n) uses τ = (θ, p) × 1 with M = exp(p∧) exp(θ∧). They share the rotational and given = − and τ = = Log( ), Z X ◦ Y Y X Z part θ, but clearly ρ = p (see [11, pag. 35] for further Log( ) −1 1 details). In short, SE 6 performs translation and rotation JY X = J Z JZ −1 JX = Jl− (τ ) (82) (n) X Z X X − Log( ) 1 simultaneously as a continuum, while T (n) SO(n) JY X = J Z JZ = Jr− (τ ) . (83) × Y Z Y performs chained translation+rotation. In radical contrast, where the former is proven here in the composite Rn, SO(n) rotations and translations do not interact ath all. By combiningi composition with Log( −1 ) ( −1 ) −1 X ◦Y X ◦Y JY X = J( −1 ) J −1 JX Exp() we obtain the (right) plus operators, X X ◦Y X X 1 1   (79, 65, 62) = Jr− (τ ) Ad − ( Ad ) Y X R Exp(θ) t + RV(θ)ρ 1 − SE(n): M τ = (33, 34) = Jr− (τ ) Ad −1 ⊕ 0 1 − Y X 1 1   = J− (τ ) Ad − R Exp(θ) t + Rp − r Exp(τ ) T (n) SO(n): M τ = 1 × ⊕ 0 1 (75) = J− (τ ) . − l   n t + p R , SO(n) : M τ = IV. COMPOSITEMANIFOLDS h i R Exp(θ) At the price of losing some consistency with the Lie theory, where either may be used for the system dynamics, ⊕ but at the benefit of obtaining some advantages in notation and e.g. motion integration, but usually not , which might manipulation, one can consider large and heterogeneous states however be used to model perturbations. Their respective as manifold composites (or bundles). minus operators read, A composite manifold = 1, , M is no less  1  M hM ··· M i V1− R1>(p2 p1) than the concatenation of M non-interacting manifolds. This SE(n): M2 M1 = − Log(R1>R2) stems from defining identity, inverse and composition acting   R1>(p2 p1) on each block of the composite separately, T (n) SO(n): M2 M1 = − × Log(R1>R2)    1   1 − 1   E X X ◦ Y n p2 p1  .   .   .  R , SO(n) : M2 M1 = − , , . ,  , . , , . , h i Log(R>R2)   .   .   .  1 E X 1 X  Y M − M M E XM X ◦ Y where now, interestingly, can be used to evaluate (84) errors and uncertainty. This makes , valuable op- thereby fulfilling the group axioms, as well as a non- erators for computing derivatives and covariances. interacting retraction map, which we will also note as “ex- ponential map” for the sake of unifying notations (notice the angled brackets), With this derivative, Jacobians of functions f : M → N     acting on composite manifolds can be determined in a per- Exp(τ1) Log( ) . . X block basis, which yields simple expressions requiring only Exp τ  .  , Log  .  , (85) h i ,  .  hX i ,  .  knowledge on the manifold blocks of the composite, Exp(τM ) Log( M ) X  Df1 Df1  D 1 D M thereby ensuring smoothness. These yield the composite’s Df( ) X. ··· X. X =  . .. .  , (89) right- plus and minus (notice the diamond symbols), D  . . .  X DfN DfN τ Exp τ (86) D 1 ··· D M X , X  h i X X , Log  . (87) where Dfi are each computed with (41a). For small values D j Y X hX  Yi X The key consequence of these considerations (see Ex.7) is of τ the following holds, 6 that new derivatives can be defined, using and , Df( ) f( τ ) f( ) X τ . (90) Df( ) f( τ ) f( ) X −−−→τ 0 X D ∈ N X , lim X X . (88) → X D τ 0 τ X → When using these derivatives, covariances and uncertainty 6We assume here right derivatives, but the same applies to left derivatives. propagation must follow the convention. In particular, the 12

covariance matrix (52) becomes b5 ¯ ¯ n n 3 Σ , E[( )( )>] R × , (91) 1 X X X X X X ∈ X 2 b6 for which the linearized propagation (55) using (88) applies. b4 X

Figure 12. SAM factor graph with 3 poses and 3 beacons. Each measurement ANDMARK BASEDLOCALIZATIONANDMAPPING V. L - contributes a factor in the graph. There are 2 motion factors (black) and 5 We provide three applicative examples of the theory for beacon factors (gray). A prior factor on X1 provides global observability. robot localization and mapping. The first one is a Kalman filter for landmark-based localization. The second one is a At each robot motion we apply ESKF prediction, graph-based smoothing method for simultaneous localization and mapping. The third one adds sensor self-calibration. They ˆj = ˆi uj (99) are based on a common setup, explained as follows. X X ⊕ P = FP F + GW G , (100) We consider a robot in the plane (see Section V-D for the j i > j > 3D case) surrounded by a small number of punctual landmarks with the Jacobians computed from the blocks in App.C, or beacons. The robot receives control actions in the form of j ˆi uj 1 X X ⊕ axial and angular velocities, and is able to measure the location F J = J = AdExp(uj )− , i ˆi X X of the beacons with respect to its own reference frame. j ˆi uj G JX = JX ⊕ = J (u ) . The robot pose is in SE(2) (App.C) and the beacon , uj uj r j 2 positions in R (App.E), At each beacon measurement yk we apply ESKF correction,     R t xk 2 Innovation : z = y ˆ 1 b = SE(2) , bk = R . k − k X 0 1 ∈ yk ∈ − X · Innovation cov. : Z = HPH> + N The control signal u is a twist in se(2) comprising longitu- 1 Kalman gain : K = PH> Z− dinal velocity v and angular velocity ω, with no lateral velocity component, integrated over the sampling time δt. The control Observed error : δx = Kz is corrupted by additive Gaussian noise w (0, W). This State update : ˆ ˆ δx (101) noise accounts for possible lateral wheel slippages∼ N u through X ← X ⊕ s Cov. update : P P KZK> , (102) a value of σ = 0, ← − s 6     with the Jacobian computed from the blocks in App.C, uv v δt −1 −1 −1 bk bk u = us  =  0  + w se(2) (92) H JX · = JX − · J ∈ , 1 X u ω δt X X  X  ω   R [1] t  2  = R> R> [1] bk − × σvδt 0 0 × 0 1 2 3 3 − W =  0 σs δt 0  R × . (93)   2 ∈ = IR [1] (bk t) . 0 0 σwδt − × − Notice that the only changes with respect to a regular EKF At the arrival of a control uj at time j, the robot pose is updated with (56), are in (99) and (101), where regular + are substituted by . The Jacobians on the contrary are all computed using the Lie⊕ j = i uj , i Exp(uj) . (94) theory (see App.C). Interstingly, their usage is the same as X X ⊕ X in standard EKF — see e.g. the equation of the Kalman gain, Landmark measurements are of the range and bearing type, which is the standard K = PH (HPH + N) 1. though they are put in Cartesian form for simplicity. Their > > − noise n (0, N) is zero mean Gaussian, ∼ N B. Smooting and Mapping with graph-based optimization 1 2 yk = − bk + n = R>(bk t) + n R (95) X · − ∈ We consider now the problem of smoothing and mapping  2  σx 0 2 2 (SAM), where the variables to estimate are the beacons’ N = 2 R × , (96) 0 σy ∈ locations and the robot’s trajectory. The solver of choice is 1 a graph-based iterative least-squares optimizer. For simplicity, where we notice the rigid motion action − bk (see App.C). X · we assume the trajectory comprised of three robot poses , and a world with three beacons b b . The A. Localization with error-state Kalman filter on manifold 1 3 4 6 {Xproblem···X state} is the composite { ··· } We initially consider the beacons b situated at known k 2 ˆ = 1, 2, 3, b4, b5, b6 , i SE(2), bk R . positions. We define the pose to estimate as SE(2). The X hX X X i X ∈ ∈ estimation error δx and its covariance P areX expressed ∈ in the (103) tangent space at ˆ with (51, 52), X The resulting factor graph [12] is shown in Fig. 12. Each 3 prior or measurement contributes a factor in the graph. Motion δx , ˆ R (97) X X ∈ measurements from pose i to j are derived from (94), while 3 3 P E[( ˆ)( ˆ)>] R × . (98) , X X X X ∈ measurements of beacon k from pose i respond to (95), 13

1 u = + w = Log( − ) + w (104) so that the control is now u˜ = (vδt + c , 0, ωδt + c )> + w. ij Xj Xi ij Xi Xj ij v ω 1 We define the bias correction function c(), yik = i− bk + nik . (105) X ·   1 u˜v cv Each factor comes with an information matrix, Ω1 , W1− , − 3 1 1 u = c (u˜, c) ,  u˜s  R = se(2) . (111) Ω W− and Ω N− . The expectation residuals are, ∼ ij , ij ik , ik u˜ c ∈ ω − ω /2 prior residual : r ( ) = Ω> ( ˆ ) The state composite is augmented with the unknowns c, 1 X 1 X1 X1 /2 ˆ ˆ motion residual : rij( ) = Ω> (uij ( j i)) = c, , , , b , b , b , X ij − X X X h X1 X2 X3 4 5 6i /2 1 2 2 beacon residual : r ( ) = Ω> (y ˆ− bˆ ) . c R , i SE(2), bk R , ik X ik ik − Xi · k ∈ X ∈ ∈ The optimum update step δx stems from minimizing and the motion residual becomes /2
X rij( ) = Ω> c (u˜ij, c) ( ˆj ˆi) . δx∗ = arg min r ( δx)>r ( δx) (106) ij p X p X X − X X δx p ∈P The procedure is as in Section V-B above, and just the total with = 1, 12, 23, 14, 15, 25, 26, 36 the set of node pairs Jacobian is modified with an extra column on the left, P { } of each measurement (see Fig. 12). The problem is solved  0 Jr1 0 0 0 0 0  1 iteratively as follows. Each residual in the sum (106) is Jr12 JrX12 Jr12 0 0 0 0  c 1 2  rp  r23 X rX23 r23  linearized to rp( δx) rp( ) J δx following (90), Jc 0 J J 0 0 0  rp X ≈ X X 2 3 where J are sparse Jacobians. The non-zero blocks of these  0 Jr14 0X 0X Jr14 0 0   1 b4  X r1 rij rij rik rik J = , Jacobians, that is J , J , J , J and J , can be easily  Xr15 r15  1 i j i bk  0 J 0 0 0 Jb 0  X X X X  X1 5  computed following the methods in Section V-A, and noticing  0 0 Jr25 0 0 Jr25 0  f( δx) f( δx) f( ) 2 b5  rX26 r26  that by definition JδxX ⊕ δx=0 = J X ⊕ δx=0 = J X .  0 0 J 0 0 0 J  | X | X 2 b6 Building the total Jacobian matrix and residual vector, X r36 r36 0 0 0 J 0 0 Jb X3 6  r1    J 0 0 0 0 0 r rij /2 c(uij ,c) c(uij ,c) 1 1 where Jc = Ω> Jc , with Jc the 3 2 Jacobian JrX12 Jr12 0 0 0 0 ij ×  1 2  r12 of (111). The optimal solution is obtained with (109, 110). The  0X JrX23 Jr23 0 0 0  r   2 3   23 resulting optimal state includes an optimal estimate of c, Jr14 0X 0X Jr14 0 0  r  X  1 b4   14 that is, the self-calibration of the sensor bias. J =  rX15 r15  r =   J 0 0 0 Jb 0  r15  X1 5    0 Jr25 0 0 Jr25 0 r  2 b5   25  rX26 r26    D. 3D implementations 0 J 0 0 0 J r26  2 b    X r r 6 0 0 J 36 0 0 J 36 r36 It is surprisingly easy to bring all the examples above to 3 b6 X (107) 3D. It suffices to define all variables in the correct spaces: SE 6 3 (3) and u R ∼= se(3) (App.D), and bk, y R the linearized (106) is now transformed [12] to minimizing X(App. ∈ E). Jacobians∈ and covariances matrices will{ follow} ∈with 2 appropriate sizes. The interest here is in realizing that all the δx∗ = arg min r + Jδx . (108) δx k k math in the algorithms, that is from (97) onwards, is exactly the same for 2D and 3D: the abstraction level provided by the This is solved via least-squares using the pseudoinverse of Lie theory has made this possible. J (for large problems, QR [12], [13] or Cholesky [14], [15] factorizations are required), VI.CONCLUSION 1 δx∗ = (J>J)− J>r , (109) We have presented the essential of Lie theory in a form that − should be useful for an audience skilled in state estimation, yielding the optimal step used to update the state, δx∗ with a focus on robotics applications. This we have done through several initiatives: δx∗ . (110) X ← X First, a selection of materials that avoids abstract mathe- The procedure is iterated until convergence. matical concepts as much as possible. This we hope helps We highlight here the use of the composite notation in (103), focusing Lie theory to make its tools easier to understand and which allows block-wise definitions of the Jacobian (107) and to use. the update (110). We also remark the use of the SE(2) manifold Second, we chose a didactical approach, with significant in the motion and measurement models, as we did in the ESKF redundancy. The main text is generic and covers the abstract case in Section V-A. points of Lie theory. It is accompanied by boxed examples, which ground the abstract concepts to particular Lie groups, and plenty of figures with very verbose captions. C. Smoothing and mapping with self-calibration Third, we have promoted the usage of handy operators, We consider the same problem as above but with a motion such as the capitalized Exp() and Log() maps, and the plus sensor affected by an unknown calibration bias c = (c , c )>, and minus operators , , , . They allow us to work on v ω ⊕ 14 the Cartesian representation of the tangent spaces, producing where (114) is the Euler formula, whereas for SO(2), formulas for derivatives and covariance handling that greatly   cos θ sin θ 2 2 resemble their counterparts in standard vector spaces. R = Exp(θ) = − R × (116) sin θ cos θ ∈ Fourth, we have made especial emphasis on the definition, θ = Log(R) = arctan(r21, r11) R . (117) geometrical interpretation, and computation of Jacobians. For ∈ this, we have introduced notations for the Jacobian matrices and covariances that allow a manipulation that is visually B. Inverse, composition, exponential map powerful. In particular, the chain rule is clearly visible with We consider generic 2D rotation elements, and note them this notation. This helps building intuition and reducing errors. with the sans-serif font, Q, R. We have Fifth, we present in the appendices that follow an extensive 1 R(θ)− = R( θ) (118) compendium of formulas for the most common groups in − robotics. In 2D, we present the rotation groups of unit complex Q R = R Q , (119) 1 ◦ ◦ numbers S and rotation matrices SO(2), and the rigid motion i.e., planar rotations are commutative. It follows that, group SE(2). In 3D, we present the groups of unit quaternions S3 and rotation matrices SO(3), both used for rotations, and Exp(θ1 + θ2) = Exp(θ1) Exp(θ2) (120) ◦ the rigid motion group SE(3). We also present the translation Log(Q R) = Log(Q) + Log(R) (121) groups for any dimension, which can be implemented by either ◦ Q R = θQ θR . (122) the standard vector space Rn under addition, or by the matrix − translation group T (n) under multiplication. C. Jacobian blocks Sixth, we have presented some applicative examples to illustrate the capacity of Lie theory to solve robotics problems Since our defined derivatives map tangent vector spaces, and 1 with elegance and precision. The somewhat naive concept of these spaces coincide for the planar rotation manifolds of S composite group helps unifying heterogeneous state vectors and SO(2), i.e., θ = Log(z) = Log(R), it follows that the into a Lie-theoretic form. Jacobians are independent of the representation used (z or R). Finally, we accompany this text with the new C++ library 1) Adjoint and other trivial Jacobians: From (41a), Sec- manif [7] implementing the tools described here. manif can tion III-B and the properties above, the following scalar be found at https://github.com/artivis/manif. The applications derivative blocks become trivial, in SectionV are demonstrated in manif as examples. AdR = 1 R (123) Tough we do not introduce any new theoretical material, we ∈ R−1 believe the form in which Lie theory is here exposed will help JR = 1 R (124) Q R Q R − ∈ many researchers enter the field for their future developments. JQ◦ = JR◦ = 1 R (125) We also believe this alone represents a valuable contribution. ∈ Jr(θ) = Jl(θ) = 1 R (126) ∈ APPENDIX A R θ R θ JR⊕ = Jθ ⊕ = 1 R (127) 1 THE 2D ROTATION GROUPS S AND SO(2) Q R Q R ∈ JQ = JR = 1 R (128) The Lie group S1 is the group of unit complex numbers − ∈ 2) Rotation action: For the action R v we have, under complex product. Its topology is the unit circle, or the · 1 unit 1-sphere, and therefore the name S . The group, Lie R v R Exp(θ)v Rv JR· = lim − algebra and vector elements have the form, θ 0 θ → R(I + [θ] )v Rv z = cos θ + i sin θ, τ ∧ = iθ, τ = θ . (112) = lim × − 1 θ 0 θ Inversion and composition are achieved by conjugation z− = → R [θ] v 2 1 z∗, and product za zb = za zb. = lim × = R [1] v R × (129) ◦ θ 0 θ × ∈ The group SO(2) is the group of special orthogonal matrices → in the plane, or rotation matrices, under matrix multiplication. and Group, Lie algebra and vector elements have the form, R v DRv 2 2 Jv· = = R R × . (130)  cos θ sin θ   0 θ  Dv ∈ R = sin θ −cos θ , τ ∧ = [θ] , θ −0 , τ = θ . (113) × Inversion and composition are achieved by transposition APPENDIX B 3 1 THE 3D ROTATION GROUPS S AND SO(3) R− = R>, and product Ra Rb = Ra Rb. Both groups rotate 2-vectors,◦ and they have isomorphic The Lie group S3 is the group of unit quaternions under tangent spaces. We thus study them together. quaternion multiplication. Its topology is the unit 3-sphere in R4, and therefore its name S3. Quaternions (please consult A. Exp and Log maps [8] for an in-depth reference) may be represented by either of Exp and Log maps may be defined for complex numbers of these equivalent forms, S1 and rotation matrices of SO(2). For S1 we have, q = w + ix + jy + kz = w + v H   ∈ z = Exp(θ) = cos θ + i sin θ C (114)   w (131) ∈ = w x y z > = H , θ = Log(z) = arctan(Im(z), Re(z)) R , (115) v ∈ ∈ 15 where w, x, y, z R, and i, j, k are three unit imaginary C. Elementary Jacobian blocks numbers such that∈i2 = j2 = k2 = ijk = 1. The scalar w is Since our defined derivatives map tangent vector spaces, known as the scalar or real part, and v − as the vector or Hp and these spaces coincide for the 3D rotation manifolds of S3 imaginary part. We note the set of pure∈ quaternions, i.e., of Hp and SO(3), i.e., θ = Log(q) = Log(R), it follows that the null scalar part, with dimension 3. Inversion and composition Jacobians are independent of the representation used (q or R). are achieved by conjugation q 1 = q , where q w v is − ∗ ∗ , We thus consider generic 3D rotation elements, and note them the conjugate, and product q q = q q . − a ◦ b a b with the sans-serif font R. The group SO(3) is the group of special orthogonal matrices 1) Adjoint: We have from (31) in 3D space, or rotation matrices, under matrix multiplication. Inversion and composition are achieved with transposition and AdRθ = (R [θ] R>)∨ = ([(Rθ)] )∨ = Rθ product as in all groups SO(n). × × Both groups rotate 3-vectors. They have isomorphic tangent therefore spaces whose elements are identifiable with rotation vectors in 3 3 R , so we study them together. It is in this space R where we AdR = R , (139) define the vectors of rotation rate ω , uω, angle-axis θ , uθ, and all perturbations and uncertainties. which means, just to clarify it once again, that Adq = R(q), 3 The quaternion manifold S is a double cover of SO(3), see (138), and AdR = R. i.e., q and q represent the same rotation R. The first cover 2) Inversion, composition: We have from Section III-B, corresponds− to quaternions with positive real part w > 0. The R−1 two groups can be considered isomorphic up to the first cover. JR = AdR = R (140) QR − 1 − JQ = AdR− = R> (141) QR A. Exp and Log maps JR = I . (142) The Exp and Log maps may be defined for quaternions 3) Right and left Jacobians: They admit the closed forms of S3 and rotation matrices of SO(3). For quaternions q = [11, pag. 40], (w, v) H we have (see Ex.5), ∈ 1 cos θ θ sin θ 2 (143) q = Exp(θu) , cos(θ/2) + u sin(θ/2) H (132) Jr(θ) = I − 2 [θ] + − 3 [θ] ∈ − θ × θ × arctan( v , w)   3 1 1 1 1+cos θ 2 θu = Log(q) , 2 v k k R . (133) J − (θ) = I+ [θ] + [θ] (144) v ∈ r 2 k k 2 × θ − 2θ sin θ × 1 cos θ θ sin θ We can avoid eventual problems due to the double cover of q 2 (145) Jl(θ) = I + − 2 [θ] + − 3 [θ] by ensuring that its scalar part w is positive before doing the θ × θ ×   . If it is not, we can substitute by before the . 1 1 1 + cos θ 2 Log q q Log 1 (146) − Jl− (θ) = I [θ] + 2 [θ] For rotation matrices we have (see Ex.4), − 2 × θ − 2θ sin θ ×

2 3 3 R = Exp(θu) , I + sin θ [u] + (1 cos θ)[u] R × where we can observe that × − × ∈ (134) 1 Jl = Jr> , Jl− = Jr−> . (147) θ(R R>)∨ 3 θu = Log(R) − R , (135) , 2 sin θ ∈ 4) Right- plus and minus: We have for θ = Q R,

1 trace(R) 1
with θ = cos− − . R θ R θ 2 JR⊕ = R(θ)> Jθ⊕ = Jr(θ) (148) Q R 1 Q R 1 JQ = Jr− (θ) JR = Jl− (θ) (149) B. Rotation action − 5) Rotation action: Given the expressions above for the quaternion and the We have rotation matrix, the rotation action of quaternions on 3-vectors R v (R θ)v Rv is performed by the double quaternion product, JR· , lim ⊕ − = θ 0 θ → R Exp(θ)v Rv R(I+[θ] )v Rv x0 = q x q∗ (136) lim − = lim × − θ 0 θ θ 0 θ → → while rotation matrices use a single matrix product, R [θ] v R [v] θ = lim × = lim − × = R [v] θ 0 θ θ 0 θ − × x0 = Rx . (137) → → (150) Both correspond to a right-hand rotation of rad around the θ where we used the properties Exp(θ) I+[θ] and [a] b = axis . Identifying in them and yields the identity u x x0 [b] a. The second Jacobian yields,≈ × × − × " w2+x2 y2 z2 2(xy wz) 2(xz+wy) # − − 2 2 − 2 2 R(q)= 2(xy+wz) w x +y z 2(yz wx) (138) R v R(v + ∂v) Rv − − 2 2− 2 2 Jv· , lim − = R . (151) 2(xz wy) 2(yz+wx) w x y +z ∂v 0 ∂v − − − → 16

APPENDIX C 4) Rigid motion action: We have the action on points p, THE 2D RIGIDMOTIONGROUP SE(2) M p t + Rp , (165) We write elements of the rigid motion group SE(2) as · , R t therefore from (156), M = SE(2) 3 3 , (152) 0 1 R × ∈ ⊂ M p M Exp(τ ) p M p   JM· = lim · − · = RR [1] p with R SO(2) a rotation and t 2 a translation. The τ 0 τ × R → (166) Lie algebra∈ and vector tangents are formed∈ by elements of the M p type Jp · = R . (167)     [θ] ρ ρ 3 τ ∧ = × se(2) , τ = R . (153) 0 0 ∈ θ ∈ APPENDIX D THE 3D RIGIDMOTIONGROUP SE(3) A. Inverse, composition We write elements of the 3D rigid motion group SE(3) as Inversion and composition are performed respectively with   matrix inversion and product, R t 4 4 M = SE(3) R × , (168)   0 1 ∈ ⊂ 1 R> R>t M− = − (154) 0 1 with R SO(3) a rotation matrix and t R3 a translation ∈ ∈ R R t + R t  vector. The Lie algebra and vector tangents are formed by M M = a b a a b . (155) a b 0 1 elements of the type     [θ] ρ ρ 6 B. Exp and Log maps τ ∧ = × se(3) , τ = R . (169) 0 0 ∈ θ ∈ Exp and Log are implemented via exponential maps directly 3 SE from the scalar tangent space R ∼= se(2) = T (2) — see [5] for the derivation, A. Inverse, composition Exp(θ) V(θ) ρ Inversion and composition are performed respectively with M = Exp(τ ) (156) , 0 1 matrix inversion and product, V 1(θ) p R R t τ = Log(M) − . (157) M 1 = > > (170) , Log(R) − 0 − 1 with R R t + R t  M M = a b a a b . (171) sin θ 1 cos θ a b 0 1 V(θ) = I + − [1] . (158) θ θ × C. Jacobian blocks B. Exp and Log maps 1) Adjoint: The adjoint is easily found from (31) using the Exp and Log are implemented via exponential maps directly 6 SE fact that planar rotations commute, from the vector tangent space R ∼= se(3) = T (3) — see     [5] for the derivation, 1 Rρ [θ] t ρ AdMτ = (Mτ ∧M− )∨ = − = AdM ,   θ × θ Exp(θ) V(θ) ρ M = Exp(τ ) , (172) leading to 0 1  1    V− (θ) p R [1] t τ = Log(M) , . (173) AdM = − . (159) Log(R) 0 1 × 2) Inversion, composition: We have from Section III-B, with (recall for Log(M) that θ = θu = Log(R))

−1   1 cos θ θ sin θ M R [1] t 2 (174) JM = AdM = − × (160) V(θ) = I + − 2 [θ] + − 3 [θ] − 0 1 θ × θ × −   which, notice, matches (145) exactly. MaMb 1 Rb> Rb> [1] tb J = AdM − = (161) Ma b 0 1 × MaMb (162) JMb = I . C. Jacobian blocks 3) Right and left Jacobians: We have from [11, pag. 36], 1) Adjoint: We have (see Ex.6),

2  sin θ/θ (1 cos θ)/θ (θρ1 ρ2+ρ2 cos θ ρ1 sin θ)/θ      − − − 2 1 Rρ + [t] Rθ ρ Jr = (cos θ 1)/θ sin θ/θ (ρ1+θρ2 ρ1 cos θ ρ2 sin θ)/θ AdMτ = (Mτ ∧M− )∨ = × = AdM 0− 0− 1 − Rθ θ (163) 2 therefore,  sin θ/θ (cos θ 1)/θ (θρ1+ρ2 ρ2 cos θ ρ1 sin θ)/θ  − − − 2 Jl = (1 cos θ)/θ sin θ/θ ( ρ1+θρ2+ρ1 cos θ ρ2 sin θ)/θ .   − 0 0− 1 − R [t] R 6 6 AdM = × R × . (175) (164) 0 R ∈ 17

2) Inversion, composition: We have from Section III-B, The T (n) exponential can be obtained also from the Taylor expansion of exp(t ) noticing that (t )2 = 0. This serves as   ∧ ∧ M−1 R [t] R immediate proof for the equivalent exponential of the ( n, +) J = × (176) R M − 0 R group, which is the identity, R R [t ]  MaMb b> b> b (177) Exp : n n t = Exp(t) . (185) JMa = − × R R 0 Rb> → MaMb This derives in trivial, commutative, right- and left- alike, plus JM = I6 . (178) b and minus operators in Rn, 3) Right and left Jacobians: A closed form of the left t t = t + t (186) Jacobian and its inverse are given by Barfoot in [10], 1 ⊕ 2 1 2 h θ ρ θ i t2 t1 = t2 t1 . (187) J (ρ, θ) = Jl( ) Q( , ) (179a) − l 0 Jl(θ) −1 −1 −1 1 h J (θ) J (θ) Q(ρ,θ) J (θ) i l − l l A. Jacobian blocks Jl− (ρ, θ) = −1 (179b) 0 Jl (θ) We express translations indistinctly for T (n) and Rn, and where Jl(θ) is the left Jacobian of SO(3), see (145), and note them S and T. The Jacobians are trivial (compare them 1 θ sin θ with those of S1 and SO(2) in Section A-C1), Q(ρ, θ) = ρ + − (θ ρ + ρ θ + θ ρ θ ) 2 × θ3 × × × × × × × n n AdT = I R × (188) θ2 ∈ 1 2 cos θ 2 2 T−1 n n − − (θ ρ + ρ θ 3θ ρ θ ) JT = I R × (189) − θ4 × × × × − × × × − ∈ 2 3 T S T S n n θ θ ! JT◦ = JS ◦ = I R × (190) 1 1 cos θ θ sin θ ∈ − 2 − 3 − − 6 n n 4 5 J = J = I R × (191) − 2 θ − θ r l ∈ T v T v n n 2 2 J ⊕ = J ⊕ = I R × (192) (θ ρ θ + θ ρ θ ) . (180) T v ∈ × × × × × × × S T S T n n J = J = I R × . (193) The right Jacobian and its inverse are obtained using (76), that S − T ∈ is, J (ρ, θ) = J ( ρ, θ). r l REFERENCES 4) Rigid motion− action:− We have the action on points p, [1] H. Abbaspour and M. Moskowitz, Basic Lie Theory. WORLD M p , t + Rp , (181) SCIENTIFIC, 2007. [Online]. Available: https://worldscientific.com/ · doi/abs/10.1142/6462 therefore from (172), [2] R. Howe, “Very basic Lie theory,” The American Mathematical Monthly, vol. 90, pp. 600–623, 1983. M p M Exp(τ ) p M p   [3] J. Stillwell, Naive Lie Theory. Springer-Verlag New York, 2008. J · = lim · − · = R R [p] [4] T. D. Barfoot, State Estimation for Robotics. Cambridge University M τ 0 → τ − × Press, 2017. (182) [5] E. Eade, “Lie groups for 2d and 3d transformations,” Tech. Rep. M p J · = R . (183) [6] C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, “On-manifold p preintegration for real-time visual–inertial odometry,” IEEE Transactions on Robotics, vol. 33, no. 1, pp. 1–21, 2017. APPENDIX E [7] J. Deray and J. Sola,` “Manif: A micro lie theory library for state estimation in robotics applications,” Journal of Open Source THE TRANSLATION GROUPS ( n, +) AND T (n) R Software, vol. 5, no. 46, p. 1371, 2020. [Online]. Available: The group (Rn, +) is the group of vectors under addition, https://doi.org/10.21105/joss.01371 and can be regarded as a translation group. We deem it trivial [8] J. Sola,` “Quaternion kinematics for the error-state Kalman filter,” CoRR, vol. abs/1711.02508, 2017. [Online]. Available: http://arxiv.org/ in the sense that the group elements, the Lie algebra, and the abs/1711.02508 tangent spaces are all the same, so t = t∧ = Exp(t). Its [9] G. Gallego and A. Yezzi, “A compact formula for the derivative of a equivalent matrix group (under multiplication) is the transla- 3-D rotation in exponential coordinates,” Tech. Rep., 2013. [10] T. D. Barfoot and P. T. Furgale, “Associating uncertainty with three- tion group T (n), whose group, Lie algebra and tangent vector dimensional poses for use in estimation problems,” IEEE Transactions elements are, on Robotics, vol. 30, no. 3, pp. 679–693, June 2014.     [11] G. Chirikjian, Stochastic Models, Information Theory, and Lie Groups, I t 0 t n Volume 2: Analytic Methods and Modern Applications, ser. Applied and T T (n), t∧ t(n), t R . , 0 1 ∈ , 0 0 ∈ ∈ Numerical Harmonic Analysis. Birkhauser¨ Boston, 2011. [Online]. Available: https://books.google.ch/books?id=hZ1CAAAAQBAJ Equivalence is easily verified by observing that T(0) = I, [12] F. Dellaert and M. Kaess, “Square Root SAM: Simultaneous Localiza- 1 tion and Mapping via Square Root Information Smoothing,” vol. 25, T( t) = T(t)− , and that the commutative composition − no. 12, pp. 1181–1203, 2006.   [13] M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. Leonard, and F. Dellaert, I t1 + t2 “iSAM2: Incremental smoothing and mapping with fluid relineariza- T1T2 = , 0 1 tion and incremental variable reordering,” in Robotics and Automation (ICRA), 2011 IEEE International Conference on, May 2011, pp. 3281– effectively adds the vectors t1 and t2 together. Since the sum 3288. in Rn is commutative, so is the composition product in T (n). [14] R. Kummerle, G. Grisetti, H. Strasdat, K. Konolige, and W. Burgard, Since is a subgroup of SE with , we can easily “G2o: A general framework for graph optimization,” in Robotics and T (n) (n) R = I Automation (ICRA), 2011 IEEE International Conference on, May 2011, determine its exponential map by taking (156, 172) with R = I pp. 3607–3613. and generalizing to any n, [15] V. Ila, L. Polok, M. Solony, and P. Svoboda, “SLAM++ - a highly   efficient and temporally scalable incremental SLAM framework,” The n I t International Journal of Robotics Research, vol. 36, no. 2, pp. 210–230, Exp : R T (n); T = Exp(t) = . (184) → 0 1 2017. [Online]. Available: https://doi.org/10.1177/0278364917691110