pypose.Jinvp¶

class pypose.Jinvp(input, p)[source]

The dot product between left Jacobian inverse at the point given by input (Lie Group) and second point (Lie Algebra).

$\mathrm{Jinvp}: (\mathcal{G}, \mathcal{g}) \mapsto \mathcal{g}$
Parameters
• input (LieTensor) – the input LieTensor (Lie Group)

• p (LieTensor) – the second LieTensor (Lie Algebra)

Returns

the output LieTensor (Lie Algebra)

Return type

LieTensor

List of supported $$\mathrm{Jinvp}$$ map

input ltype

$$(\mathcal{G}, \mathcal{g})$$ (Lie Group, Lie Algebra)

$$\mapsto$$

$$\mathcal{g}$$ (Lie Algebra)

output ltype

(SO3_type, so3_type)

$$(\mathcal{G}\in\mathbb{R}^{*\times4}, \mathcal{g}\in\mathbb{R}^{*\times3})$$

$$\mapsto$$

$$\mathcal{g}\in\mathbb{R}^{*\times3}$$

so3_type

(SE3_type, se3_type)

$$(\mathcal{G}\in\mathbb{R}^{*\times7}, \mathcal{g}\in\mathbb{R}^{*\times6})$$

$$\mapsto$$

$$\mathcal{g}\in\mathbb{R}^{*\times6}$$

se3_type

(Sim3_type, sim3_type)

$$(\mathcal{G}\in\mathbb{R}^{*\times8}, \mathcal{g}\in\mathbb{R}^{*\times7})$$

$$\mapsto$$

$$\mathcal{g}\in\mathbb{R}^{*\times7}$$

sim3_type

(RxSO3_type, rxso3_type)

$$(\mathcal{G}\in\mathbb{R}^{*\times5}, \mathcal{g}\in\mathbb{R}^{*\times4})$$

$$\mapsto$$

$$\mathcal{g}\in\mathbb{R}^{*\times4}$$

rxso3_type

Let the input be ($$\mathbf{x}$$, $$\mathbf{p}$$), $$\mathbf{y}$$ be the output.

$\mathbf{y}_i = \mathbf{J}^{-1}_i(\mathbf{x}_i)\mathbf{p}_i,$

where $$\mathbf{J}^{-1}_i(\mathbf{x}_i)$$ is the inverse of left Jacobian of $$\mathbf{x}_i$$.

• If input ($$\mathbf{x}$$, $$\mathbf{p}$$)’s ltype are SO3_type and so3_type (input $$\mathbf{x}$$ is an instance of SO3(), $$\mathbf{p}$$ is an instance of so3()). Let $$\boldsymbol{\phi}_i = \theta_i\mathbf{n}_i$$ be the corresponding Lie Algebra of $$\mathbf{x}_i$$, $$\boldsymbol{\Phi}_i$$ be the skew matrix (pypose.vec2skew()) of $$\boldsymbol{\phi}_i$$:

$\mathbf{J}^{-1}_i(\mathbf{x}_i) = \mathbf{I} - \frac{1}{2}\boldsymbol{\Phi}_i + \mathrm{coef}\boldsymbol{\Phi}_i^2$

where $$\mathbf{I}$$ is the identity matrix with the same dimension as $$\boldsymbol{\Phi}_i$$, and

$\mathrm{coef} = \left\{ \begin{array}{ll} \frac{1}{\theta_i^2} - \frac{\cos{\frac{\theta_i}{2}}}{2\theta\sin{\frac{\theta_i}{2}}}, \quad \|\theta_i\| > \text{eps}, \\ \frac{1}{12}, \quad \|\theta_i\| \leq \text{eps} \end{array} \right.$
• If input ($$\mathbf{x}$$, $$\mathbf{p}$$)’s ltype are SE3_type and se3_type (input $$\mathbf{x}$$ is an instance of SE3(), $$\mathbf{p}$$ is an instance of se3()). Let $$\boldsymbol{\phi}_i = \theta_i\mathbf{n}_i$$ be the corresponding Lie Algebra of the SO3 part of $$\mathbf{x}_i$$, $$\boldsymbol{\tau}_i$$ be the Lie Algebra of the translation part of $$\mathbf{x}_i$$; $$\boldsymbol{\Phi}_i$$ and $$\boldsymbol{\Tau}_i$$ be the skew matrices, respectively:

$\mathbf{J}^{-1}_i(\mathbf{x}_i) = \left[ \begin{array}{cc} \mathbf{J}_i^{-1}(\boldsymbol{\Phi}_i) & - \mathbf{J}_i^{-1}(\boldsymbol{\Phi}_i) \mathbf{Q}_i(\boldsymbol{\tau}_i, \boldsymbol{\phi}_i) \mathbf{J}_i^{-1}(\boldsymbol{\Phi}_i) \\ \mathbf{0} & \mathbf{J}_i^{-1}(\boldsymbol{\Phi}_i) \end{array} \right]$

where $$\mathbf{J}_i^{-1}(\boldsymbol{\Phi}_i)$$ is the inverse of left Jacobian of the SO3 part of $$\mathbf{x}_i$$. $$\mathbf{Q}_i(\boldsymbol{\tau}_i, \boldsymbol{\phi}_i)$$ is

\begin{align*} \mathbf{Q}_i(\boldsymbol{\tau}_i, \boldsymbol{\phi}_i) = \frac{1}{2}\boldsymbol{\Tau}_i &+ c_1 (\boldsymbol{\Phi_i\Tau_i} + \boldsymbol{\Tau_i\Phi_i} + \boldsymbol{\Phi_i\Tau_i\Phi_i}) \\ &+ c_2 (\boldsymbol{\Phi_i^2\Tau_i} + \boldsymbol{\Tau_i\Phi_i^2} - 3\boldsymbol{\Phi_i\Tau_i\Phi_i})\\ &+ c_3 (\boldsymbol{\Phi_i\Tau_i\Phi_i^2} + \boldsymbol{\Phi_i^2\Tau_i\Phi_i}) \end{align*}

where,

$c_1 = \left\{ \begin{array}{ll} \frac{\theta_i - \sin\theta_i}{\theta_i^3}, \quad \|\theta_i\| > \text{eps}, \\ \frac{1}{6}-\frac{1}{120}\theta_i^2, \quad \|\theta_i\| \leq \text{eps} \end{array} \right.$
$c_2 = \left\{ \begin{array}{ll} \frac{\theta_i^2 +2\cos\theta_i - 2}{2\theta_i^4}, \quad \|\theta_i\| > \text{eps}, \\ \frac{1}{24}-\frac{1}{720}\theta_i^2, \quad \|\theta_i\| \leq \text{eps} \end{array} \right.$
$c_3 = \left\{ \begin{array}{ll} \frac{2\theta_i - 3\sin\theta_i + \theta_i\cos\theta_i}{2\theta_i^5}, \quad \|\theta_i\| > \text{eps}, \\ \frac{1}{120}-\frac{1}{2520}\theta_i^2, \quad \|\theta_i\| \leq \text{eps} \end{array} \right.$
• If input ($$\mathbf{x}$$, $$\mathbf{p}$$)’s ltype are Sim3_type and sim3_type (input $$\mathbf{x}$$ is an instance of Sim3(), $$\mathbf{p}$$ is an instance of sim3()). The inverse of left Jacobian can be approximated as:

$\mathbf{J}^{-1}_i(\mathbf{x}_i) = \sum_{n=0}(-1)^n\frac{B_n}{n!}(\boldsymbol{\xi}_i^{\curlywedge})^n$

where $$B_n$$ is the Bernoulli number: $$B_0 = 1$$, $$B_1 = -\frac{1}{2}$$, $$B_2 = \frac{1}{6}$$, $$B_3 = 0$$, $$B_4 = -\frac{1}{30}$$. $$\boldsymbol{\xi}_i^{\curlywedge} = \mathrm{adj}(\boldsymbol{\xi}_i^{\wedge})$$ and $$\mathrm{adj}$$ is the adjoint of the Lie algebra $$\mathfrak{sim}(3)$$, e.g., $$\boldsymbol{\xi}_i \in \mathfrak{sim}(3)$$. Notice that if notate $$\boldsymbol{X}_i = \mathrm{Adj}(\mathbf{x}_i)$$ and $$\mathrm{Adj}$$ is the adjoint of the Lie group $$\mathrm{Sim}(3)$$, there is a nice property: $$\mathrm{Adj}(\mathrm{Exp}(\boldsymbol{\xi}_i^{\curlywedge})) = \mathrm{Exp}(\mathrm{adj}(\boldsymbol{\xi}_i^{\wedge}))$$, or $$\boldsymbol{X}_i = \mathrm{Exp}(\boldsymbol{\xi}_i^{\curlywedge})$$.

• If input ($$\mathbf{x}$$, $$\mathbf{p}$$)’s ltype are RxSO3_type and rxso3_type (input $$\mathbf{x}$$ is an instance of RxSO3(), $$\mathbf{p}$$ is an instance of rxso3()). Let $$\boldsymbol{\phi}_i$$ be the corresponding Lie Algebra of the SO3 part of $$\mathbf{x}_i$$, $$\boldsymbol{\Phi}_i$$ be the skew matrix (pypose.vec2skew()), The inverse of left Jacobian of $$\mathbf{x}_i$$ is the same as that for the SO3 part of $$\mathbf{x}_i$$.

$\mathbf{J}^{-1}_i(\mathbf{x}_i) = \left[ \begin{array}{cc} \mathbf{J}_i^{-1}(\boldsymbol{\Phi}_i) & \mathbf{0} \\ \mathbf{0} & 1 \end{array} \right]$

where $$\mathbf{J}_i^{-1}(\boldsymbol{\Phi}_i)$$ is the inverse of left Jacobian of the SO3 part of $$\mathbf{x}_i$$.

Note

$$\mathrm{Jinvp}$$ is usually used in the Baker-Campbell-Hausdorff formula (BCH formula) when performing LieTensor multiplication. One can refer to this paper for more details:

In particular, Eq. (146) is the math used in the SO3_type, so3_type scenario; Eq. (179b) and Eq. (180) are the math used in the SE3_type, se3_type scenario.

For Lie groups such as Sim3_type, sim3_type, there is no analytic expression for the left Jacobian and its inverse. Numerical approximation is used based on series expansion. One can refer to Eq. (26) of this paper for more details about the approximation:

In particular, the Bernoulli numbers can be obtained from Eq. (7.72) of this famous book:

Example

>>> x = pp.randn_SO3()
>>> p = pp.randn_so3()
>>> x.Jinvp(p) # equivalent to: pp.Jinvp(x, p)
so3Type LieTensor:
tensor([-2.0248,  1.1116, -0.0251])

>>> x = pp.randn_SE3(2)
>>> p = pp.randn_se3(2)
>>> x.Jinvp(p) # equivalent to: pp.Jinvp(x, p)
se3Type LieTensor:
tensor([[ 0.4304,  2.0565,  1.0256,  0.0666, -0.2252, -0.7425],
[-0.9317, -1.7806,  0.8660, -2.0028,  0.6098, -0.6517]])

>>> x = pp.randn_Sim3(2)
>>> p = pp.randn_sim3(2)
>>> x.Jinvp(p) # equivalent to: pp.Jinvp(x, p)
sim3Type LieTensor:
tensor([[-1.7231, -1.6650, -0.0202, -0.3731,  0.8441, -0.5438,  0.2879],
[ 0.9965,  0.6337, -0.7320, -0.1874,  0.6312,  0.3919,  0.6938]])

>>> x = pp.randn_RxSO3(2)
>>> p = pp.randn_rxso3(2)
>>> x.Jinvp(p) # equivalent to: pp.Jinvp(x, p)
rxso3Type LieTensor:
tensor([[ 0.9308, -1.4965, -0.1347,  0.4894],
[ 0.6558,  1.2221, -0.8190,  0.2108]])