preprocessing/data_preprocessing/build_view_actions.py
7.1 KB · 153 lines · python Raw
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