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
- Returns
the output LieTensor (Lie Algebra)
- Return type
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_typeLet 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
ltypeareSO3_typeandso3_type(input \(\mathbf{x}\) is an instance ofSO3(), \(\mathbf{p}\) is an instance ofso3()). 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
ltypeareSE3_typeandse3_type(input \(\mathbf{x}\) is an instance ofSE3(), \(\mathbf{p}\) is an instance ofse3()). 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
ltypeareSim3_typeandsim3_type(input \(\mathbf{x}\) is an instance ofSim3(), \(\mathbf{p}\) is an instance ofsim3()). 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
ltypeareRxSO3_typeandrxso3_type(input \(\mathbf{x}\) is an instance ofRxSO3(), \(\mathbf{p}\) is an instance ofrxso3()). 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:
J. Sola et al., A micro Lie theory for state estimation in robotics, arXiv preprint arXiv:1812.01537 (2018).
In particular, Eq. (146) is the math used in the
SO3_type,so3_typescenario; Eq. (179b) and Eq. (180) are the math used in theSE3_type,se3_typescenario.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:Z. Teed et al., Tangent Space Backpropagation for 3D Transformation Groups., in IEEE/CVF Conference on Computer Vision and Pattern Recognition (CVPR) (2021).
In particular, the Bernoulli numbers can be obtained from Eq. (7.72) of this famous book:
T. Barfoot, State Estimation for Robotics., Cambridge University Press (2017).
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]])
