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[3]{\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:

Please make sure your own convention before using this function.

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