Lie Groups

Implementation of the more used matrix Lie groups using numpy. The implementation of \(SO(2)\), \(SE(2)\), \(SO(3)\), and \(SE(3)\) is based on the liegroups github repo. The implementation of \(SE_k(2)\) and \(SE_k(3)\) works for any \(k>0\).

\(SO(2)\)

class ukfm.SO2

Rotation matrix in \(SO(2)\).

\[\begin{split}SO(2) &= \left\{ \mathbf{C} \in \mathbb{R}^{2 \times 2} ~\middle|~ \mathbf{C}\mathbf{C}^T = \mathbf{1}, \det \mathbf{C} = 1 \right\} \\ \mathfrak{so}(2) &= \left\{ \boldsymbol{\Phi} = \phi^\wedge \in \mathbb{R}^{2 \times 2} ~\middle|~ \phi \in \mathbb{R} \right\}\end{split}\]

classmethod exp(phi)

Exponential map for \(SO(2)\), which computes a transformation from a tangent vector:

\[\begin{split}\mathbf{C}(\phi) = \exp(\phi^\wedge) = \cos \phi \mathbf{1} + \sin \phi 1^\wedge = \begin{bmatrix} \cos \phi & -\sin \phi \\ \sin \phi & \cos \phi \end{bmatrix}\end{split}\]

This is the inverse operation to log().

classmethod inv_left_jacobian(phi)

\(SO(2)\) inverse left Jacobian.

\[\begin{split}\mathbf{J}^{-1}(\phi) = \begin{cases} \mathbf{1} - \frac{1}{2} \phi^\wedge, & \text{if } \phi \text{ is small} \\ \frac{\phi}{2} \cot \frac{\phi}{2} \mathbf{1} - \frac{\phi}{2} 1^\wedge, & \text{otherwise} \end{cases}\end{split}\]

classmethod left_jacobian(phi)

\(SO(2)\) left Jacobian.

\[\begin{split}\mathbf{J}(\phi) = \begin{cases} \mathbf{1} + \frac{1}{2} \phi^\wedge, & \text{if } \phi \text{ is small} \\ \frac{\sin \phi}{\phi} \mathbf{1} - \frac{1 - \cos \phi}{\phi} 1^\wedge, & \text{otherwise} \end{cases}\end{split}\]

classmethod log(Rot)

Logarithmic map for \(SO(2)\), which computes a tangent vector from a transformation:

\[\phi(\mathbf{C}) = \ln(\mathbf{C})^\vee = \text{atan2}(C_{1,0}, C_{0,0})\]

This is the inverse operation to exp().

classmethod wedge(phi)

\(SO(2)\) wedge (skew-symmetric) operator.

\[\begin{split}\boldsymbol{\Phi} = \phi^\wedge = \begin{bmatrix} 0 & -\phi \\ \phi & 0 \end{bmatrix}\end{split}\]

\(SE(2)\)

class ukfm.SE2

Homogeneous transformation matrix in \(SE(2)\)

\[\begin{split}SE(2) &= \left\{ \mathbf{T}= \begin{bmatrix} \mathbf{C} & \mathbf{r} \\ \mathbf{0}^T & 1 \end{bmatrix} \in \mathbb{R}^{3 \times 3} ~\middle|~ \mathbf{C} \in SO(2), \mathbf{r} \in \mathbb{R}^2 \right\} \\ \mathfrak{se}(2) &= \left\{ \boldsymbol{\Xi} = \boldsymbol{\xi}^\wedge \in \mathbb{R}^{3 \times 3} ~\middle| ~ \boldsymbol{\xi}= \begin{bmatrix} \phi \\ \boldsymbol{\rho} \end{bmatrix} \in \mathbb{R}^3, \boldsymbol{\rho} \in \mathbb{R}^2, \phi \in \mathbb{R} \right\}\end{split}\]

classmethod Ad(chi)

Adjoint matrix of the transformation.

\[\begin{split}\text{Ad}(\mathbf{T}) = \begin{bmatrix} \mathbf{C} & 1^\wedge \mathbf{r} \\ \mathbf{0}^T & 1 \end{bmatrix} \in \mathbb{R}^{3 \times 3}\end{split}\]

classmethod exp(xi)

Exponential map for \(SE(2)\), which computes a transformation from a tangent vector:

\[\begin{split}\mathbf{T}(\boldsymbol{\xi}) = \exp(\boldsymbol{\xi}^\wedge) = \begin{bmatrix} \exp(\phi ^\wedge) & \mathbf{J} \boldsymbol{\rho} \\ \mathbf{0} ^ T & 1 \end{bmatrix}\end{split}\]

This is the inverse operation to log().

classmethod inv(chi)

Inverse map for \(SE(2)\).

\[\begin{split}\mathbf{T}^{-1} = \begin{bmatrix} \mathbf{C}^T & -\mathbf{C}^T \boldsymbol{\rho} \\ \mathbf{0} ^ T & 1 \end{bmatrix}\end{split}\]

classmethod log(chi)

Logarithmic map for \(SE(2)\), which computes a tangent vector from a transformation:

\[\begin{split}\boldsymbol{\xi}(\mathbf{T}) = \ln(\mathbf{T})^\vee = \begin{bmatrix} \ln(\boldsymbol{C}) ^\vee \\ \mathbf{J} ^ {-1} \mathbf{r} \end{bmatrix}\end{split}\]

This is the inverse operation to log().

\(SE_k(2)\)

class ukfm.SEK2

Homogeneous transformation matrix in \(SE_k(2)\)

\[\begin{split}SE_k(2) &= \left\{ \mathbf{T}= \begin{bmatrix} \mathbf{C} & \mathbf{r_1} & \cdots &\mathbf{r}_k \\ \mathbf{0}^T & &\mathbf{I}& \end{bmatrix} \in \mathbb{R}^{(2+k) \times (2+k)} ~\middle|~ \mathbf{C} \in SO(2), \mathbf{r}_1 \in \mathbb{R}^2, \cdots, \mathbf{r}_k \in \mathbb{R}^2 \right\} \\ \mathfrak{se}_k(2) &= \left\{ \boldsymbol{\Xi} = \boldsymbol{\xi}^\wedge \in \mathbb{R}^{(2+k) \times (2+k)} ~\middle|~ \boldsymbol{\xi}= \begin{bmatrix} \phi \\ \boldsymbol{\rho}_1 \\ \vdots \\ \boldsymbol{\rho}_k \end{bmatrix} \in \mathbb{R}^{1+2k}, \boldsymbol{\rho}_1 \in \mathbb{R}^2, \cdots, \boldsymbol{\rho}_k \in \mathbb{R}^2, \phi \in \mathbb{R} \right\}\end{split}\]

classmethod exp(xi)

Exponential map for \(SE_k(2)\), which computes a transformation from a tangent vector:

\[\begin{split}\mathbf{T}(\boldsymbol{\xi}) = \exp(\boldsymbol{\xi}^\wedge) = \begin{bmatrix} \exp(\phi ^\wedge) & \mathbf{J} \boldsymbol{\rho}_1 & \cdots && \mathbf{J} \boldsymbol{\rho}_k \\ \mathbf{0} ^ T & & \mathbf{I} & \end{bmatrix}\end{split}\]

This is the inverse operation to log().

classmethod inv(chi)

Inverse map for \(SE_k(2)\).

\[\begin{split}\mathbf{T}^{-1} = \begin{bmatrix} \mathbf{C}^T & -\mathbf{C}^T \boldsymbol{\rho}_1 & \cdots & & -\mathbf{C}^T \boldsymbol{\rho}_k \\ \mathbf{0} ^ T & & \mathbf{I} & \end{bmatrix}\end{split}\]

classmethod log(chi)

Logarithmic map for \(SE_k(2)\), which computes a tangent vector from a transformation:

\[\begin{split}\boldsymbol{\xi}(\mathbf{T}) = \ln(\mathbf{T})^\vee = \begin{bmatrix} \ln(\boldsymbol{C}) ^\vee \\ \mathbf{J} ^ {-1} \mathbf{r}_1 \\ \vdots \\ \mathbf{J} ^ {-1} \mathbf{r}_k \end{bmatrix}\end{split}\]

This is the inverse operation to log().

\(SO(3)\)

class ukfm.SO3

Rotation matrix in \(SO(3)\)

\[\begin{split}SO(3) &= \left\{ \mathbf{C} \in \mathbb{R}^{3 \times 3} ~\middle|~ \mathbf{C}\mathbf{C}^T = \mathbf{1}, \det \mathbf{C} = 1 \right\} \\ \mathfrak{so}(3) &= \left\{ \boldsymbol{\Phi} = \boldsymbol{\phi}^\wedge \in \mathbb{R}^{3 \times 3} ~\middle|~ \boldsymbol{\phi} = \phi \mathbf{a} \in \mathbb{R} ^3, \phi = \Vert \boldsymbol{\phi} \Vert \right\}\end{split}\]

classmethod Ad(Rot)

Adjoint matrix of the transformation.

\[\text{Ad}(\mathbf{C}) = \mathbf{C} \in \mathbb{R}^{3 \times 3}\]

classmethod exp(phi)

Exponential map for \(SO(3)\), which computes a transformation from a tangent vector:

\[\begin{split}\mathbf{C}(\boldsymbol{\phi}) = \exp(\boldsymbol{\phi}^\wedge) = \begin{cases} \mathbf{1} + \boldsymbol{\phi}^\wedge, & \text{if } \phi \text{ is small} \\ \cos \phi \mathbf{1} + (1 - \cos \phi) \mathbf{a}\mathbf{a}^T + \sin \phi \mathbf{a}^\wedge, & \text{otherwise} \end{cases}\end{split}\]

This is the inverse operation to log().

classmethod inv_left_jacobian(phi)

\(SO(3)\) inverse left Jacobian

\[\begin{split}\mathbf{J}^{-1}(\boldsymbol{\phi}) = \begin{cases} \mathbf{1} - \frac{1}{2} \boldsymbol{\phi}^\wedge, & \text{if } \phi \text{ is small} \\ \frac{\phi}{2} \cot \frac{\phi}{2} \mathbf{1} + \left( 1 - \frac{\phi}{2} \cot \frac{\phi}{2} \right) \mathbf{a}\mathbf{a}^T - \frac{\phi}{2} \mathbf{a}^\wedge, & \text{otherwise} \end{cases}\end{split}\]

classmethod left_jacobian(phi)

\(SO(3)\) left Jacobian.

\[\begin{split}\mathbf{J}(\boldsymbol{\phi}) = \begin{cases} \mathbf{1} + \frac{1}{2} \boldsymbol{\phi}^\wedge, & \text{if } \phi \text{ is small} \\ \frac{\sin \phi}{\phi} \mathbf{1} + \left(1 - \frac{\sin \phi}{\phi} \right) \mathbf{a}\mathbf{a}^T + \frac{1 - \cos \phi}{\phi} \mathbf{a}^\wedge, & \text{otherwise} \end{cases}\end{split}\]

classmethod log(Rot)

Logarithmic map for \(SO(3)\), which computes a tangent vector from a transformation:

\[\begin{split}\phi &= \frac{1}{2} \left( \mathrm{Tr}(\mathbf{C}) - 1 \right) \\ \boldsymbol{\phi}(\mathbf{C}) &= \ln(\mathbf{C})^\vee = \begin{cases} \mathbf{1} - \boldsymbol{\phi}^\wedge, & \text{if } \phi \text{ is small} \\ \left( \frac{1}{2} \frac{\phi}{\sin \phi} \left( \mathbf{C} - \mathbf{C}^T \right) \right)^\vee, & \text{otherwise} \end{cases}\end{split}\]

This is the inverse operation to log().

classmethod to_rpy(Rot)

Convert a rotation matrix to RPY Euler angles \((\alpha, \beta, \gamma)\).

classmethod vee(Phi)

\(SO(3)\) vee operator as defined by [BF14].

\[\phi = \boldsymbol{\Phi}^\vee\]

This is the inverse operation to wedge().

classmethod wedge(phi)

\(SO(3)\) wedge operator as defined by [BF14].

\[\begin{split}\boldsymbol{\Phi} = \boldsymbol{\phi}^\wedge = \begin{bmatrix} 0 & -\phi_3 & \phi_2 \\ \phi_3 & 0 & -\phi_1 \\ -\phi_2 & \phi_1 & 0 \end{bmatrix}\end{split}\]

This is the inverse operation to vee().

classmethod from_rpy(roll, pitch, yaw)

Form a rotation matrix from RPY Euler angles \((\alpha, \beta, \gamma)\).

\[\mathbf{C} = \mathbf{C}_z(\gamma) \mathbf{C}_y(\beta) \mathbf{C}_x(\alpha)\]

classmethod rotx(angle_in_radians)

Form a rotation matrix given an angle in rad about the x-axis.

\[\begin{split}\mathbf{C}_x(\phi) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos \phi & -\sin \phi \\ 0 & \sin \phi & \cos \phi \end{bmatrix}\end{split}\]

classmethod roty(angle_in_radians)

Form a rotation matrix given an angle in rad about the y-axis.

\[\begin{split}\mathbf{C}_y(\phi) = \begin{bmatrix} \cos \phi & 0 & \sin \phi \\ 0 & 1 & 0 \\ \sin \phi & 0 & \cos \phi \end{bmatrix}\end{split}\]

classmethod rotz(angle_in_radians)

Form a rotation matrix given an angle in rad about the z-axis.

\[\begin{split}\mathbf{C}_z(\phi) = \begin{bmatrix} \cos \phi & -\sin \phi & 0 \\ \sin \phi & \cos \phi & 0 \\ 0 & 0 & 1 \end{bmatrix}\end{split}\]

\(SE(3)\)

class ukfm.SE3

Homogeneous transformation matrix in \(SE(3)\).

\[\begin{split}SE(3) &= \left\{ \mathbf{T}= \begin{bmatrix} \mathbf{C} & \mathbf{r} \\ \mathbf{0}^T & 1 \end{bmatrix} \in \mathbb{R}^{4 \times 4} ~\middle|~ \mathbf{C} \in SO(3), \mathbf{r} \in \mathbb{R}^3 \right\} \\ \mathfrak{se}(3) &= \left\{ \boldsymbol{\Xi} = \boldsymbol{\xi}^\wedge \in \mathbb{R}^{4 \times 4} ~\middle|~ \boldsymbol{\xi}= \begin{bmatrix} \boldsymbol{\phi} \\ \boldsymbol{\rho} \end{bmatrix} \in \mathbb{R}^6, \boldsymbol{\rho} \in \mathbb{R}^3, \boldsymbol{\phi} \in \mathbb{R}^3 \right\}\end{split}\]

classmethod exp(xi)

Exponential map for \(SE(3)\), which computes a transformation from a tangent vector:

\[\begin{split}\mathbf{T}(\boldsymbol{\xi}) = \exp(\boldsymbol{\xi}^\wedge) = \begin{bmatrix} \exp(\boldsymbol{\phi}^\wedge) & \mathbf{J} \boldsymbol{\rho} \\ \mathbf{0} ^ T & 1 \end{bmatrix}\end{split}\]

This is the inverse operation to log().

classmethod inv(chi)

Inverse map for \(SE(3)\).

\[\begin{split}\mathbf{T}^{-1} = \begin{bmatrix} \mathbf{C}^T & -\mathbf{C}^T \boldsymbol{\rho} \\ \mathbf{0} ^ T & 1 \end{bmatrix}\end{split}\]

classmethod log(chi)

Logarithmic map for \(SE(3)\), which computes a tangent vector from a transformation:

\[\begin{split}\boldsymbol{\xi}(\mathbf{T}) = \ln(\mathbf{T})^\vee = \begin{bmatrix} \mathbf{J} ^ {-1} \mathbf{r} \\ \ln(\boldsymbol{C}) ^\vee \end{bmatrix}\end{split}\]

This is the inverse operation to exp().

\(SE_k(3)\)

class ukfm.SEK3

Homogeneous transformation matrix in \(SE_k(3)\).

\[\begin{split}SE_k(3) &= \left\{ \mathbf{T}= \begin{bmatrix} \mathbf{C} & \mathbf{r_1} & \cdots &\mathbf{r}_k \\ \mathbf{0}^T & & \mathbf{I} & \end{bmatrix} \in \mathbb{R}^{(3+k) \times (3+k)} ~\middle|~ \mathbf{C} \in SO(3), \mathbf{r}_1 \in \mathbb{R}^3, \cdots, \mathbf{r}_k \in \mathbb{R}^3 \right\} \\ \mathfrak{se}_k(3) &= \left\{ \boldsymbol{\Xi} = \boldsymbol{\xi}^\wedge \in \mathbb{R}^{(3+k) \times (3+k)} ~\middle|~ \boldsymbol{\xi}= \begin{bmatrix} \boldsymbol{\phi} \\ \boldsymbol{\rho}_1 \\ \vdots \\ \boldsymbol{\rho}_k \end{bmatrix} \in \mathbb{R}^{3+3k}, \boldsymbol{\phi} \in \mathbb{R}^3, \boldsymbol{\rho}_1 \in \mathbb{R}^3, \cdots, \boldsymbol{\rho}_k \in \mathbb{R}^3 \right\}\end{split}\]

classmethod exp(xi)

Exponential map for \(SE_k(3)\), which computes a transformation from a tangent vector:

\[\begin{split}\mathbf{T}(\boldsymbol{\xi}) = \exp(\boldsymbol{\xi}^\wedge) = \begin{bmatrix} \exp(\boldsymbol{\phi}^\wedge) & \mathbf{J} \boldsymbol{\rho}_1 & \cdots & \mathbf{J} \boldsymbol{\rho}_k \\ \mathbf{0} ^ T & & \mathbf{I} & \end{bmatrix}\end{split}\]

This is the inverse operation to log().

classmethod inv(chi)

Inverse map for \(SE_k(3)\).

\[\begin{split}\mathbf{T}^{-1} = \begin{bmatrix} \mathbf{C}^T & -\mathbf{C}^T \boldsymbol{\rho}_1 & \cdots & & -\mathbf{C}^T \boldsymbol{\rho}_k \\ \mathbf{0} ^ T & & \mathbf{I} & \end{bmatrix}\end{split}\]

classmethod log(chi)

Logarithmic map for \(SE_k(3)\), which computes a tangent vector from a transformation:

\[\begin{split}\boldsymbol{\xi}(\mathbf{T}) = \ln(\mathbf{T})^\vee = \begin{bmatrix} \ln(\boldsymbol{C}) ^\vee \\ \mathbf{J} ^ {-1} \mathbf{r}_1 \\ \vdots \\ \mathbf{J} ^ {-1} \mathbf{r}_k \end{bmatrix}\end{split}\]

This is the inverse operation to exp().