pypose.mat2RxSO3

pypose.mat2RxSO3(mat, check=True, rtol=1e-05, atol=1e-05)

Convert batched rotation or transformation matrices to RxSO3Type LieTensor.

Parameters

• mat (Tensor) – the batched matrices to convert. If input is of shape (*, 3, 4) or (*, 4, 4), only the top left 3x3 submatrix is used.

• 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 RxSO3Type LieTensor.

Return type

LieTensor

Shape:

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

Output: (*, 5)

Let the input be matrix \(\mathbf{T}\), \(\mathbf{T}_i\) represents each individual matrix in the batch. \(\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{T_i} \vert}\\ \mathbf{R}_i &= \mathbf{R}_i/s_i \end{aligned}, \]

the translation and quaternion can be computed by:

\[\left\{\begin{aligned} 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 = [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.

Examples

>>> input = torch.tensor([[ 0., -0.5,  0.],
...                       [0.5,   0.,  0.],
...                       [ 0.,   0., 0.5]])
>>> pp.mat2RxSO3(input)
RxSO3Type LieTensor:
tensor([0.0000, 0.0000, 0.7071, 0.7071, 0.5000])

Note

The individual matrix in a batch can be written as: \(s\mathbf{R}_{3\times3}\), where \(\mathbf{R}\) is the rotation matrix. where the scaling factor \(s\) defines a linear transformation that enlarges or diminishes the object in the same ratio across 3 dimensions.

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