Lie Groups and Lie Algebras

一些在SLAM中经常用到的关于李群李代数方面的知识

Special Orthogonal Group $SO(3)$ and $\mathfrak{so}(3)$

The group $SO(3)$ forms a smooth manifold, and its tangent space (at the identity) denoted as $\mathfrak{so}(3)$.

$$\begin{aligned} SO(3) &= \left\{\mathbf{R} \in \mathbb{R}^{3\times3} \mid \mathbf{R} \mathbf{R}^\intercal = \mathbf{I}, \det(\mathbf{R}) = 1 \right\} \\ \mathfrak{so}(3) &= \left\{ \boldsymbol{\phi}\in \mathbb{R}^3, \mathbf{\Phi} = \boldsymbol{\phi}^{\wedge{}} \in \mathbb{R}^{3 \times 3} \right\} \end{aligned}$$
where
$$\boldsymbol{\phi} ^\wedge = \begin{bmatrix} 0 & -\phi_3 & \phi_2 \\ \phi_3 & 0 & -\phi_1 \\ -\phi_2 & \phi_1 & 0 \end{bmatrix}$$

• $SO(3)$: Rotation Matrix $R$
• $\mathfrak{so}(3)$: Angle-Axis $\boldsymbol{\phi} = \theta \mathbf{a}$
• Connection: Rodrigues’s Formula

Exp Map

$\mathfrak{so}(3) \rightarrow SO(3)$

$$\mathbf{R} = \exp(\boldsymbol{\phi}^\wedge) = \exp(\theta \mathbf{a}^\wedge) = \cos\theta \;\mathbf{I} + (1 - \sin\theta) \mathbf{a} \mathbf{a}^\intercal + \sin\theta \;\mathbf{a} ^\wedge$$

Introduce $\mathrm{Exp}(\boldsymbol{\phi}) = \exp(\boldsymbol{\phi} ^\wedge)$, then the abode equation can written as:

$$\mathbf{R} = \mathrm{Exp}(\boldsymbol{\phi}) = \mathrm{Exp}(\theta \mathbf{a}) = \cos\theta \;\mathbf{I} + (1 - \sin\theta) \mathbf{a} \mathbf{a}^\intercal + \sin\theta \;\mathbf{a} ^\wedge$$

Log Map

$SO(3) \rightarrow \mathfrak{so}(3)$

$$\begin{aligned} \theta &= \arccos\left( \frac{\mathtt{tr}(\mathbf{R}) - 1}{2} \right) \\ \mathbf{R} \mathbf{a} &= \mathbf{a} \\ \boldsymbol{\phi} &= \theta \mathbf{a} \end{aligned}$$

BCH Formula

$$\mathrm{Exp}(\boldsymbol{\phi} + \Delta \boldsymbol{\phi} ) \approx \mathrm{Exp}(\boldsymbol{\phi}) \mathrm{Exp} \big(\mathbf{J}_r(\boldsymbol{\phi}) \Delta\boldsymbol{\phi} \big) = \mathrm{Exp} \big(\mathbf{J}_l(\boldsymbol{\phi}) \Delta\boldsymbol{\phi}\big) \mathrm{Exp}(\boldsymbol{\phi})$$

where

$$\mathbf{J}_l(\boldsymbol{\phi}) = \mathbf{J}_r(\mathbf{-\phi}) = \frac{\sin\theta}{\theta} \mathbf{I} + \left(1 - \frac{\sin\theta}{\theta} \right) \mathbf{a} \mathbf{a}^\intercal + \left(1 - \frac{\cos\theta}{\theta} \right) \mathbf{a}^\wedge$$

$$\begin{aligned} \mathrm{Log}\big(\mathrm{Exp}(\boldsymbol{\phi}) \mathrm{Exp}(\delta\boldsymbol{\phi}) \big) \approx& \boldsymbol{\phi} + \mathbf{J}_r^{-1}(\boldsymbol{\phi}) \delta\boldsymbol{\phi} \\ \mathrm{Log}\big(\mathrm{Exp}(\delta\boldsymbol{\phi}) \mathrm{Exp}(\boldsymbol{\phi}) \big) \approx& \mathbf{J}_l^{-1}(\boldsymbol{\phi}) \delta\boldsymbol{\phi} + \boldsymbol{\phi}\\ \end{aligned}$$

Property

$$\begin{aligned} \mathbf{R} \mathrm{Exp}(\boldsymbol{\phi}) \mathbf{R}^\intercal =& \exp(\mathbf{R} \boldsymbol{\phi}^\wedge \mathbf{R}^\intercal) = \mathrm{Exp}(\mathbf{R} \boldsymbol{\phi}) \\ \Leftrightarrow \quad \mathrm{Exp}(\boldsymbol{\phi}) \mathbf{R} =& \mathbf{R} \mathrm{Exp}(\mathbf{R}^\intercal \boldsymbol{\phi}) \end{aligned}$$

Special Euclidean Group $SE(3)$ and $\mathfrak{se}(3)$

$$\begin{aligned} SE(3) &= \left\{ \mathbf{T} = \begin{bmatrix} \mathbf{R} & \mathbf{p} \\ \mathbf{0}^\intercal & 1 \\ \end{bmatrix} \in \mathbb{R}^{4 \times 4} \mid \mathbf{R} \in SO(3), \mathbf{p} \in \mathbb{R}^3 \right\} \\ \mathfrak{se}(3) &= \left\{ \xi= \begin{bmatrix} \mathbf{\rho} \\ \boldsymbol{\phi} \end{bmatrix} \in \mathbb{R}^6, \mathbf{\rho} \in \mathbb{R}^3, \boldsymbol{\phi} \in \mathfrak{so}(3), \xi ^\wedge = \begin{bmatrix} \boldsymbol{\phi}^\wedge & \mathbf{\rho} \\ \mathbf{0}^\intercal & 0 \end{bmatrix} \in \mathbb{R}^{4 \times 4} \right\}\end{aligned}$$

• $SE(3)$: Pose (Rotation $\mathbf{R}$ + Translation $\mathbf{p}$), Matrix
• $\mathfrak{se}(3)$: Angle Axis $\boldsymbol{\phi}$ + Translation $\mathbf{\rho}$, Vector

Property

$$\mathbf{T} ^{-1} = \begin{bmatrix} \mathbf{R}^\intercal & -\mathbf{R}^\intercal \mathbf{p} \\ \mathbf{0}^\intercal & 1 \end{bmatrix}$$

Gaussian Random Variables

$$\begin{aligned} \widetilde{\mathbf{R}} &= \mathbf{R} \mathrm{Exp}(\Delta\boldsymbol{\phi}) \\ \widetilde{\mathbf{T}} &= \begin{bmatrix} \mathbf{R} \mathrm{Exp}(\Delta\boldsymbol{\phi}) & \mathbf{p} + \mathbf{R} \Delta\mathbf{p} \\ \mathbf{0}^\intercal & 1 \end{bmatrix} \end{aligned}$$

where $\mathbf{R}$ and $\mathbf{p}$ is the noise-free rotation and translation respectively, and $\Delta\boldsymbol{\phi}$ and $\Delta\mathbf{p}$ is the perturbations (Gaussian white noise)