Buckets:
| import numpy as np | |
| from scipy.spatial.transform import Rotation | |
| Z_UP_FROM_ARKIT = np.array([[1.0, 0.0, 0.0], [0.0, 0.0, -1.0], [0.0, 1.0, 0.0]]) | |
| MUJOCO_CAMERA_FROM_OPENCV = np.diag([1.0, -1.0, -1.0]) | |
| def to_z_up(poses): | |
| out = np.array(poses, dtype=np.float64) | |
| out[..., :3, :3] = Z_UP_FROM_ARKIT @ out[..., :3, :3] | |
| out[..., :3, 3] = out[..., :3, 3] @ Z_UP_FROM_ARKIT.T | |
| return out | |
| def quat_wxyz(rotations): | |
| flat = np.asarray(rotations).reshape(-1, 3, 3) | |
| xyzw = Rotation.from_matrix(flat).as_quat() | |
| return np.roll(xyzw, 1, axis=-1).reshape(rotations.shape[:-2] + (4,)) | |
| def relative_rotation(parent, child): | |
| return np.swapaxes(parent, -1, -2) @ child | |
| def look_at_xyaxes(eye, target, up=(0.0, 0.0, 1.0)): | |
| forward = np.asarray(target, dtype=np.float64) - np.asarray(eye, dtype=np.float64) | |
| forward /= np.linalg.norm(forward) | |
| right = np.cross(forward, up) | |
| right /= np.linalg.norm(right) | |
| return np.concatenate([right, np.cross(right, forward)]) | |
| def swing(rest, current): | |
| rest = rest / np.linalg.norm(rest) | |
| current = current / np.linalg.norm(current, axis=-1, keepdims=True) | |
| axis = np.cross(rest, current) | |
| cosine = current @ rest | |
| skew = np.zeros(current.shape[:-1] + (3, 3)) | |
| skew[..., 0, 1], skew[..., 0, 2] = -axis[..., 2], axis[..., 1] | |
| skew[..., 1, 0], skew[..., 1, 2] = axis[..., 2], -axis[..., 0] | |
| skew[..., 2, 0], skew[..., 2, 1] = -axis[..., 1], axis[..., 0] | |
| return np.eye(3) + skew + skew @ skew / (1.0 + cosine)[..., None, None] | |
| def best_rotation(rest, current): | |
| if rest.shape[0] == 1: | |
| return swing(rest[0], current[:, 0]) | |
| covariance = np.einsum("ci,tcj->tij", rest, current) | |
| u, _, vt = np.linalg.svd(covariance) | |
| sign = np.sign(np.linalg.det(np.swapaxes(vt, -1, -2) @ np.swapaxes(u, -1, -2))) | |
| correction = np.zeros_like(u) | |
| correction[:, 0, 0] = correction[:, 1, 1] = 1.0 | |
| correction[:, 2, 2] = sign | |
| return np.swapaxes(vt, -1, -2) @ correction @ np.swapaxes(u, -1, -2) | |
Xet Storage Details
- Size:
- 2.01 kB
- Xet hash:
- bbb139dcff983f59f5394db02ac76db3cb3453b5b17bf17bf8ecf0b63c247af5
·
Xet efficiently stores files, intelligently splitting them into unique chunks and accelerating uploads and downloads. More info.