preprocessing/data_preprocessing/build_episode_actions.py
9.3 KB · 220 lines · python Raw
1 #!/usr/bin/env python
2 """Precompute per-episode sensor-frame 9D actions from the mode1_v2 parquets.
3
4 Reads the world-frame sensor poses
5
6 <parquet-root>/<episode>.parquet columns sensor_left_pose / sensor_right_pose,
7 each (T, 7) as [x, y, z, qx, qy, qz, qw]
8
9 and writes, next to that episode's videos,
10
11 <videos-root>/<episode>/sensor_actions.pt
12 action_left : (T-1, 9) float32 [dp(3, metres), Rot6D(dR)(6)]
13 action_right : (T-1, 9) float32
14 valid_left : (T-1,) bool False where the step looks like tracking dropout
15 valid_right : (T-1,) bool
16 rot_deg_left : (T-1,) float32 per-step rotation magnitude, degrees
17 rot_deg_right: (T-1,) float32
18 frame_idx : (T,) int32 action[t] maps frame t -> t+1
19 meta : dict conventions, so consumers need not guess
20
21 Both actions are expressed in the sensor frame at time t (see common/actions.py);
22 the rotation is stored as raw Rot6D(dR), NOT identity-subtracted — that transform
23 belongs at the model input, alongside standardization, so this file stays a
24 faithful encoding of the pose stream.
25
26 Left and right stay as two separate (T-1, 9) tensors rather than one (T-1, 18):
27 consumers that want the concatenated form build it at the model boundary, and
28 single-sided uses do not have to slice.
29
30 With --gel-frame the poses are first composed with T_rigid_gel (see common/gel_frame.py)
31 and written to sensor_actions_gel.pt instead. The action is then expressed in the GEL
32 frame: +z is the real gel normal and the origin sits on the gel surface, so dp_z is press
33 depth and dp_x, dp_y are in-plane shear. sensor_action_9d itself is unchanged — only its
34 input poses are. rot_deg and valid are identical either way, because dR_gel = R_g^T dR R_g
35 is a conjugation and so preserves the geodesic angle.
36 """
37 from __future__ import annotations
38
39 import argparse
40 import os
41 import sys
42 from pathlib import Path
43
44 import numpy as np
45 import torch
46
47 sys.path.insert(0, str(Path(__file__).resolve().parents[1]))
48
49 from common.actions import rot6d_to_matrix, sensor_action_9d # noqa: E402
50 from common.gel_frame import calib_dir_for, gel_frame, poses_to_gel # noqa: E402
51
52 DEFAULT_PARQUET_ROOT = Path(os.environ.get("ACTS_PARQUET_ROOT", "mode1_v2"))
53 DEFAULT_VIDEOS_ROOT = Path(os.environ.get("ACTS_ACTIONS_OUT", "mode1_v2_videos"))
54 OUT_NAME = "sensor_actions.pt"
55 GEL_OUT_NAME = "sensor_actions_gel.pt"
56
57 POSE_COLUMNS = ("sensor_left_pose", "sensor_right_pose")
58
59
60 def read_poses(path: Path) -> tuple[torch.Tensor, torch.Tensor, np.ndarray]:
61 """Parquet -> (pose_left, pose_right) as (T, 7) float64 tensors, plus frame_idx."""
62 import pyarrow.parquet as pq
63
64 cols = list(POSE_COLUMNS)
65 have = set(pq.ParquetFile(str(path)).schema_arrow.names)
66 if "frame_idx" in have:
67 cols.append("frame_idx")
68 missing = set(POSE_COLUMNS) - have
69 if missing:
70 raise RuntimeError(f"{path.name}: missing pose column(s) {sorted(missing)}")
71
72 table = pq.read_table(str(path), columns=cols)
73
74 def _pose(name: str) -> torch.Tensor:
75 arr = np.asarray(table.column(name).to_pylist(), dtype=np.float64)
76 if arr.ndim != 2 or arr.shape[1] != 7:
77 raise RuntimeError(f"{path.name}: expected (T,7) for {name}, got {arr.shape}")
78 return torch.from_numpy(arr)
79
80 pl, pr = _pose(POSE_COLUMNS[0]), _pose(POSE_COLUMNS[1])
81 if pl.shape != pr.shape:
82 raise RuntimeError(f"{path.name}: left/right pose length mismatch {pl.shape} vs {pr.shape}")
83
84 if "frame_idx" in cols:
85 frame_idx = np.asarray(table.column("frame_idx").to_pylist(), dtype=np.int32)
86 else:
87 frame_idx = np.arange(pl.shape[0], dtype=np.int32)
88
89 return pl, pr, frame_idx
90
91
92 def rotation_deg(action9: torch.Tensor) -> torch.Tensor:
93 """Per-step geodesic rotation magnitude, degrees, recovered from the 6D block."""
94 R = rot6d_to_matrix(action9[:, 3:])
95 cos = (R.diagonal(dim1=-2, dim2=-1).sum(-1) - 1.0) / 2.0
96 return torch.rad2deg(torch.arccos(cos.clamp(-1.0, 1.0)))
97
98
99 def build_episode(parquet: Path, out_dir: Path, max_rot_deg: float,
100 out_name: str = OUT_NAME, gel: bool = False) -> dict:
101 pose_l, pose_r, frame_idx = read_poses(parquet)
102
103 payload: dict[str, object] = {}
104 gel_meta: dict[str, object] = {}
105 n_flagged = 0
106 for side, pose in (("left", pose_l), ("right", pose_r)):
107 if gel:
108 # Re-express the poses in the gel frame BEFORE the (unchanged) converter, so
109 # the 9D action comes out with +z along the real gel normal and its origin on
110 # the gel surface. Every downstream raster inherits the fix for free.
111 R_g, c_gel, info = gel_frame(side, calib_dir_for(parquet.stem))
112 pose = poses_to_gel(pose, R_g, c_gel)
113 gel_meta[side] = info
114 action = sensor_action_9d(pose)
115 rot_deg = rotation_deg(action)
116 # A >30 deg/frame step at 30 fps is tracking dropout or a quaternion sign
117 # flip, not real motion. Flag rather than drop, so indices stay aligned.
118 valid = rot_deg <= max_rot_deg
119 n_flagged += int((~valid).sum())
120
121 if not torch.isfinite(action).all():
122 raise RuntimeError(f"{parquet.name}: non-finite action for {side}")
123
124 payload[f"action_{side}"] = action.float()
125 payload[f"valid_{side}"] = valid
126 payload[f"rot_deg_{side}"] = rot_deg.float()
127
128 payload["frame_idx"] = torch.from_numpy(frame_idx)
129 payload["meta"] = {
130 "layout": "[dp_x, dp_y, dp_z, r1..r6]",
131 "frame": "sensor frame at time t; dp = R_t^T (p_{t+1} - p_t), dR = R_t^T R_{t+1}",
132 "rot6d": "raw Rot6D(dR), first two columns as [c1x,c1y,c1z, c2x,c2y,c2z]; NOT identity-subtracted",
133 "units": "metres",
134 "quat_order": "xyzw",
135 "source": str(parquet),
136 "max_rot_deg": max_rot_deg,
137 "gel_frame": bool(gel),
138 }
139 if gel:
140 # Record the full basis: the in-plane axes are a free choice, so Stage 2 has to
141 # be able to reproduce this exact one.
142 payload["meta"]["gel_calibration"] = gel_meta
143 payload["meta"]["frame"] = (
144 "GEL frame at time t (+z = gel_axis_in_rigid, origin = gel surface centre); "
145 "poses composed with T_rigid_gel, then the standard sensor_action_9d"
146 )
147
148 out_dir.mkdir(parents=True, exist_ok=True)
149 out_path = out_dir / out_name
150 tmp = out_path.with_name(out_path.name + ".tmp") # NOT with_suffix: it eats ".pt"
151 torch.save(payload, tmp)
152 tmp.replace(out_path)
153
154 return {
155 "path": out_path,
156 "T": int(pose_l.shape[0]),
157 "n_actions": int(payload["action_left"].shape[0]),
158 "flagged": n_flagged,
159 }
160
161
162 def main():
163 ap = argparse.ArgumentParser(
164 description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter
165 )
166 ap.add_argument("--parquet-root", type=Path, default=DEFAULT_PARQUET_ROOT)
167 ap.add_argument("--videos-root", type=Path, default=DEFAULT_VIDEOS_ROOT)
168 ap.add_argument("--episodes", nargs="*", default=None,
169 help="explicit episode stems (default: every parquet in --parquet-root)")
170 ap.add_argument("--max-rot-deg", type=float, default=30.0,
171 help="per-step rotation above this is flagged invalid (not dropped)")
172 ap.add_argument("--gel-frame", action="store_true",
173 help="express the action in the gel frame (+z = gel normal, origin on "
174 "the gel surface) instead of the raw OptiTrack rigid frame")
175 ap.add_argument("--out-name", default=None,
176 help=f"output filename (default: {OUT_NAME}, or {GEL_OUT_NAME} with --gel-frame)")
177 ap.add_argument("--overwrite", action="store_true")
178 args = ap.parse_args()
179
180 out_name = args.out_name or (GEL_OUT_NAME if args.gel_frame else OUT_NAME)
181
182 if args.episodes:
183 parquets = [args.parquet_root / f"{e}.parquet" for e in args.episodes]
184 for p in parquets:
185 if not p.exists():
186 raise FileNotFoundError(p)
187 else:
188 parquets = sorted(args.parquet_root.glob("*.parquet"))
189 if not parquets:
190 raise FileNotFoundError(f"no parquet found under {args.parquet_root}")
191
192 print(f"building {'GEL-frame ' if args.gel_frame else ''}actions for {len(parquets)} "
193 f"episode(s) -> {args.videos_root}/*/{out_name}")
194 n_written = n_skipped = 0
195 total_flagged = 0
196 for parquet in parquets:
197 out_dir = args.videos_root / parquet.stem
198 if not out_dir.is_dir():
199 # A parquet with no video folder means the two trees disagree; that is a
200 # data problem worth stopping for, not something to paper over.
201 raise FileNotFoundError(
202 f"{parquet.name}: no destination folder {out_dir} — parquet and video trees disagree"
203 )
204 if (out_dir / out_name).exists() and not args.overwrite:
205 print(f" [skip] {parquet.stem}: {out_name} exists")
206 n_skipped += 1
207 continue
208
209 info = build_episode(parquet, out_dir, args.max_rot_deg, out_name, args.gel_frame)
210 total_flagged += info["flagged"]
211 print(f" [write] {parquet.stem}: T={info['T']} actions={info['n_actions']}x9 "
212 f"per side ({info['flagged']} flagged)")
213 n_written += 1
214
215 print(f"done ({n_written} written, {n_skipped} skipped, {total_flagged} flagged steps)")
216
217
218 if __name__ == "__main__":
219 main()
220