preprocessing/data_preprocessing/build_view_actions.py
| 1 | #!/usr/bin/env python |
| 2 | """Batch the sensor-pose -> camera projection over every episode -> <episode>/view_actions.pt. |
| 3 | |
| 4 | For each episode this projects both tactile sensors' absolute pose into all three camera views |
| 5 | (see project_pose_to_camera.py for the geometry and its departures from arXiv 2604.06168) and |
| 6 | stores the result on the **6 fps grid** the rest of the pipeline uses. |
| 7 | |
| 8 | What is stored is the projected *points*, not the RGB Gaussian "action image". The image is a |
| 9 | deterministic rendering of these three points, so keeping the points (~250 kB/episode instead of |
| 10 | ~130 MB) leaves sigma, resolution and the render itself changeable in the dataloader without |
| 11 | rebuilding anything. Use build_action_images.py to materialize images when a run needs them. |
| 12 | |
| 13 | These are per-FRAME quantities (an absolute pose), not per-transition, so N = ceil(n_native/5) -- |
| 14 | the same length as the tactile latents' num_frames, one more than the fps6 action count. |
| 15 | |
| 16 | python data_preprocessing/build_view_actions.py # all episodes |
| 17 | python data_preprocessing/build_view_actions.py --episodes motherboard_0510_episode_004 |
| 18 | """ |
| 19 | from __future__ import annotations |
| 20 | |
| 21 | import argparse |
| 22 | import sys |
| 23 | from pathlib import Path |
| 24 | |
| 25 | import numpy as np |
| 26 | import torch |
| 27 | |
| 28 | sys.path.insert(0, str(Path(__file__).resolve().parent)) |
| 29 | from project_pose_to_camera import ( # noqa: E402 |
| 30 | PARQUET_ROOT, ROOT, SENSORS, VIEWS, AXIS_LEN_MM, load_calibration, load_episode_poses, |
| 31 | project, sensor_points, task_for_episode, world_frame_offset, |
| 32 | ) |
| 33 | |
| 34 | VID = ROOT / "mode1_v2_videos" |
| 35 | OUT_NAME = "view_actions.pt" |
| 36 | POINT_ORDER = ("pos", "normal", "up") |
| 37 | |
| 38 | |
| 39 | def build_episode(episode: str, stride: int, apply_world_offset: bool = False) -> dict: |
| 40 | task = task_for_episode(episode) |
| 41 | cal = load_calibration(task) |
| 42 | poses = load_episode_poses(episode, apply_world_offset=apply_world_offset) |
| 43 | |
| 44 | n_native = len(poses["left"]) |
| 45 | idx = np.arange(0, n_native, stride) # 0, 5, 10, ... -- same grid as build_fps6_actions |
| 46 | |
| 47 | pts3d = {} |
| 48 | for s_ in SENSORS: |
| 49 | q = np.stack(sensor_points(poses[s_], cal["gel"][s_], AXIS_LEN_MM), axis=1) # (n,3,3) mocap mm |
| 50 | pts3d[s_] = q[idx] |
| 51 | |
| 52 | out = { |
| 53 | "frame_idx": torch.from_numpy(idx.astype(np.int32)), |
| 54 | "fps": 30.0 / stride, "src_fps": 30.0, "fps_stride": stride, |
| 55 | "num_frames": len(idx), "native_num_frames": n_native, |
| 56 | "cameras": {}, |
| 57 | "points_mm": {s_: torch.from_numpy(pts3d[s_].astype(np.float32)) for s_ in SENSORS}, |
| 58 | "pose": {s_: torch.from_numpy(poses[s_][idx].astype(np.float32)) for s_ in SENSORS}, |
| 59 | } |
| 60 | |
| 61 | for v in VIEWS: |
| 62 | view = cal["views"][v] |
| 63 | intr = view["intr"] |
| 64 | W, H = float(intr["width"]), float(intr["height"]) |
| 65 | |
| 66 | # (N, sensor, point, 3) with the last axis (u_px, v_px, z_mm). |
| 67 | # sensor axis = SENSORS order = [left, right]; point axis = [pos, normal, up]. |
| 68 | uvz = np.zeros((len(idx), len(SENSORS), 3, 3), np.float32) |
| 69 | valid = np.zeros((len(idx), len(SENSORS), 3), bool) |
| 70 | for si, s_ in enumerate(SENSORS): |
| 71 | for k in range(3): |
| 72 | u_, v_, z_, ok_ = project(pts3d[s_][:, k, :], view) |
| 73 | uvz[:, si, k, 0] = u_ |
| 74 | uvz[:, si, k, 1] = v_ |
| 75 | uvz[:, si, k, 2] = z_ |
| 76 | valid[:, si, k] = ok_ |
| 77 | |
| 78 | uvz_norm = uvz.copy() |
| 79 | uvz_norm[..., 0] /= W # z is left in mm: there is no natural depth normalizer, |
| 80 | uvz_norm[..., 1] /= H # so the two tensors differ only in the u,v channels. |
| 81 | |
| 82 | # `view_` prefix: a bare "left" would collide with the *sensor* key "left" in |
| 83 | # points_mm/pose, which mean different things. This matches the view_*.mp4 names. |
| 84 | out[f"view_{v}"] = torch.from_numpy(uvz) |
| 85 | out[f"view_{v}_norm"] = torch.from_numpy(uvz_norm) |
| 86 | out[f"view_{v}_valid"] = torch.from_numpy(valid) |
| 87 | out["cameras"][v] = {"intrinsics": dict(intr), |
| 88 | "T_mocap_to_cam": torch.from_numpy(view["T"].copy()), |
| 89 | "rmse_mm": view["rmse"], |
| 90 | "camera_serial": view["serial"]} |
| 91 | |
| 92 | out["meta"] = { |
| 93 | "source": "arXiv 2604.06168 action-image points, projected per camera view", |
| 94 | "view_tensor": "view_{left,middle,right}: (N, sensor, point, 3) = (u_px, v_px, z_mm); " |
| 95 | "view_*_norm has u/W, v/H and the SAME z in mm", |
| 96 | "sensor_axis": list(SENSORS), |
| 97 | "point_axis": list(POINT_ORDER), |
| 98 | "valid": "view_*_valid (N, sensor, point) bool = z>0 AND inside the image", |
| 99 | "axis_len_mm": AXIS_LEN_MM, |
| 100 | "q_pos": "measured gel centre (T_gel_to_rigid.gel_center_in_rigid_mm), NOT the " |
| 101 | "tracked rigid-body origin", |
| 102 | "q_normal": "measured gel axis (gel_axis_in_rigid) -- a deliberate departure from " |
| 103 | "the paper, which rotates a canonical -z because it has no gel calibration", |
| 104 | "q_up": "rigid-body x, Gram-Schmidt-orthogonalized against the gel normal", |
| 105 | "chain": "pose(m) x1000 -> +R@gel_center -> T_mocap_to_cam -> fx*X/Z+ppx", |
| 106 | "task": task, "calibration": cal["meta"].get("calibration_id"), |
| 107 | "world_frame_offset_applied": bool(apply_world_offset), |
| 108 | "world_frame_offset_m": list(world_frame_offset(task, episode)), |
| 109 | "units": "u,v px (or [0,1] in *_norm); z mm; points_mm mm; pose [xyz m, quat xyzw]", |
| 110 | } |
| 111 | return out |
| 112 | |
| 113 | |
| 114 | def main() -> None: |
| 115 | ap = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) |
| 116 | ap.add_argument("--videos-root", type=Path, default=VID) |
| 117 | ap.add_argument("--episodes", nargs="*", default=None) |
| 118 | ap.add_argument("--stride", type=int, default=5, help="5 -> 6 fps, matching the rest of the pipeline") |
| 119 | ap.add_argument("--overwrite", action="store_true") |
| 120 | ap.add_argument("--apply-world-offset", action="store_true", |
| 121 | help="treat episodes.jsonl:world_frame_offset as a PENDING correction and " |
| 122 | "add it to the translation (default: off -- it is already applied)") |
| 123 | ap.add_argument("--out-name", default=OUT_NAME, |
| 124 | help="output filename, so both hypotheses can coexist for comparison") |
| 125 | a = ap.parse_args() |
| 126 | |
| 127 | eps = a.episodes or sorted( |
| 128 | p.name for p in a.videos_root.iterdir() |
| 129 | if p.is_dir() and (PARQUET_ROOT / f"{p.name}.parquet").is_file() |
| 130 | ) |
| 131 | print(f"{len(eps)} episode(s), stride={a.stride} -> {30/a.stride:g} fps\n") |
| 132 | |
| 133 | n_written = 0 |
| 134 | for ep in eps: |
| 135 | out = a.videos_root / ep / a.out_name |
| 136 | if out.exists() and not a.overwrite: |
| 137 | print(f" [skip ] {ep}: {a.out_name} exists") |
| 138 | continue |
| 139 | d = build_episode(ep, a.stride, a.apply_world_offset) |
| 140 | tmp = out.with_suffix(".pt.tmp") |
| 141 | torch.save(d, tmp) |
| 142 | tmp.replace(out) # atomic; a killed run leaves no partial file |
| 143 | n_written += 1 |
| 144 | frac = {v: float(d[f"view_{v}_valid"][:, :, 0].float().mean()) for v in VIEWS} |
| 145 | print(f" [write] {ep:<34} N={d['num_frames']:>5} in-frame " |
| 146 | + " ".join(f"{v}={100*frac[v]:5.1f}%" for v in VIEWS) |
| 147 | + f" {out.stat().st_size/1e3:6.0f} kB") |
| 148 | print(f"\ndone ({n_written} written)") |
| 149 | |
| 150 | |
| 151 | if __name__ == "__main__": |
| 152 | main() |
| 153 | |