Rotate points by angles according to axis. Args: points (np.ndarray | torch.Tensor | list | tuple ): Points of shape (N, M, 3). angles (np.ndarray | torch.Tensor | list | tuple): Vector of angles in shape (N, 3) return_mat: Whether or not return t
(points, angles, return_mat=False, clockwise=False)
| 12 | |
| 13 | |
| 14 | def rotation_3d_in_euler(points, angles, return_mat=False, clockwise=False): |
| 15 | """Rotate points by angles according to axis. |
| 16 | |
| 17 | Args: |
| 18 | points (np.ndarray | torch.Tensor | list | tuple ): |
| 19 | Points of shape (N, M, 3). |
| 20 | angles (np.ndarray | torch.Tensor | list | tuple): |
| 21 | Vector of angles in shape (N, 3) |
| 22 | return_mat: Whether or not return the rotation matrix (transposed). |
| 23 | Defaults to False. |
| 24 | clockwise: Whether the rotation is clockwise. Defaults to False. |
| 25 | |
| 26 | Raises: |
| 27 | ValueError: when the axis is not in range [0, 1, 2], it will |
| 28 | raise value error. |
| 29 | |
| 30 | Returns: |
| 31 | (torch.Tensor | np.ndarray): Rotated points in shape (N, M, 3). |
| 32 | """ |
| 33 | batch_free = len(points.shape) == 2 |
| 34 | if batch_free: |
| 35 | points = points[None] |
| 36 | |
| 37 | if len(angles.shape) == 1: |
| 38 | angles = angles.expand(points.shape[:1] + (3, )) |
| 39 | # angles = torch.full(points.shape[:1], angles) |
| 40 | |
| 41 | assert len(points.shape) == 3 and len(angles.shape) == 2 \ |
| 42 | and points.shape[0] == angles.shape[0], f'Incorrect shape of points ' \ |
| 43 | f'angles: {points.shape}, {angles.shape}' |
| 44 | |
| 45 | assert points.shape[-1] in [2, 3], \ |
| 46 | f'Points size should be 2 or 3 instead of {points.shape[-1]}' |
| 47 | |
| 48 | rot_mat_T = euler_angles_to_matrix(angles, 'ZXY') # N, 3,3 |
| 49 | rot_mat_T = rot_mat_T.transpose(-2, -1) |
| 50 | |
| 51 | if clockwise: |
| 52 | raise NotImplementedError('clockwise') |
| 53 | |
| 54 | if points.shape[0] == 0: |
| 55 | points_new = points |
| 56 | else: |
| 57 | points_new = torch.bmm(points, rot_mat_T) |
| 58 | |
| 59 | if batch_free: |
| 60 | points_new = points_new.squeeze(0) |
| 61 | |
| 62 | if return_mat: |
| 63 | if batch_free: |
| 64 | rot_mat_T = rot_mat_T.squeeze(0) |
| 65 | return points_new, rot_mat_T |
| 66 | else: |
| 67 | return points_new |
| 68 | |
| 69 | |
| 70 | class EulerDepthInstance3DBoxes: |