File size: 7,299 Bytes
b1b6f13
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
#!/usr/bin/env python
"""Batch the sensor-pose -> camera projection over every episode -> <episode>/view_actions.pt.

For each episode this projects both tactile sensors' absolute pose into all three camera views
(see project_pose_to_camera.py for the geometry and its departures from arXiv 2604.06168) and
stores the result on the **6 fps grid** the rest of the pipeline uses.

What is stored is the projected *points*, not the RGB Gaussian "action image". The image is a
deterministic rendering of these three points, so keeping the points (~250 kB/episode instead of
~130 MB) leaves sigma, resolution and the render itself changeable in the dataloader without
rebuilding anything. Use build_action_images.py to materialize images when a run needs them.

These are per-FRAME quantities (an absolute pose), not per-transition, so N = ceil(n_native/5) --
the same length as the tactile latents' num_frames, one more than the fps6 action count.

    python data_preprocessing/build_view_actions.py            # all episodes
    python data_preprocessing/build_view_actions.py --episodes motherboard_0510_episode_004
"""
from __future__ import annotations

import argparse
import sys
from pathlib import Path

import numpy as np
import torch

sys.path.insert(0, str(Path(__file__).resolve().parent))
from project_pose_to_camera import (  # noqa: E402
    PARQUET_ROOT, ROOT, SENSORS, VIEWS, AXIS_LEN_MM, load_calibration, load_episode_poses,
    project, sensor_points, task_for_episode, world_frame_offset,
)

VID = ROOT / "mode1_v2_videos"
OUT_NAME = "view_actions.pt"
POINT_ORDER = ("pos", "normal", "up")


def build_episode(episode: str, stride: int, apply_world_offset: bool = False) -> dict:
    task = task_for_episode(episode)
    cal = load_calibration(task)
    poses = load_episode_poses(episode, apply_world_offset=apply_world_offset)

    n_native = len(poses["left"])
    idx = np.arange(0, n_native, stride)          # 0, 5, 10, ... -- same grid as build_fps6_actions

    pts3d = {}
    for s_ in SENSORS:
        q = np.stack(sensor_points(poses[s_], cal["gel"][s_], AXIS_LEN_MM), axis=1)  # (n,3,3) mocap mm
        pts3d[s_] = q[idx]

    out = {
        "frame_idx": torch.from_numpy(idx.astype(np.int32)),
        "fps": 30.0 / stride, "src_fps": 30.0, "fps_stride": stride,
        "num_frames": len(idx), "native_num_frames": n_native,
        "cameras": {},
        "points_mm": {s_: torch.from_numpy(pts3d[s_].astype(np.float32)) for s_ in SENSORS},
        "pose": {s_: torch.from_numpy(poses[s_][idx].astype(np.float32)) for s_ in SENSORS},
    }

    for v in VIEWS:
        view = cal["views"][v]
        intr = view["intr"]
        W, H = float(intr["width"]), float(intr["height"])

        # (N, sensor, point, 3) with the last axis (u_px, v_px, z_mm).
        # sensor axis = SENSORS order = [left, right]; point axis = [pos, normal, up].
        uvz = np.zeros((len(idx), len(SENSORS), 3, 3), np.float32)
        valid = np.zeros((len(idx), len(SENSORS), 3), bool)
        for si, s_ in enumerate(SENSORS):
            for k in range(3):
                u_, v_, z_, ok_ = project(pts3d[s_][:, k, :], view)
                uvz[:, si, k, 0] = u_
                uvz[:, si, k, 1] = v_
                uvz[:, si, k, 2] = z_
                valid[:, si, k] = ok_

        uvz_norm = uvz.copy()
        uvz_norm[..., 0] /= W          # z is left in mm: there is no natural depth normalizer,
        uvz_norm[..., 1] /= H          # so the two tensors differ only in the u,v channels.

        # `view_` prefix: a bare "left" would collide with the *sensor* key "left" in
        # points_mm/pose, which mean different things. This matches the view_*.mp4 names.
        out[f"view_{v}"] = torch.from_numpy(uvz)
        out[f"view_{v}_norm"] = torch.from_numpy(uvz_norm)
        out[f"view_{v}_valid"] = torch.from_numpy(valid)
        out["cameras"][v] = {"intrinsics": dict(intr),
                             "T_mocap_to_cam": torch.from_numpy(view["T"].copy()),
                             "rmse_mm": view["rmse"],
                             "camera_serial": view["serial"]}

    out["meta"] = {
        "source": "arXiv 2604.06168 action-image points, projected per camera view",
        "view_tensor": "view_{left,middle,right}: (N, sensor, point, 3) = (u_px, v_px, z_mm); "
                       "view_*_norm has u/W, v/H and the SAME z in mm",
        "sensor_axis": list(SENSORS),
        "point_axis": list(POINT_ORDER),
        "valid": "view_*_valid (N, sensor, point) bool = z>0 AND inside the image",
        "axis_len_mm": AXIS_LEN_MM,
        "q_pos": "measured gel centre (T_gel_to_rigid.gel_center_in_rigid_mm), NOT the "
                 "tracked rigid-body origin",
        "q_normal": "measured gel axis (gel_axis_in_rigid) -- a deliberate departure from "
                    "the paper, which rotates a canonical -z because it has no gel calibration",
        "q_up": "rigid-body x, Gram-Schmidt-orthogonalized against the gel normal",
        "chain": "pose(m) x1000 -> +R@gel_center -> T_mocap_to_cam -> fx*X/Z+ppx",
        "task": task, "calibration": cal["meta"].get("calibration_id"),
        "world_frame_offset_applied": bool(apply_world_offset),
        "world_frame_offset_m": list(world_frame_offset(task, episode)),
        "units": "u,v px (or [0,1] in *_norm); z mm; points_mm mm; pose [xyz m, quat xyzw]",
    }
    return out


def main() -> None:
    ap = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter)
    ap.add_argument("--videos-root", type=Path, default=VID)
    ap.add_argument("--episodes", nargs="*", default=None)
    ap.add_argument("--stride", type=int, default=5, help="5 -> 6 fps, matching the rest of the pipeline")
    ap.add_argument("--overwrite", action="store_true")
    ap.add_argument("--apply-world-offset", action="store_true",
                    help="treat episodes.jsonl:world_frame_offset as a PENDING correction and "
                         "add it to the translation (default: off -- it is already applied)")
    ap.add_argument("--out-name", default=OUT_NAME,
                    help="output filename, so both hypotheses can coexist for comparison")
    a = ap.parse_args()

    eps = a.episodes or sorted(
        p.name for p in a.videos_root.iterdir()
        if p.is_dir() and (PARQUET_ROOT / f"{p.name}.parquet").is_file()
    )
    print(f"{len(eps)} episode(s), stride={a.stride} -> {30/a.stride:g} fps\n")

    n_written = 0
    for ep in eps:
        out = a.videos_root / ep / a.out_name
        if out.exists() and not a.overwrite:
            print(f"  [skip ] {ep}: {a.out_name} exists")
            continue
        d = build_episode(ep, a.stride, a.apply_world_offset)
        tmp = out.with_suffix(".pt.tmp")
        torch.save(d, tmp)
        tmp.replace(out)                                   # atomic; a killed run leaves no partial file
        n_written += 1
        frac = {v: float(d[f"view_{v}_valid"][:, :, 0].float().mean()) for v in VIEWS}
        print(f"  [write] {ep:<34} N={d['num_frames']:>5}  in-frame "
              + "  ".join(f"{v}={100*frac[v]:5.1f}%" for v in VIEWS)
              + f"  {out.stat().st_size/1e3:6.0f} kB")
    print(f"\ndone ({n_written} written)")


if __name__ == "__main__":
    main()