preprocessing/data_preprocessing/project_pose_to_camera.py
13.3 KB · 269 lines · python Raw
1 #!/usr/bin/env python
2 """Project the absolute sensor pose into the camera views.
3
4 Implements the geometry behind "action images" (arXiv 2604.06168, *Action Images:
5 End-to-End Policy Learning via Multiview Video Generation*): rather than feeding a policy a
6 low-dimensional action vector, the end-effector pose is projected into each camera view so the
7 action lives in the same 2D pixel space as the observation.
8
9 Three 3D points per sensor per timestep, following the paper:
10
11 q_pos the contact point itself
12 q_normal q_pos + l * (surface normal)
13 q_up q_pos + l * (an axis orthogonal to the normal)
14
15 with ``l = 100 mm`` (the paper's 0.1 m). The paper obtains its two axes by rotating the
16 canonical ``x`` and ``-z`` by the end-effector orientation, because it has no knowledge of the
17 gripper's geometry. **We deliberately depart from that**: this rig is calibrated
18 (``T_gel_to_rigid_*.json`` gives a measured ``gel_axis_in_rigid``), so the normal is the real
19 gel normal and ``q_pos`` is the real gel centre rather than the tracked rigid-body origin. That
20 is better grounded for this hardware, but it is not the paper's literal recipe.
21
22 Projection chain, exactly as ``calibration.json`` documents it:
23
24 sensor_pose (metres, xyzw quat)
25 -> x1000 -> mocap mm
26 -> + R_rigid @ gel_center_in_rigid_mm -> gel centre, mocap mm
27 -> T_mocap_to_cam -> camera mm
28 -> fx*X/Z + ppx, fy*Y/Z + ppy -> pixels
29
30 Verified over all 2406 frames of motherboard_0510_episode_004: 100% of points land in-frame
31 for all three views and both sensors, with Z > 0 throughout.
32
33 **``world_frame_offset`` is deliberately NOT applied here.** ``<task>/episodes.jsonl`` records
34 ``world_frame_offset = [0.23, 0, 0.175]`` m for the five 2026-05-19 episodes (zero for 05-10,
35 05-11 and pushT), and nothing in this repo reads it — which looks exactly like a missed step.
36 It is not. The offset is already baked into the released poses; the field is provenance, not a
37 pending correction. Evidence, reproducible via ``vm_diffusion/scripts/verify_view_projection.py``:
38
39 * The projected gel centre lands in that frame's top-10% optical-flow cells (AllTracker,
40 24x32) on 69-90% of moving 05-19 frames, against a 10% chance level, and against 56-65%
41 for the 05-10/05-11 episodes whose offset is zero. Adding the offset drops 05-19 to
42 24-35%; subtracting it drops every date to 0%.
43 * Overlaid on the middle-view mp4, the as-is projection sits on the operator's contact
44 point in 05-19 episodes; both shifted variants sit on empty table.
45 * Mean sensor position is 0.33 m (05-19) vs 0.29 m (05-10) in x — the same workspace.
46 A pending 0.23 m offset would put them 0.23 m apart, and neither correction closes it.
47 * ``mode1_v2/*.parquet`` poses are bit-identical to ``data/<task>/meta/<date>/*.parquet``,
48 so nothing was silently re-framed during export either.
49
50 ``load_episode_poses`` raises on any NEW date with a nonzero offset, because a genuinely
51 unapplied offset would project 23 cm into empty table and nothing downstream would notice.
52 """
53 from __future__ import annotations
54
55 import json
56 import os
57 import re
58 from pathlib import Path
59
60 import numpy as np
61 import torch
62
63 import sys
64 sys.path.insert(0, str(Path(__file__).resolve().parents[1]))
65
66 from common.actions import quat_to_rotmat # noqa: E402 (xyzw, renormalizes)
67
68 # release layout: preprocessing/ holds data/<task>/calibration; parquets via $ACTS_PARQUET_ROOT
69 ROOT = Path(os.environ.get("ACTS_PREPROC_ROOT", Path(__file__).resolve().parents[1]))
70 CAL_ROOT = ROOT / "data"
71 PARQUET_ROOT = Path(os.environ.get("ACTS_PARQUET_ROOT", ROOT / "mode1_v2"))
72 VIEWS = ("left", "middle", "right")
73 SENSORS = ("left", "right")
74 AXIS_LEN_MM = 100.0 # the paper's l = 0.1 m
75 # (date, offset) pairs whose NON-ZERO world_frame_offset has been verified to be ALREADY
76 # APPLIED in the released poses -- see the module docstring and
77 # vm_diffusion/scripts/verify_view_projection.py. Anything else nonzero raises: an offset
78 # that really were pending would shift the projection 23 cm into empty table, and no
79 # downstream stage checks that.
80 VERIFIED_PREAPPLIED_OFFSETS = {"05-19": (0.23, 0.0, 0.175)}
81 _OFFSET_CACHE: dict[str, dict[str, tuple]] = {}
82
83
84 def episode_world_offsets(task: str) -> dict[str, tuple]:
85 """-> {'MM-DD': (x, y, z)} from <task>/episodes.jsonl, or {} if the manifest is absent."""
86 if task in _OFFSET_CACHE:
87 return _OFFSET_CACHE[task]
88 p = CAL_ROOT / task / "episodes.jsonl"
89 out: dict[str, tuple] = {}
90 if p.is_file():
91 for line in p.read_text().splitlines():
92 if not line.strip():
93 continue
94 r = json.loads(line)
95 off = r.get("world_frame_offset")
96 if off is not None and r.get("date"):
97 out[str(r["date"])[-5:]] = tuple(float(x) for x in off)
98 _OFFSET_CACHE[task] = out
99 return out
100
101
102 def world_frame_offset(task: str, episode: str) -> tuple:
103 """-> the (x, y, z) metres recorded for this episode's date, or zeros."""
104 m = re.search(r"_(\d{2})(\d{2})_", episode)
105 mmdd = f"{m.group(1)}-{m.group(2)}" if m else ""
106 return episode_world_offsets(task).get(mmdd, (0.0, 0.0, 0.0))
107
108
109 def check_world_frame_offset(task: str, episode: str) -> tuple:
110 """Raise if this episode's date carries an unverified nonzero world_frame_offset."""
111 m = re.search(r"_(\d{2})(\d{2})_", episode)
112 mmdd = f"{m.group(1)}-{m.group(2)}" if m else ""
113 off = episode_world_offsets(task).get(mmdd, (0.0, 0.0, 0.0))
114 if any(abs(x) > 1e-9 for x in off) and \
115 not np.allclose(off, VERIFIED_PREAPPLIED_OFFSETS.get(mmdd, (np.nan,) * 3)):
116 raise ValueError(
117 f"{episode}: {task}/episodes.jsonl records world_frame_offset={off} for {mmdd}, "
118 f"which is not in VERIFIED_PREAPPLIED_OFFSETS. On 05-19 that field turned out to "
119 f"be provenance (already applied in the released poses), but that was ESTABLISHED, "
120 f"not assumed — run vm_diffusion/scripts/verify_view_projection.py on this date "
121 f"before adding it, or the projection may be off by {np.linalg.norm(off)*100:.0f} cm.")
122 return off
123
124
125 def task_for_episode(episode: str) -> str:
126 """Episode name -> calibration task dir, resolved via calibration.json:applies_to_dates.
127
128 The date is embedded in the episode name (motherboard_0510_..., pusht_0618_...). Matching on
129 the recorded dates rather than the task prefix means a future recalibration that declares new
130 dates is picked up without editing this file.
131 """
132 m = re.search(r"_(\d{2})(\d{2})_", episode)
133 if not m:
134 raise ValueError(f"cannot parse a MMDD date out of episode name {episode!r}")
135 mmdd = f"{m.group(1)}-{m.group(2)}"
136 for task_dir in sorted(p for p in CAL_ROOT.iterdir() if (p / "calibration" / "calibration.json").is_file()):
137 meta = json.loads((task_dir / "calibration" / "calibration.json").read_text())
138 if any(d.endswith(mmdd) for d in meta.get("applies_to_dates", [])):
139 return task_dir.name
140 raise FileNotFoundError(
141 f"no calibration covers date {mmdd} (from {episode!r}); "
142 f"checked {[p.name for p in CAL_ROOT.iterdir() if p.is_dir()]}"
143 )
144
145
146 def load_calibration(task: str) -> dict:
147 """-> {'views': {view: {T, intr}}, 'gel': {sensor: {...}}, 'meta': calibration.json}."""
148 d = CAL_ROOT / task / "calibration"
149 out = {"views": {}, "gel": {}, "meta": json.loads((d / "calibration.json").read_text())}
150 for v in VIEWS:
151 c = json.loads((d / f"T_mocap_to_cam_{v}.json").read_text())
152 T = np.asarray(c["T_mocap_to_cam"], dtype=np.float64)
153 R = T[:3, :3]
154 if not np.allclose(R @ R.T, np.eye(3), atol=1e-5) or abs(np.linalg.det(R) - 1) > 1e-5:
155 raise ValueError(f"{task}/{v}: T_mocap_to_cam rotation is not a proper rotation")
156 out["views"][v] = {"T": T, "intr": c["intrinsics"], "rmse": c.get("rmse_mm"),
157 "serial": c.get("camera_serial")}
158 for s in SENSORS:
159 g = json.loads((d / f"T_gel_to_rigid_{s}.json").read_text())
160 n = np.asarray(g["gel_axis_in_rigid"], dtype=np.float64)
161 out["gel"][s] = {"center_mm": np.asarray(g["gel_center_in_rigid_mm"], dtype=np.float64),
162 "axis": n / np.linalg.norm(n),
163 "scale": float(g.get("mocap_scale", 1000.0))}
164 return out
165
166
167 def sensor_points(pose: np.ndarray, gel: dict, axis_len_mm: float = AXIS_LEN_MM):
168 """(T,7) pose [xyz m, quat xyzw] + gel calib -> (q_pos, q_normal, q_up), each (T,3) mocap mm.
169
170 ``q_up`` uses the rigid body's x-axis Gram-Schmidt-orthogonalized against the gel normal, so
171 the triad is well defined even when the two are nearly parallel (a raw cross product would be
172 numerically unstable there).
173 """
174 pose = np.asarray(pose, dtype=np.float64)
175 R = quat_to_rotmat(torch.from_numpy(pose[:, 3:])).numpy() # (T,3,3), xyzw
176 p_mm = pose[:, :3] * gel["scale"] # metres -> mocap mm
177
178 q_pos = p_mm + np.einsum("nij,j->ni", R, gel["center_mm"]) # gel centre
179 n_hat = np.einsum("nij,j->ni", R, gel["axis"]) # measured gel normal
180 n_hat /= np.linalg.norm(n_hat, axis=1, keepdims=True)
181
182 x_hat = R[:, :, 0] # rigid-body x
183 u_hat = x_hat - (x_hat * n_hat).sum(1, keepdims=True) * n_hat # orthogonalize vs normal
184 bad = np.linalg.norm(u_hat, axis=1) < 1e-6 # x nearly || normal
185 if bad.any(): # fall back to rigid-body y
186 y_hat = R[bad, :, 1]
187 u_hat[bad] = y_hat - (y_hat * n_hat[bad]).sum(1, keepdims=True) * n_hat[bad]
188 u_hat /= np.linalg.norm(u_hat, axis=1, keepdims=True)
189
190 return q_pos, q_pos + axis_len_mm * n_hat, q_pos + axis_len_mm * u_hat
191
192
193 def project(points_mm: np.ndarray, view: dict):
194 """(N,3) mocap mm -> (u, v, z, valid). `valid` = in front of the camera AND inside the image."""
195 T, intr = view["T"], view["intr"]
196 Xc = points_mm @ T[:3, :3].T + T[:3, 3] # -> camera mm
197 z = Xc[:, 2]
198 safe = np.where(np.abs(z) < 1e-9, 1e-9, z) # never divide by ~0
199 u = intr["fx"] * Xc[:, 0] / safe + intr["ppx"]
200 v = intr["fy"] * Xc[:, 1] / safe + intr["ppy"]
201 valid = (z > 0) & (u >= 0) & (u < intr["width"]) & (v >= 0) & (v < intr["height"])
202 return u, v, z, valid
203
204
205 def load_episode_poses(episode: str, apply_world_offset: bool = False):
206 """-> {'left': (T,7), 'right': (T,7)} absolute poses from the mode1_v2 parquet.
207
208 ``apply_world_offset`` adds ``episodes.jsonl:world_frame_offset`` to the translation, i.e.
209 treats that field as a PENDING correction. It defaults to False because every measurement
210 on this copy of the data says the offset is already applied (see the module docstring);
211 the flag exists so the two hypotheses can be built side by side and compared rather than
212 argued about. Rotation is untouched either way -- a world translation does not rotate the
213 body -- and the tactile action is unaffected in both cases, since dp = R_t^T (p_{t+1}-p_t)
214 cancels any constant translation.
215 """
216 import pyarrow.parquet as pq
217 task = task_for_episode(episode)
218 if apply_world_offset:
219 off = np.asarray(world_frame_offset(task, episode), dtype=np.float64)
220 else:
221 check_world_frame_offset(task, episode)
222 off = np.zeros(3)
223 f = PARQUET_ROOT / f"{episode}.parquet"
224 t = pq.read_table(str(f), columns=[f"sensor_{s}_pose" for s in SENSORS])
225 out = {s: np.asarray(t.column(f"sensor_{s}_pose").to_pylist(), dtype=np.float64)
226 for s in SENSORS}
227 if off.any():
228 for s in SENSORS:
229 out[s] = out[s].copy()
230 out[s][:, :3] += off # translation only; quaternion unchanged
231 return out
232
233
234 def project_episode(episode: str, axis_len_mm: float = AXIS_LEN_MM) -> dict:
235 """Everything a renderer needs: per view, per sensor, the three projected points."""
236 cal = load_calibration(task_for_episode(episode))
237 poses = load_episode_poses(episode)
238 out = {"episode": episode, "calibration": cal["meta"].get("calibration_id"), "views": {}}
239 for v in VIEWS:
240 out["views"][v] = {}
241 for s in SENSORS:
242 pts = sensor_points(poses[s], cal["gel"][s], axis_len_mm)
243 proj = {}
244 for name, q in zip(("pos", "normal", "up"), pts):
245 u, vv, z, ok = project(q, cal["views"][v])
246 proj[name] = {"u": u, "v": vv, "z": z, "valid": ok}
247 out["views"][v][s] = proj
248 out["views"][v]["intr"] = cal["views"][v]["intr"]
249 out["views"][v]["rmse_mm"] = cal["views"][v]["rmse"]
250 return out
251
252
253 if __name__ == "__main__":
254 import argparse
255 ap = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter)
256 ap.add_argument("--episode", default="motherboard_0510_episode_004")
257 a = ap.parse_args()
258
259 r = project_episode(a.episode)
260 print(f"{a.episode} calibration={r['calibration']}")
261 print(f"{'view':<8}{'sensor':<7}{'in-frame':>10}{'u range':>20}{'v range':>20}{'depth mm':>16}")
262 for v in VIEWS:
263 for s in SENSORS:
264 d = r["views"][v][s]["pos"]
265 print(f"{v:<8}{s:<7}{d['valid'].mean()*100:>9.1f}%"
266 f" [{d['u'].min():7.1f},{d['u'].max():7.1f}]"
267 f" [{d['v'].min():7.1f},{d['v'].max():7.1f}]"
268 f" [{d['z'].min():6.0f},{d['z'].max():6.0f}]")
269