Convert quaternion(s) q to Euler angles. Expects a tensor of shape (*, 4), where * denotes any number of dimensions. Returns a tensor of shape (*, 3).
(q, order, epsilon=0, deg=True)
| 77 | |
| 78 | |
| 79 | def qeuler(q, order, epsilon=0, deg=True): |
| 80 | """ |
| 81 | Convert quaternion(s) q to Euler angles. |
| 82 | Expects a tensor of shape (*, 4), where * denotes any number of dimensions. |
| 83 | Returns a tensor of shape (*, 3). |
| 84 | """ |
| 85 | assert q.shape[-1] == 4 |
| 86 | |
| 87 | original_shape = list(q.shape) |
| 88 | original_shape[-1] = 3 |
| 89 | q = q.view(-1, 4) |
| 90 | |
| 91 | q0 = q[:, 0] |
| 92 | q1 = q[:, 1] |
| 93 | q2 = q[:, 2] |
| 94 | q3 = q[:, 3] |
| 95 | |
| 96 | if order == 'xyz': |
| 97 | x = torch.atan2(2 * (q0 * q1 - q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2)) |
| 98 | y = torch.asin( |
| 99 | torch.clamp(2 * (q1 * q3 + q0 * q2), -1 + epsilon, 1 - epsilon)) |
| 100 | z = torch.atan2(2 * (q0 * q3 - q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3)) |
| 101 | elif order == 'yzx': |
| 102 | x = torch.atan2(2 * (q0 * q1 - q2 * q3), 1 - 2 * (q1 * q1 + q3 * q3)) |
| 103 | y = torch.atan2(2 * (q0 * q2 - q1 * q3), 1 - 2 * (q2 * q2 + q3 * q3)) |
| 104 | z = torch.asin( |
| 105 | torch.clamp(2 * (q1 * q2 + q0 * q3), -1 + epsilon, 1 - epsilon)) |
| 106 | elif order == 'zxy': |
| 107 | x = torch.asin( |
| 108 | torch.clamp(2 * (q0 * q1 + q2 * q3), -1 + epsilon, 1 - epsilon)) |
| 109 | y = torch.atan2(2 * (q0 * q2 - q1 * q3), 1 - 2 * (q1 * q1 + q2 * q2)) |
| 110 | z = torch.atan2(2 * (q0 * q3 - q1 * q2), 1 - 2 * (q1 * q1 + q3 * q3)) |
| 111 | elif order == 'xzy': |
| 112 | x = torch.atan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q3 * q3)) |
| 113 | y = torch.atan2(2 * (q0 * q2 + q1 * q3), 1 - 2 * (q2 * q2 + q3 * q3)) |
| 114 | z = torch.asin( |
| 115 | torch.clamp(2 * (q0 * q3 - q1 * q2), -1 + epsilon, 1 - epsilon)) |
| 116 | elif order == 'yxz': |
| 117 | x = torch.asin( |
| 118 | torch.clamp(2 * (q0 * q1 - q2 * q3), -1 + epsilon, 1 - epsilon)) |
| 119 | y = torch.atan2(2 * (q1 * q3 + q0 * q2), 1 - 2 * (q1 * q1 + q2 * q2)) |
| 120 | z = torch.atan2(2 * (q1 * q2 + q0 * q3), 1 - 2 * (q1 * q1 + q3 * q3)) |
| 121 | elif order == 'zyx': |
| 122 | x = torch.atan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2)) |
| 123 | y = torch.asin( |
| 124 | torch.clamp(2 * (q0 * q2 - q1 * q3), -1 + epsilon, 1 - epsilon)) |
| 125 | z = torch.atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3)) |
| 126 | else: |
| 127 | raise |
| 128 | |
| 129 | if deg: |
| 130 | return torch.stack((x, y, z), dim=1).view(original_shape) * 180 / np.pi |
| 131 | else: |
| 132 | return torch.stack((x, y, z), dim=1).view(original_shape) |
| 133 | |
| 134 | |
| 135 | # Numpy-backed implementations |