pypose.Adj¶
- class pypose.Adj(input, p)[source]¶
The dot product between the Adjoint matrix at the point given by an input (Lie Group) and the second point (Lie Algebra).
\[\mathrm{Adj}: (\mathcal{G}, \mathcal{g}) \mapsto \mathcal{g} \]- Parameters
- Returns
the output LieTensor (Lie Algebra)
- Return type
List of supported \(\mathrm{Adj}\) 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 = \mathrm{Adj}(\mathbf{x}_i)\mathbf{p}_i, \]where, \(\mathrm{Adj}(\mathbf{x}_i)\) is the adjoint matrix of the Lie group of \(\mathbf{x}_i\).
If input (\(\mathbf{x}\), \(\mathbf{p}\))’s
ltype
areSO3_type
andso3_type
(input \(\mathbf{x}\) is an instance ofSO3()
, \(\mathbf{p}\) is an instance ofso3()
). Given \(\mathbf{x}_i \in\) \(\textrm{SO(3)}\). The adjoint transformation is given by:\[\mathrm{Adj}(\mathbf{x}_i) = \mathbf{x}_i \]In the case of \(\textrm{SO3}\), the adjoint transformation for an element is the same rotation matrix used to represent the element. Rotating a tangent vector by an element “moves” it from the tangent space on the right side of the element to the tangent space on the left.
If input (\(\mathbf{x}\), \(\mathbf{p}\))’s
ltype
areSE3_type
andse3_type
(input \(\mathbf{x}\) is an instance ofSE3()
, \(\mathbf{p}\) is an instance ofse3()
). Let \(\mathbf{R}_i \in\) \(\textrm{SO(3)}\) and \(\mathbf{t}_i \in \mathbb{R}^{3\times3}\) represent the rotation and translation part of the group. Let \(\mathbf{t}_{i\times}\) be the skew matrix (pypose.vec2skew()
) of \(\mathbf{t}_i\). The adjoint transformation is given by:\[\mathrm{Adj}(\mathbf{x}_i) = \left[ \begin{array}{cc} \mathbf{R}_i & \mathbf{t}_{i\times}\mathbf{R}_i \\ 0 & \mathbf{R}_i \end{array} \right] \in \mathbb{R}^{6\times6} \]where,
\[\mathbf{x}_i = \left[ \begin{array}{cc} \mathbf{R}_i& \mathbf{t}_i \\ 0 & 1 \end{array} \right] \in \mathrm{SE(3)} \]If input (\(\mathbf{x}\), \(\mathbf{p}\))’s
ltype
areSim3_type
andsim3_type
(input \(\mathbf{x}\) is an instance ofSim3()
, \(\mathbf{p}\) is an instance ofsim3()
). Let \(\mathbf{R}_i\in\) \(\textrm{SO(3)}\), \(\mathbf{t}_i \in \mathbb{R}^{3\times3}\), and \(s_i \in \mathbb{R}^+\) represent the rotation, translation, and scale parts of the group. Let \(\mathbf{t}_{i\times}\) be the skew matrix (pypose.vec2skew()
) of \(\mathbf{t}_i\). The adjoint transformation is given by:\[\mathrm{Adj}(\mathbf{x}_i) = \left[ \begin{array}{cc} s_i\mathbf{R}_i& \mathbf{t}_{i\times}\mathbf{R}_i& -\mathbf{t}_i \\ 0 & \mathbf{R}_i& 0 \\ 0 & 0 & 1 \end{array} \right] \in \mathbb{R}^{7\times7} \]where,
\[\mathbf{x}_i = \left[ \begin{array}{cc} s_i\mathbf{R}_i & \mathbf{t}_i \\ 0 & 1 \end{array} \right] \in \textrm{Sim(3)} \]If input (\(\mathbf{x}\), \(\mathbf{p}\))’s
ltype
areRxSO3_type
andrxso3_type
(input \(\mathbf{x}\) is an instance ofRxSO3()
, \(\mathbf{p}\) is an instance ofrxso3()
). Let \(\mathbf{R}_i \in\) \(\textrm{SO(3)}\), and \(s_i \in \mathbb{R}^+\) represent the rotation and scale parts of the group. The adjoint transformation is given by:\[\mathrm{Adj}(\mathbf{x}_i) = \left[ \begin{array}{cc} \mathbf{R}_i & 0 \\ 0 & 1 \end{array} \right] \in \mathbb{R}^{4\times4} \]where,
\[\mathbf{x}_i = \left[ \begin{array}{cc} s_i\mathbf{R}_i & 0 \\ 0 & 1 \end{array} \right] \in \mathrm{RxSO(3)} \]In the case of \(\textrm{RxSO3}\) group, the adjoint transformation is the same as the rotation matrix of the group i.e. the \(\textrm{SO3}\) part of the group.
Note
The adjoint operator is a linear map which moves an element \(\mathbf{p} \in \mathcal{g}\) in the right tangent space of \(\mathbf{x} \in \mathcal{G}\) to the left tangent space.
\[\mathrm{Exp}(\mathrm{Adj}(\mathbf{x}) \mathbf{p}) * \mathbf{x} = \mathbf{x} * \mathrm{Exp}(\mathbf{p}) \]It can be easily verified:
>>> x, p = pp.randn_SO3(), pp.randn_so3() >>> torch.allclose(x*p.Exp(), x.Adj(p).Exp()*x) True
One can refer to Eq. (8) of the following paper:
Zachary Teed et al., Tangent Space Backpropagation for 3D Transformation Groups, IEEE/CVF Conference on Computer Vision and Pattern Recognition (CVPR), 2021.
Note
\(\mathrm{Adj}\) is generally used to transform a tangent vector from the tangent space around one element to the tangent space of another. 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).
The following thesis and the tutorial serve as a good reading material to learn more about deriving the adjoint matrices for different transformation groups.
Strasdat, H., 2012. Local accuracy and global consistency for efficient visual SLAM, (Doctoral dissertation, Department of Computing, Imperial College London).
Lie Groups for 2D and 3D Transformations., by Ethan Eade.
Example
\(\mathrm{Adj}\): (
SO3
,so3
) \(\mapsto\)so3
>>> x = pp.randn_SO3(2) >>> p = pp.randn_so3(2) >>> x.Adj(p) # equivalent to: pp.Adj(x, p) so3Type LieTensor: tensor([[-0.4171, 2.1218, 0.9951], [ 1.8415, -1.2185, -0.4082]])
\(\mathrm{Adj}\): (
SE3
,se3
) \(\mapsto\)se3
>>> x = pp.randn_SE3(2) >>> p = pp.randn_se3(2) >>> x.Adj(p) # equivalent to: pp.Adj(x, p) se3Type LieTensor: tensor([[-0.8536, -0.1984, -0.4554, -0.4868, 0.3231, 0.8535], [ 0.1577, -1.7625, 1.7997, -1.5085, -0.2098, 0.3538]])
\(\mathrm{Adj}\): (
Sim3
,sim3
) \(\mapsto\)sim3
>>> x = pp.randn_Sim3(2) >>> p = pp.randn_sim3(2) >>> x.Adj(p) # equivalent to: pp.Adj(x, p) sim3Type LieTensor: tensor([[ 0.1455, -0.5653, -0.1845, 0.0502, 1.3125, 1.5217, -0.8964], [-4.8724, -0.5254, 3.9559, 1.5170, 1.7610, 0.4375, 0.4248]])
\(\mathrm{Adj}\): (
RxSO3
,rxso3
) \(\mapsto\)rxso3
>>> x = pp.randn_RxSO3(2) >>> p = pp.randn_rxso3(2) >>> x.Adj(p) # equivalent to: pp.Adj(x, p) rxso3Type LieTensor: tensor([[-1.3590, -0.4314, -0.0297, 1.0166], [-0.3378, -0.4942, -2.0083, -0.4321]])