Source code for ukfm.geometry.se2

import numpy as np
from ukfm.geometry.so2 import SO2


[docs]class SE2: """Homogeneous transformation matrix in :math:`SE(2)` .. math:: 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\\} """
[docs] @classmethod def Ad(cls, chi): """Adjoint matrix of the transformation. .. math:: \\text{Ad}(\\mathbf{T}) = \\begin{bmatrix} \\mathbf{C} & 1^\\wedge \\mathbf{r} \\\\ \\mathbf{0}^T & 1 \\end{bmatrix} \\in \\mathbb{R}^{3 \\times 3} """ Rot = chi[:2, :2] Jp = np.array([chi[1, 2], -chi[0, 2]]) Ad = np.vstack([np.hstack([Jp, Rot]), [1, 0, 0]]) return Ad
[docs] @classmethod def exp(cls, xi): """Exponential map for :math:`SE(2)`, which computes a transformation from a tangent vector: .. math:: \\mathbf{T}(\\boldsymbol{\\xi}) = \\exp(\\boldsymbol{\\xi}^\\wedge) = \\begin{bmatrix} \\exp(\\phi ^\\wedge) & \\mathbf{J} \\boldsymbol{\\rho} \\\\ \\mathbf{0} ^ T & 1 \\end{bmatrix} This is the inverse operation to :meth:`~ukfm.SE2.log`. """ chi = np.eye(3) chi[:2, :2] = SO2.exp(xi[0]) chi[:2, 2] = SO2.left_jacobian(xi[0]).dot(xi[1:3]) return chi
[docs] @classmethod def inv(cls, chi): """Inverse map for :math:`SE(2)`. .. math:: \\mathbf{T}^{-1} = \\begin{bmatrix} \\mathbf{C}^T & -\\mathbf{C}^T \\boldsymbol{\\rho} \\\\ \\mathbf{0} ^ T & 1 \\end{bmatrix} """ chi_inv = np.eye(3) chi_inv[:2, :2] = chi[:2, :2].T chi_inv[:2, 2] = -chi[:2, :2].T.dot(chi[:2, 2]) return chi_inv
[docs] @classmethod def log(cls, chi): """Logarithmic map for :math:`SE(2)`, which computes a tangent vector from a transformation: .. math:: \\boldsymbol{\\xi}(\\mathbf{T}) = \\ln(\\mathbf{T})^\\vee = \\begin{bmatrix} \\ln(\\boldsymbol{C}) ^\\vee \\\\ \\mathbf{J} ^ {-1} \\mathbf{r} \\end{bmatrix} This is the inverse operation to :meth:`~ukfm.SE2.log`. """ phi = SO2.log(chi[:2, :2]) xi = np.hstack([phi, SO2.inv_left_jacobian(phi).dot(chi[:2, 2])]) return xi