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