Decompose a 4x4 transform matrix into position and orientation. Args: mat: A 4x4 transform matrix. Returns: ``(pos, quat_xyzw)`` with position ``[x, y, z]`` and SciPy quaternion ``[x, y, z, w]``.
(mat: np.ndarray)
| 27 | |
| 28 | |
| 29 | def decompose_transform_matrix(mat: np.ndarray) -> Tuple[np.ndarray, np.ndarray]: |
| 30 | """ |
| 31 | Decompose a 4x4 transform matrix into position and orientation. |
| 32 | |
| 33 | Args: |
| 34 | mat: A 4x4 transform matrix. |
| 35 | |
| 36 | Returns: |
| 37 | ``(pos, quat_xyzw)`` with position ``[x, y, z]`` and SciPy quaternion |
| 38 | ``[x, y, z, w]``. |
| 39 | """ |
| 40 | pos = mat[:3, 3].copy() |
| 41 | quat_xyzw = R.from_matrix(mat[:3, :3]).as_quat() |
| 42 | return pos, quat_xyzw |
| 43 | |
| 44 | |
| 45 | def compute_relative_transform(source_mat: np.ndarray, ref_mat: np.ndarray) -> np.ndarray: |
no outgoing calls
no test coverage detected