Get a coordinate frame from z and y vector. z will be used directly as the z axis. y will be made orthogonal to y and used as the y axis. x axis will be the the cross-product of y axis and z axis. All axes are normalised to have unit norm. Args: z: (*, 3) y:
(
z: T.Union[np.ndarray, torch.Tensor] = (0, 0, -1.),
y: T.Union[np.ndarray, torch.Tensor] = (0, -1., 0.),
)
| 408 | |
| 409 | |
| 410 | def construct_coord_frame( |
| 411 | z: T.Union[np.ndarray, torch.Tensor] = (0, 0, -1.), |
| 412 | y: T.Union[np.ndarray, torch.Tensor] = (0, -1., 0.), |
| 413 | ) -> T.Union[np.ndarray, torch.Tensor]: |
| 414 | """ |
| 415 | Get a coordinate frame from z and y vector. |
| 416 | z will be used directly as the z axis. |
| 417 | y will be made orthogonal to y and used as the y axis. |
| 418 | x axis will be the the cross-product of y axis and z axis. |
| 419 | All axes are normalised to have unit norm. |
| 420 | |
| 421 | Args: |
| 422 | z: (*, 3) |
| 423 | y: (*, 3) |
| 424 | |
| 425 | Returns: |
| 426 | (*, 3, 3): |
| 427 | For the last 2 dimension, the first column is the x axis, second y, last z. |
| 428 | It can be used as the rotation matrix that transform |
| 429 | a vector in camera coord to world coord. |
| 430 | """ |
| 431 | |
| 432 | if isinstance(z, (tuple, list)): |
| 433 | z = torch.tensor(z) |
| 434 | if isinstance(y, (tuple, list)): |
| 435 | y = torch.tensor(y) |
| 436 | |
| 437 | is_numpy = False |
| 438 | if isinstance(z, np.ndarray): |
| 439 | z = torch.from_numpy(z) |
| 440 | is_numpy = True |
| 441 | if isinstance(y, np.ndarray): |
| 442 | y = torch.from_numpy(y) |
| 443 | is_numpy = True |
| 444 | |
| 445 | z_norm = torch.linalg.norm(z, ord=2, dim=-1, keepdim=True) # (*, 1) |
| 446 | assert torch.all(z_norm > 0) |
| 447 | assert torch.all(torch.linalg.norm(y, ord=2, dim=-1) > 0) |
| 448 | x = torch.cross(y, z, dim=-1) # (*, 3) |
| 449 | if torch.any(torch.linalg.norm(x, ord=2, dim=-1) == 0): |
| 450 | raise ValueError("y and z cannot be parallel.") |
| 451 | |
| 452 | # make sure y-axis is perpendicular to z-axis |
| 453 | z = z / z_norm # (*, 3) |
| 454 | y_on_z = torch.sum(y * z, dim=-1, keepdim=True) * z # (*, 3) |
| 455 | y = y - y_on_z |
| 456 | |
| 457 | # normalize |
| 458 | y = y / torch.linalg.norm(y, ord=2, dim=-1, keepdim=True) # (*, 3) |
| 459 | x = x / torch.linalg.norm(x, ord=2, dim=-1, keepdim=True) # (*, 3) |
| 460 | |
| 461 | Rs = torch.stack((x, y, z), dim=-1) # (*, 3, 3) |
| 462 | |
| 463 | if is_numpy: |
| 464 | Rs = Rs.detach().cpu().numpy() |
| 465 | |
| 466 | return Rs |
| 467 |
no test coverage detected