Shortcuts

pypose.mat2SE3

pypose.mat2SE3(mat, check=True, rtol=1e-05, atol=1e-05)[source]

Convert batched rotation or transformation matrices to SE3Type LieTensor.

Parameters
  • mat (Tensor) – the batched matrices to convert. If input is of shape (*, 3, 3), then translation will be filled with zero. For input with shape (*, 3, 4), the last row will be treated as [0, 0, 0, 1].

  • check (bool, optional) – flag to check if the input is valid rotation matrices (orthogonal and with a determinant of one). Set to False if less computation is needed. Default: True.

  • rtol (float, optional) – relative tolerance when check is enabled. Default: 1e-05

  • atol (float, optional) – absolute tolerance when check is enabled. Default: 1e-05

Returns

the converted SE3Type LieTensor.

Return type

LieTensor

Shape:

Input: (*, 3, 3) or (*, 3, 4) or (*, 4, 4)

Output: (*, 7)

Let the input be matrix \(\mathbf{T}\), \(\mathbf{T}_i\) represents each individual matrix in the batch. \(\mathbf{R}_i\in\mathbb{R}^{3\times 3}\) be the top left 3x3 block matrix of \(\mathbf{T}_i\). \(\mathbf{T}^{m,n}_i\) represents the \(m^{\mathrm{th}}\) row and \(n^{\mathrm{th}}\) column of \(\mathbf{T}_i\), \(m,n\geq 1\), then the translation and quaternion can be computed by:

\[\left\{\begin{aligned} t^x_i &= \mathbf{T}^{1,4}_i\\ t^y_i &= \mathbf{T}^{2,4}_i\\ t^z_i &= \mathbf{T}^{3,4}_i\\ q^x_i &= \mathrm{sign}(\mathbf{R}^{2,3}_i - \mathbf{R}^{3,2}_i) \frac{1}{2} \sqrt{1 + \mathbf{R}^{1,1}_i - \mathbf{R}^{2,2}_i - \mathbf{R}^{3,3}_i}\\ q^y_i &= \mathrm{sign}(\mathbf{R}^{3,1}_i - \mathbf{R}^{1,3}_i) \frac{1}{2} \sqrt{1 - \mathbf{R}^{1,1}_i + \mathbf{R}^{2,2}_i - \mathbf{R}^{3,3}_i}\\ q^z_i &= \mathrm{sign}(\mathbf{R}^{1,2}_i - \mathbf{R}^{2,1}_i) \frac{1}{2} \sqrt{1 - \mathbf{R}^{1,1}_i - \mathbf{R}^{2,2}_i + \mathbf{R}^{3,3}_i}\\ q^w_i &= \frac{1}{2} \sqrt{1 + \mathbf{R}^{1,1}_i + \mathbf{R}^{2,2}_i + \mathbf{R}^{3,3}_i} \end{aligned}\right., \]

In summary, the output LieTensor should be of format:

\[\textbf{y}_i = [t^x_i, t^y_i, t^z_i, q^x_i, q^y_i, q^z_i, q^w_i] \]

Warning

Numerically, a transformation matrix is considered legal if:

\[|{\rm det}(\mathbf{R}) - 1| \leq \texttt{atol} + \texttt{rtol}\times 1\\ |\mathbf{RR}^{T} - \mathbf{I}| \leq \texttt{atol} + \texttt{rtol}\times \mathbf{I} \]

where \(|\cdot |\) is element-wise absolute function. When check is set to True, illegal input will raise a ValueError. Otherwise, no data validation is performed. Illegal input will output an irrelevant result, which likely contains nan.

For input with shape (*, 4, 4), when check is set to True and the last row of the each individual matrix is not [0, 0, 0, 1], a warning will be triggered. Even though the last row is not used in the computation, it is worth noting that a matrix not satisfying this condition is not a valid transformation matrix.

Examples

>>> input = torch.tensor([[0., -1., 0., 0.1],
...                       [1.,  0., 0., 0.2],
...                       [0.,  0., 1., 0.3],
...                       [0.,  0., 0.,  1.]])
>>> pp.mat2SE3(input)
SE3Type LieTensor:
tensor([0.1000, 0.2000, 0.3000, 0.0000, 0.0000, 0.7071, 0.7071])

Note

The individual matrix in a batch can be written as:

\[\begin{bmatrix} \mathbf{R}_{3\times3} & \mathbf{t}_{3\times1}\\ \textbf{0} & 1 \end{bmatrix}, \]

where \(\mathbf{R}\) is the rotation matrix. The translation vector \(\mathbf{t}\) defines the displacement between the original position and the transformed position.

See pypose.SE3() for more details of the output LieTensor format.

Docs

Access documentation for PyPose

View Docs

Tutorials

Get started with tutorials and examples

View Tutorials

Get Started

Find resources and how to start using pypose

View Resources