Convert Euler angles to quaternions.
(e, order, deg=True)
| 167 | |
| 168 | |
| 169 | def euler2quat(e, order, deg=True): |
| 170 | """ |
| 171 | Convert Euler angles to quaternions. |
| 172 | """ |
| 173 | assert e.shape[-1] == 3 |
| 174 | |
| 175 | original_shape = list(e.shape) |
| 176 | original_shape[-1] = 4 |
| 177 | |
| 178 | e = e.view(-1, 3) |
| 179 | |
| 180 | ## if euler angles in degrees |
| 181 | if deg: |
| 182 | e = e * np.pi / 180. |
| 183 | |
| 184 | x = e[:, 0] |
| 185 | y = e[:, 1] |
| 186 | z = e[:, 2] |
| 187 | |
| 188 | rx = torch.stack((torch.cos(x / 2), torch.sin(x / 2), torch.zeros_like(x), torch.zeros_like(x)), dim=1) |
| 189 | ry = torch.stack((torch.cos(y / 2), torch.zeros_like(y), torch.sin(y / 2), torch.zeros_like(y)), dim=1) |
| 190 | rz = torch.stack((torch.cos(z / 2), torch.zeros_like(z), torch.zeros_like(z), torch.sin(z / 2)), dim=1) |
| 191 | |
| 192 | result = None |
| 193 | for coord in order: |
| 194 | if coord == 'x': |
| 195 | r = rx |
| 196 | elif coord == 'y': |
| 197 | r = ry |
| 198 | elif coord == 'z': |
| 199 | r = rz |
| 200 | else: |
| 201 | raise |
| 202 | if result is None: |
| 203 | result = r |
| 204 | else: |
| 205 | result = qmul(result, r) |
| 206 | |
| 207 | # Reverse antipodal representation to have a non-negative "w" |
| 208 | if order in ['xyz', 'yzx', 'zxy']: |
| 209 | result *= -1 |
| 210 | |
| 211 | return result.view(original_shape) |
| 212 | |
| 213 | |
| 214 | def expmap_to_quaternion(e): |