Shortcuts

# pypose.mat2Sim3¶

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

Convert batched rotation or transformation matrices to Sim3Type 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 Sim3Type LieTensor.

Return type

LieTensor

Shape:

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

Output: (*, 8)

Let the input be matrix $$\mathbf{T}$$, $$\mathbf{T}_i$$ represents each individual matrix in the batch. $$\mathbf{U}_i\in\mathbb{R}^{3\times 3}$$ be the top left 3x3 block matrix of $$\mathbf{T}_i$$. Let $$\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 scaling factor $$s_i\in\mathbb{R}$$ and the rotation matrix $$\mathbf{R}_i\in\mathbb{R}^{3\times 3}$$ can be computed as:

\begin{aligned} s_i &= \sqrt{\vert \mathbf{U}_i \vert}\\ \mathbf{R}_i &= \mathbf{U}_i/s_i \end{aligned}

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, s_i]$

Warning

Numerically, a transformation matrix is considered legal if:

$\vert s \vert > \texttt{atol} \\ |{\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.,-0.5,  0., 0.1],
...                       [0.5,  0.,  0., 0.2],
...                       [ 0.,  0., 0.5, 0.3],
...                       [ 0.,  0.,  0.,  1.]])
>>> pp.mat2Sim3(input)
Sim3Type LieTensor:
tensor([0.1000, 0.2000, 0.3000, 0.0000, 0.0000, 0.7071, 0.7071, 0.5000])


Note

We follow the convention below to express Sim3:

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

referred to this paper:

where $$\mathbf{R}$$ is the individual matrix in a batch. The scaling factor $$s$$ defines a linear transformation that enlarges or diminishes the object in the same ratio across 3 dimensions, the translation vector $$\mathbf{t}$$ defines the displacement between the original position and the transformed position.

We also notice that there is another popular convention:

$\begin{bmatrix} \mathbf{R}_{3\times3} & \mathbf{t}_{3\times1}\\ \textbf{0} & 1/s \end{bmatrix},$

referred to this tutorial:

See pypose.Sim3() 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