Given a (*, 3) vector v, return a (*, 3, 3) cross_product matrix [v]_x, such that for all (3,) vector u, we have v * u = [v]_x * u. Vx = np.array([ [0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0], ])
(
v: T.Union[np.ndarray, torch.Tensor],
)
| 311 | |
| 312 | |
| 313 | def get_cross_product_matrix( |
| 314 | v: T.Union[np.ndarray, torch.Tensor], |
| 315 | ) -> T.Union[np.ndarray, torch.Tensor]: |
| 316 | """ |
| 317 | Given a (*, 3) vector v, return a (*, 3, 3) cross_product matrix [v]_x, |
| 318 | such that for all (3,) vector u, we have v * u = [v]_x * u. |
| 319 | |
| 320 | Vx = np.array([ |
| 321 | [0, -v[2], v[1]], |
| 322 | [v[2], 0, -v[0]], |
| 323 | [-v[1], v[0], 0], |
| 324 | ]) |
| 325 | |
| 326 | """ |
| 327 | is_numpy = False |
| 328 | if isinstance(v, np.ndarray): |
| 329 | is_numpy = True |
| 330 | v = torch.from_numpy(v) |
| 331 | |
| 332 | *b_shape, d = v.shape |
| 333 | assert d == 3 |
| 334 | Vx = torch.zeros(*b_shape, 3, 3, dtype=v.dtype, device=v.device) |
| 335 | Vx[..., 0, 1] = -v[..., 2] |
| 336 | Vx[..., 0, 2] = v[..., 1] |
| 337 | Vx[..., 1, 2] = -v[..., 0] |
| 338 | Vx = Vx - Vx.transpose(-1, -2) |
| 339 | |
| 340 | if is_numpy: |
| 341 | Vx = Vx.detach().cpu().numpy() |
| 342 | |
| 343 | return Vx # (*, 3, 3) |
| 344 | |
| 345 | |
| 346 | def get_random_direction(*shape, rng: np.random.RandomState = None): |
no test coverage detected