Common Lie Group and Lie Algebra Formulae in SLAM

In SLAM, rotation and pose are usually represented by rotation matrices or quaternions rather than ordinary vectors. Lie groups and Lie algebras are therefore introduced for optimization and filtering. Classic references include:

• Joan Sola, A Micro Lie Theory for State Estimation in Robotics.
• E. Eade, Lie Groups for 2D and 3D Transformations.
• C. Forster, On-Manifold Preintegration for Real-Time Visual-Inertial Odometry.
• T. D. Barfoot, State Estimation for Robotics.
• G. Chirikjian, Stochastic Models, Information Theory, and Lie Groups.

Joan Sola’s article is particularly detailed and strongly recommended for understanding the concepts. It resolves most practical questions about using Lie groups and Lie algebras for attitude estimation. The formulae below are excerpts for reference.

Key properties of the exponential map

$$\begin{aligned} \exp((t+s)\tau^\wedge)&=\exp(t\tau^\wedge)\exp(s\tau^\wedge),\\ \exp(t\tau^\wedge)&=\exp(\tau^\wedge)^t,\\ \exp(-\tau^\wedge)&=\exp(\tau^\wedge)^{-1},\\ \exp(\mathcal X\tau^\wedge\mathcal X^{-1})&=\mathcal X\exp(\tau^\wedge)\mathcal X^{-1}. \end{aligned}$$

Two plus and minus operators

$$\begin{aligned} \text{right-}\oplus:\quad \mathcal Y&=\mathcal X\oplus{}^\mathcal X\tau =\mathcal X\operatorname{Exp}({}^\mathcal X\tau)\in\mathcal M,\\ \text{right-}\ominus:\quad {}^\mathcal X\tau&=\mathcal Y\ominus\mathcal X =\operatorname{Log}(\mathcal X^{-1}\mathcal Y)\in T_\mathcal X\mathcal M. \end{aligned}$$ $$\begin{aligned} \text{left-}\oplus:\quad \mathcal Y&={}^{\varepsilon}\tau\oplus\mathcal X =\operatorname{Exp}({}^{\varepsilon}\tau)\mathcal X\in\mathcal M,\\ \text{left-}\ominus:\quad {}^{\varepsilon}\tau&=\mathcal Y\ominus\mathcal X =\operatorname{Log}(\mathcal Y^{-1}\mathcal X)\in T_{\varepsilon}\mathcal M. \end{aligned}$$

The adjoint

$$\begin{aligned} \operatorname{Ad}_{\mathcal X}:\mathfrak m&\to\mathfrak m, &\tau^\wedge&\mapsto\mathcal X\tau^\wedge\mathcal X^{-1},\\ \text{Linear:}\quad \operatorname{Ad}_{\mathcal X}(a\tau^\wedge+b\sigma^\wedge) &=a\operatorname{Ad}_{\mathcal X}(\tau^\wedge) +b\operatorname{Ad}_{\mathcal X}(\sigma^\wedge),\\ \text{Homomorphism:}\quad \operatorname{Ad}_{\mathcal X}(\operatorname{Ad}_{\mathcal Y}(\tau^\wedge)) &=\operatorname{Ad}_{\mathcal X\mathcal Y}(\tau^\wedge). \end{aligned}$$

The adjoint matrix

$$\begin{aligned} \mathbf{Ad}_{\mathcal X}:\mathbb R^m&\to\mathbb R^m, &{}^\mathcal X\tau&\mapsto{}^\varepsilon\tau =\mathbf{Ad}_{\mathcal X}{}^\mathcal X\tau,\\ \mathbf{Ad}_{\mathcal X}\tau&=(\mathcal X\tau^\wedge\mathcal X^{-1})^\vee,\\ \mathcal X\oplus\tau&=(\mathbf{Ad}_{\mathcal X}\tau)\oplus\mathcal X,\\ \mathbf{Ad}_{\mathcal X^{-1}}&=(\mathbf{Ad}_{\mathcal X})^{-1},\\ \mathbf{Ad}_{\mathcal X}\mathbf{Ad}_{\mathcal Y} &=\mathbf{Ad}_{\mathcal X\mathcal Y}. \end{aligned}$$

Jacobian

$$\begin{aligned} \frac{{}^\mathcal XDf(\mathcal X)}{D\mathcal X} &=\lim_{\tau\to0}\frac{f(\mathcal X\oplus\tau)\ominus f(\mathcal X)}{\tau}\\ &=\lim_{\tau\to0}\frac{\operatorname{Log}(f(\mathcal X)^{-1}f(\mathcal X\operatorname{Exp}(\tau)))}{\tau}\\ &=\left.\frac{\partial\operatorname{Log}(f(\mathcal X)^{-1}f(\mathcal X\operatorname{Exp}(\tau)))}{\partial\tau}\right|_{\tau=0}. \end{aligned}$$

Relationship between different Jacobians:

$$\frac{{}^\epsilon Df(\mathcal X)}{D\mathcal X} =\mathbf{Ad}_{f(\mathcal X)} \frac{{}^\mathcal XDf(\mathcal X)}{D\mathcal X} \mathbf{Ad}_{\mathcal X}^{-1}.$$

Approximations:

$$\begin{aligned} \operatorname{Exp}(\tau+\delta\tau)&\approx\operatorname{Exp}(\tau)\operatorname{Exp}(J_r(\tau)\delta\tau),\\ \operatorname{Exp}(\tau)\operatorname{Exp}(\delta\tau)&\approx\operatorname{Exp}(\tau+J_r(\tau)^{-1}\delta\tau),\\ \operatorname{Log}(\operatorname{Exp}(\tau)\operatorname{Exp}(\delta\tau))&\approx\tau+J_r(\tau)^{-1}\delta\tau. \end{aligned}$$ $$\begin{aligned} \operatorname{Exp}(\tau+\delta\tau)&\approx\operatorname{Exp}(J_l(\tau)\delta\tau)\operatorname{Exp}(\tau),\\ \operatorname{Exp}(\delta\tau)\operatorname{Exp}(\tau)&\approx\operatorname{Exp}(\tau+J_l(\tau)^{-1}\delta\tau),\\ \operatorname{Log}(\operatorname{Exp}(\delta\tau)\operatorname{Exp}(\tau))&\approx\tau+J_l(\tau)^{-1}\delta\tau. \end{aligned}$$

Relationship between the left and right Jacobians:

$$\mathbf{Ad}_{\operatorname{Exp}(\tau)}=J_l(\tau)J_r(\tau)^{-1}, \qquad J_r(-\tau)=J_l(\tau).$$

Elementary Jacobian blocks:

$$\begin{aligned} \mathbf J_{\mathcal X}^{\mathcal X^{-1}}&=-\mathbf{Ad}_{\mathcal X},& \mathbf J_{\mathcal X}^{\mathcal X\circ\mathcal Y}&=\mathbf{Ad}_{\mathcal Y^{-1}},& \mathbf J_{\mathcal Y}^{\mathcal X\circ\mathcal Y}&=\mathbf I,\\ \mathbf J_{\mathcal X}^{\operatorname{Log}(\mathcal X)}&=\mathbf J_r^{-1}(\tau),& \mathbf J_{\mathcal X}^{\mathcal X\oplus\tau}&=\mathbf{Ad}_{\operatorname{Exp}(\tau)}^{-1},& \mathbf J_{\tau}^{\mathcal X\oplus\tau}&=\mathbf J_r,\\ \mathbf J_{\mathcal X}^{\mathcal Y\ominus\mathcal X}&=-\mathbf J_l(\tau)^{-1},& \mathbf J_{\mathcal Y}^{\mathcal Y\ominus\mathcal X}&=\mathbf J_r(\tau)^{-1}. \end{aligned}$$

Uncertainty on manifolds and covariance propagation

$$\begin{aligned} \Sigma_{\mathcal X}&=\mathbb E\left((\mathcal X\ominus\bar{\mathcal X})(\mathcal X\ominus\bar{\mathcal X})^\mathsf T\right)\in\mathbb R^{m\times m},\\ {}^\varepsilon\Sigma_{\mathcal X}&=\mathbf{Ad}_{\mathcal X}\,{}^\mathcal X\Sigma_{\mathcal X}\,\mathbf{Ad}_{\mathcal X}^\mathsf T,\\ \Sigma_{\mathcal Y}&\approx\frac{Df}{D\mathcal X}\Sigma_{\mathcal X}\left(\frac{Df}{D\mathcal X}\right)^\mathsf T. \end{aligned}$$

Discrete integration on manifolds

$$\mathcal X_k=\mathcal X_{k-1}\oplus\tau_k =\mathcal X_{k-1}\operatorname{Exp}(\tau_k).$$

The 3D rotation group $\mathrm{SO}(3)$

Exponential and logarithm

$$\mathbf R=\operatorname{Exp}(\theta\mathbf u) =\mathbf I+\sin\theta[\mathbf u]_\times+ (1-\cos\theta)[\mathbf u]_\times^2 \in\mathbb R^{3\times3}.$$ $$\theta\mathbf u=\operatorname{Log}(\mathbf R) =\frac{\theta(\mathbf R-\mathbf R^\mathsf T)^\vee}{2\sin\theta}, \qquad \theta=\cos^{-1}\left(\frac{\operatorname{tr}(\mathbf R)-1}{2}\right).$$

Adjoint

$$\mathbf{Ad}_{\mathbf R}=\mathbf R.$$

Elementary Jacobian blocks

$$\begin{aligned} \mathbf J_{\mathbf R}^{\mathbf R^{-1}}&=-\mathbf R,& \mathbf J_{\mathbf Q}^{\mathbf Q\mathbf R}&=\mathbf R^\mathsf T,& \mathbf J_{\mathbf R}^{\mathbf Q\mathbf R}&=\mathbf I,\\ \mathbf J_{\mathbf R}^{\mathbf R\mathbf v}&=-\mathbf R[\mathbf v]_\times,& \mathbf J_{\mathbf v}^{\mathbf R\mathbf v}&=\mathbf R,& \mathbf J_{\mathbf R}^{\mathbf R\oplus\theta}&=\mathbf R(\theta)^\mathsf T,\\ \mathbf J_{\theta}^{\mathbf R\oplus\theta}&=\mathbf J_r(\theta),& \mathbf J_{\mathbf Q}^{\mathbf Q\ominus\mathbf R}&=\mathbf J_r(\theta)^{-1},& \mathbf J_{\mathbf R}^{\mathbf Q\ominus\mathbf R}&=-\mathbf J_l(\theta)^{-1},\\ \mathbf J_l&=\mathbf J_r^\mathsf T,& \mathbf J_l^{-1}&=\mathbf J_r^{-\mathsf T}. \end{aligned}$$

In many cases, an $\mathrm{SE}(3)$ or $\mathrm{Sim}(3)$ Lie group is unnecessary: only rotation is matrix-valued, while translation and scale can be treated as vectors.