pypose.Log¶
- class pypose.Log(input)[source]¶
The Logarithm map for
LieTensor
(Lie Group).\[\mathrm{Log}: \mathcal{G} \mapsto \mathcal{g} \]- Parameters
input (LieTensor) – the input LieTensor (Lie Group)
- Returns
the output LieTensor (Lie Algebra)
- Return type
List of supported \(\mathrm{Log}\) map¶ input
ltype
\(\mathcal{G}\) (Lie Group)
\(\mapsto\)
\(\mathcal{g}\) (Lie Algebra)
output
ltype
SO3_type
\(\mathcal{G}\in\mathbb{R}^{*\times4}\)
\(\mapsto\)
\(\mathcal{g}\in\mathbb{R}^{*\times3}\)
so3_type
SE3_type
\(\mathcal{G}\in\mathbb{R}^{*\times7}\)
\(\mapsto\)
\(\mathcal{g}\in\mathbb{R}^{*\times6}\)
se3_type
Sim3_type
\(\mathcal{G}\in\mathbb{R}^{*\times8}\)
\(\mapsto\)
\(\mathcal{g}\in\mathbb{R}^{*\times7}\)
sim3_type
RxSO3_type
\(\mathcal{G}\in\mathbb{R}^{*\times5}\)
\(\mapsto\)
\(\mathcal{g}\in\mathbb{R}^{*\times4}\)
rxso3_type
Warning
This function
Log()
is different fromlog()
, which returns a new torch tensor with the logarithm of the elements of the input tensor.If input \(\mathbf{x}\)’s
ltype
isSO3_type
(input \(\mathbf{x}\) is an instance ofSO3()
):Let \(w_i\), \(\boldsymbol{\nu}_i\) be the scalar and vector parts of \(\mathbf{x}_i\), respectively; \(\mathbf{y}\) be the output.
If \(\|\boldsymbol{\nu}_i\| > \text{eps}\):
\[\mathbf{y}_i = \left\{ \begin{array}{ll} 2\frac{\mathrm{arctan}(\|\boldsymbol{\nu}_i\|/w_i)}{\| \boldsymbol{\nu}_i\|}\boldsymbol{\nu}_i, \quad \|w_i\| > \text{eps}, \\ \mathrm{sign}(w_i) \frac{\pi}{\|\boldsymbol{\nu}_i\|} \boldsymbol{\nu}_i, \quad \|w_i\| \leq \text{eps}, \end{array} \right. \]otherwise:
\[\mathbf{y}_i = 2\left( \frac{1}{w_i} - \frac{\|\boldsymbol{\nu}_i\|^2}{3w_i^3}\right)\boldsymbol{\nu}_i. \]If input \(\mathbf{x}\)’s
ltype
isSE3_type
(input \(\mathbf{x}\) is an instance ofSE3()
):Let \(\mathbf{t}_i\), \(\mathbf{q}_i\) be the translation and rotation parts of \(\mathbf{x}_i\), respectively; \(\mathbf{y}\) be the output.
\[\mathbf{y}_i = \left[\mathbf{J}_i^{-1}\mathbf{t}_i, \mathrm{Log}(\mathbf{q}_i) \right], \]where \(\mathrm{Log}\) is the Logarithm map for
SO3_type
input and \(\mathbf{J}_i\) is the left Jacobian forSO3_type
input.If input \(\mathbf{x}\)’s
ltype
isRxSO3_type
(input \(\mathbf{x}\) is an instance ofRxSO3()
):Let \(\mathbf{q}_i\), \(s_i\) be the rotation and scale parts of \(\mathbf{x}_i\), respectively; \(\mathbf{y}\) be the output.
\[\mathbf{y}_i = \left[\mathrm{Log}(\mathbf{q}_i), \log(s_i) \right]. \]If input \(\mathbf{x}\)’s
ltype
isSim3_type
(input \(\mathbf{x}\) is an instance ofSim3()
):Let \(\mathbf{t}_i\), \(^s\mathbf{q}_i\) be the translation and
RxSO3
parts of \(\mathbf{x}_i\), respectively; \(\boldsymbol{\phi}_i\), \(\sigma_i\) be the corresponding Lie Algebra of the SO3 and scale part of \(^s\mathbf{q}_i\), \(\boldsymbol{\Phi}_i\) be the skew matrix of \(\boldsymbol{\phi}_i\), \(\boldsymbol{\phi}_i\) can be represented as \(\theta_i\mathbf{n}_i\), \(s_i = e^\sigma_i\), \(\mathbf{y}\) be the output.\[\mathbf{y}_i = \left[^s\mathbf{W}_i^{-1}\mathbf{t}_i, \mathrm{Log}(^s\mathbf{q}_i) \right], \]where
\[^s\mathbf{W}_i = A\boldsymbol{\Phi}_i + B\boldsymbol{\Phi}_i^2 + C\mathbf{I} \]in which if \(\|\sigma_i\| > \text{eps}\):
\[A = \left\{ \begin{array}{ll} \frac{s_i\sin\theta_i\sigma_i + (1-s_i\cos\theta_i)\theta_i} {\theta_i(\sigma_i^2 + \theta_i^2)}, \quad \|\theta_i\| > \text{eps}, \\ \frac{(\sigma_i-1)s_i+1}{\sigma_i^2}, \quad \|\theta_i\| \leq \text{eps}, \end{array} \right. \]\[B = \left\{ \begin{array}{ll} \left( C - \frac{(s_i\cos\theta_i-1)\sigma+ s_i\sin\theta_i\sigma_i} {\theta_i^2+\sigma_i^2}\right)\frac{1}{\theta_i^2}, \quad \|\theta_i\| > \text{eps}, \\ \frac{s_i\sigma_i^2/2 + s_i-1-\sigma_i s_i}{\sigma_i^3}, \quad \|\theta_i\| \leq \text{eps}, \end{array} \right. \]\[C = \frac{e^{\sigma_i} - 1}{\sigma_i}\mathbf{I} \]otherwise:
\[A = \left\{ \begin{array}{ll} \frac{1-\cos\theta_i}{\theta_i^2}, \quad \|\theta_i\| > \text{eps}, \\ \frac{1}{2}, \quad \|\theta_i\| \leq \text{eps}, \end{array} \right. \]\[B = \left\{ \begin{array}{ll} \frac{\theta_i - \sin\theta_i}{\theta_i^3}, \quad \|\theta_i\| > \text{eps}, \\ \frac{1}{6}, \quad \|\theta_i\| \leq \text{eps}, \end{array} \right. \]\[C = 1 \]
Note
The \(\mathrm{arctan}\)-based Logarithm map implementation thanks to the paper:
C. Hertzberg et al., Integrating Generic Sensor Fusion Algorithms with Sound State Representation through Encapsulation of Manifolds, Information Fusion, 2013.
Assume we have a unit rotation axis \(\mathbf{n}\) and rotation angle \(\theta~(0\leq\theta<2\pi)\), then the corresponding quaternion with unit norm \(\mathbf{q}\) can be represented as
\[\mathbf{q} = \left[\sin(\theta/2) \mathbf{n}, \cos(\theta/2) \right] \]Therefore, given a quaternion \(\mathbf{q}=[\boldsymbol{\nu}, w]\), where \(\boldsymbol{\nu}\) is the vector part, \(w\) is the scalar part, to find the corresponding rotation vector, the rotation angle \(\theta\) can be obtained as
\[\theta = 2\mathrm{arctan}(\|\boldsymbol{\nu}\|/w),~\|\boldsymbol{\nu}\| = \sin(\theta/2), \]The unit rotation axis \(\mathbf{n}\) can be obtained as \(\mathbf{n} = \frac{\boldsymbol{\nu}}{{\|\boldsymbol{\nu}\|}}\). Hence, the corresponding rotation vector is
\[\theta \mathbf{n} = 2\frac{\mathrm{arctan} (\|\boldsymbol{\nu}\|/w)}{\|\boldsymbol{\nu}\|}\boldsymbol{\nu}. \]More details about \(^s\mathbf{W}_i\) in
Sim3_type
can be found in Eq. (5.7):H. Strasdat, Local accuracy and global consistency for efficient visual SLAM, Dissertation. Department of Computing, Imperial College London, 2012.
Example
\(\mathrm{Log}\):
SO3
\(\mapsto\)so3
>>> x = pp.randn_SO3() >>> x.Log() # equivalent to: pp.Log(x) so3Type LieTensor: tensor([-0.3060, 0.2344, 1.2724])
\(\mathrm{Log}\):
SE3
\(\mapsto\)se3
>>> x = pp.randn_SE3(2) >>> x.Log() # equivalent to: pp.Log(x) se3Type LieTensor: tensor([[ 0.2958, -0.0840, -1.4733, 0.7004, 0.4483, -0.9009], [ 0.0850, -0.1020, -1.2616, -1.0524, -1.2031, 0.8377]])
\(\mathrm{Log}\):
RxSO3
\(\mapsto\)rxso3
>>> x = pp.randn_RxSO3(2) >>> x.Log() # equivalent to: pp.Log(x) rxso3Type LieTensor: tensor([[-1.3755, 0.3525, -2.2367, 0.5409], [ 0.5929, -0.3250, -0.7394, 1.0965]])
\(\mathrm{Log}\):
Sim3
\(\mapsto\)sim3
>>> x = pp.randn_Sim3(2) >>> x.Log() # equivalent to: pp.Log(x) sim3Type LieTensor: tensor([[-0.1747, -0.3698, 0.2000, 0.1735, 0.6220, 1.1852, -0.6402], [-0.8685, -0.1717, 1.2139, -0.8385, -2.2957, -1.9545, 0.8474]])