preprocessing/data_preprocessing/project_pose_to_camera.py
| 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 | |