mirror of
https://github.com/ruvnet/RuView
synced 2026-07-05 14:33:19 +00:00
ADR-152: WiFi-Pose SOTA 2026 intake — WiFlow-STD benchmark, Rust integrations, ADR-153 802.11bf layer, efficiency frontier (#1008)
* feat(calibration): NodeGeometry transceiver-geometry recording (ADR-152 §2.1.1) PerceptAlign-motivated geometry capture at enrollment: per-node optional records (position, antenna orientation, inter-node distances, acquisition method) — recorded when known, never required. Event-sourced via EnrollmentEvent::GeometryRecorded (latest recording wins); persisted on SpecialistBank with serde defaults so pre-ADR-152 bank JSON loads cleanly (fixture-proven, and geometry-free banks serialize byte-shape-identical to the old schema); threaded through MultiNodeMixture as data only — the learned geometry embeddings and algorithmic fusion use are §2.1.2, deliberately deferred until the ADR-151 P6 LoRA heads exist. Geometry recorded from now on means banks captured today remain usable for layout-conditioned training later — you can't retroactively add geometry to data you didn't record. 8 new tests (3 geometry, 2 anchor, 2 bank, 1 multistatic) + full-loop extension (2-node geometry, one tape-measured + one unknown, surviving the bank JSON round-trip the runtime loads from). 50/50 calibration (both feature configs) + 23 CLI tests green. Co-Authored-By: RuFlo <ruv@ruv.net> * feat(training): two-checkerboard camera↔room calibration for ADR-079 labels (ADR-152 §2.1.3) Defends the camera-supervised pipeline against PerceptAlign's "coordinate overfitting": MediaPipe keypoints were emitted in raw camera coordinates with no shared frame and no transceiver-geometry metadata — the exact label shape that memorizes deployment layout and collapses cross-layout. - scripts/calibrate-camera-room.py + calibration_lib.py: OpenCV two-checkerboard calibration → versioned bundle JSON (intrinsics, camera→room extrinsics, checkerboard spec, transceiver geometry, sha256 calibration_id). Intrinsics resolve from file > cache > multi-view computation > loud-warning 2-view fallback. - collect-ground-truth.py --calibration <bundle>: every sample gains keypoints_room (unit bearing rays from the camera center in the room frame — documented projective alignment; raw image coords preserved so training chooses), camera_origin_room, calibration_id, and the transceiver geometry stamp. Without the flag, output is byte-identical to before (tested) + a one-line ADR-152 warning. Design finding (recorded for ADR-152): a single planar checkerboard's corner grid is centrosymmetric — the reversed corner ordering fits a ghost camera pose with IDENTICAL reprojection error, so per-board flip disambiguation is mathematically ill-posed. solve_two_board_extrinsics solves the joint wall+floor set over all 4 flip combinations, where the minimum is unique — an independent reason the TWO-checkerboard method is required, beyond what PerceptAlign states. 15 headless pytest tests green (synthetic corners: extrinsics recovery incl. ghost resolution, bundle round-trip + hash stability, ray transforms w/ distortion + cross-resolution, no-calibration byte identity). Co-Authored-By: RuFlo <ruv@ruv.net> * feat(benchmarks): WiFlow-STD reproduction harness + measurement (a) results (ADR-152 §2.2) Shipped checkpoint REFUTED (0.08% PCK@20, wrong keypoint normalization); 6 reproducibility defects documented (broken imports, corrupted dataset tail with float32-max garbage that NaN-poisons fp16 BatchNorm, unreachable test phase). After repairs, retraining with upstream defaults reproduces 96.09% PCK@20 full-test / 96.61% corruption-free (published 97.25%) on RTX 5080. Claims graded MEASURED-EQUIVALENT; 2.23M params + ~0.055 GFLOPs verified. Third-party code/weights/data stay out of tree (gitignored). Co-Authored-By: claude-flow <ruv@ruv.net> * feat: ADR-152 Rust integrations + ADR-153 802.11bf protocol model - calibration: GeometryEmbedding — 32-slot permutation-invariant NodeGeometry featurization for future LoRA-head conditioning (ADR-152 §2.1.2); derived SpecialistBank::geometry_embedding() accessor; 59 tests - train: MaePretrainConfig + patchify/random-mask with UNSW measured recipe (80% masking, (30,3) patches; ADR-152 §2.3, arXiv 2511.18792); strict no-truncate/no-NaN policy; proptest properties - train: WiFlowStdModel — tch-gated port of the verified ~96%-PCK@20 WiFlow-STD architecture (ADR-152 §2.2 beyond-SOTA); ungated param formula pinned to 2,225,042; 15/17-keypoint support; 239 crate tests - hardware: ieee80211bf forward-compatibility protocol model (ADR-153): SpecProfile gates, SensingCapabilities negotiation, required ConsentMode, session FSM, SensingTransport + SimTransport + OpportunisticCsiBridge; full acceptance checklist covered; 156+4 tests - deps: ruvector bumps per ADR-152 §2.6 survey (mincut/solver 2.0.6, attention 2.1.0, gnn 2.2.0); vendor/ruvector synced to a083bd77f - docs: ADR-153 accepted; ADR-152 §2.2 status, §2.4 amendment, §2.6 added Workspace: 162 test suites green (--no-default-features); Python proof PASS. Known pre-existing flake: homecore-api env_empty_falls_back_to_defaults (unserialized env-var mutation) — untouched, follow-up. Co-Authored-By: claude-flow <ruv@ruv.net> * docs: CHANGELOG + CLAUDE.md entries for ADR-152 integrations and ADR-153 Co-Authored-By: claude-flow <ruv@ruv.net> * fix(train): repair tch-backend bit-rot — gated path compiles and tests run again Mechanical API refresh against current tch: Vec::from(Tensor) -> try_from (+ explicit flatten), numel() usize cast, Rem/div ops -> remainder() / divide_scalar_mode(floor) — the latter fixed a silent true-division bug in heatmap argmax decoding; clamp(1.0, f64::MAX) -> clamp_min (torch 2.x scalar overflow panic); petgraph EdgeRef import; missing EvalMetrics and verify_checkpoint_dir APIs that tests documented. wiflow_std roundtrip test uses safetensors (.pt _save_parameters roundtrip broken in torch 2.11 Windows). Gated: 349 passed (incl. all 20 wiflow_std); ungated: unchanged. Known pre-existing: gaussian-heatmap convention mismatch (2 tests), proof seed race under parallel threads — documented, deliberate follow-ups. Co-Authored-By: claude-flow <ruv@ruv.net> * feat(train): WiFlow-STD PyTorch->tch weight import + numerical parity proof export_to_safetensors.py maps the retrained checkpoint (295 tensors -> 248 mapped, param sum exactly 2,225,042; num_batches_tracked dropped) into a tch-loadable safetensors plus a deterministic parity fixture. Gated #[ignore] integration test loads it strictly and asserts forward-pass agreement: max abs diff 1.192e-7 on the seed-42 fixture. dump_variable_names test makes the tch name layout authoritative. Zero architecture discrepancies found. Co-Authored-By: claude-flow <ruv@ruv.net> * fix: workflow-review findings — BN gamma init, ThresholdParams serde, init docs Concurrent validation workflow (2 review lanes + adversarial verification, 13 agents): 5 confirmed findings, 3 refuted. Fixes: - wiflow_std: pin BatchNorm gamma to 1.0 (tch default draws Uniform(0,1) — silently halves activations in from-scratch training; loaded checkpoints unaffected, parity re-verified after the change) - wiflow_std: document the conv-init divergences vs the reference's effective kaiming_normal(fan_out) re-init (from-scratch dynamics only) - ieee80211bf: ThresholdParams deserialization validates via try_from so the <=100 invariant holds for untrusted payloads (+ rejection test) Benchmarks (release, ruvzen): GeometryEmbedding 1.84us/call (542k/s), MAE tokenization 7.38us/window (135k/s), 802.11bf FSM 8.9M events/s — nothing suspicious. Co-Authored-By: claude-flow <ruv@ruv.net> * docs(adr): ADR-152 §2.1.4 gate resolved — PerceptAlign repo MIT, dataset on HF Co-Authored-By: claude-flow <ruv@ruv.net> * feat(benchmarks): edge optimization measured + measurement (b) blocked + 92.9% retraction Edge optimization (ADR-152 optimize track): ONNX Runtime fp32 is the CPU latency win (3.2 ms/window, ~3.4x faster than torch, parity 2.4e-7); ORT dynamic int8 reaches 2.44 MB (paper's ~2.2 MB claim plausible only via conv-capable toolchains; -0.16pt PCK@20, +18% MPJPE, 2x slower); torch dynamic quant converts 0% of this conv-only model; fp16 halves storage free but is slower on CPU. Measurement (b) BLOCKED-ON-DATA: only 1,077 paired ESP32 windows exist (stop rule <2k). Forensic recheck of the surviving April holdout RETRACTS the ADR-079 '92.9% PCK@20' figure: constant-output model, absolute (not torso) threshold, 69 near-static frames — mean predictor scores 100% under that protocol; torso-PCK@20 is 19.1%. Corroborates PR #535. Stale citations removed from user-guide, readme-details, ADR-152 §2.1.3; no-citation rule extended to ADR-079 accuracy claims. Unblock: >=2k-window multi-pose paired session + torso-PCK re-baseline. Co-Authored-By: claude-flow <ruv@ruv.net> * docs(user-guide): corrected camera-supervised collection tutorial Step 0 CSI-rate check + session-length math (window yield = frames/20 — the May session's 8x under-delivery was a ~12 Hz CSI rate, not an aligner bug); two-checkerboard calibration step (ADR-152 §2.1.3); pose-variety and confidence guidance; torso-normalized PCK + temporal-split + pred-variance eval protocol (lessons from the 92.9% retraction); scale presets re-keyed to realistic window counts. Co-Authored-By: claude-flow <ruv@ruv.net> * feat(benchmarks): static PTQ int8 (calibrated) results + overnight capture script Conv-only static QDQ beats dynamic int8 on accuracy (PCK@20 96.61-96.63% vs 96.52%, MPJPE +10% vs +18% over fp32) at ~equal size/latency; all-ops QDQ strictly worse (int8 activations through attention glue). Entropy calibration verified bit-identical to MinMax on this data. Deployment: ONNX fp32 for speed (3.2ms), static conv-only QDQ for smallest (2.53MB). Also: scripts/overnight-empty-capture.py — segmented UDP CSI recorder for empty-room baselines (no glob collisions, detach-safe). Co-Authored-By: claude-flow <ruv@ruv.net> * feat(benchmarks): measurement (b) MEASURED — optimization transfer only, mean-pose baseline wins WiFlow-STD fine-tuned on 2,046 fresh single-room ESP32 paired windows (temporal 70/15/15, 70->540 adapter, K=17): pretrained-init 65% PCK@20 vs scratch 0% (optimization transfer) but frozen-trunk ~0% (no feature transfer), and NOTHING beats the mean-pose baseline (95.9% PCK@20 — single subject, near-static normalized coords). Honesty gates held: pred std 0.0113 (non-constant model) but mean-baseline dominance means no citable CSI->pose capability from this data. ADR-152 open question 1 answered partially; definitive answer needs multi-subject/position data. Two new aligner findings: heterogeneous csi_shape with silent zero-padding (~20%), and extractCsiMatrix's transposed shape label (frame-major data, [nSc, nFrames] label) — fixes pending. Co-Authored-By: claude-flow <ruv@ruv.net> * feat(benchmarks): efficiency sweep MEASURED — half model dominates full reference Compact WiFlow-STD variants on the same data/split/protocol: half (843,834 params, 0.38x) strictly dominates the 2.23M reference (PCK@20 96.62 vs 96.61, PCK@50 99.47 vs 99.11, MPJPE 0.00898 vs 0.0094) — the published architecture is over-parameterized for its own benchmark. quarter (338k) 96.05%; tiny (56,290 params, 1/39.5) holds 94.11% — a ~220KB fp32 edge candidate. In-domain caveats recorded; cross-domain untested. Co-Authored-By: claude-flow <ruv@ruv.net> * feat(train): compact WiFlow-STD presets in Rust + tiny edge artifact (ADR-152) WiFlowStdConfig gains half()/quarter()/tiny() mirroring the overnight sweep exactly: TcnGroupsMode (Fixed/Gcd/Depthwise), input_pw_groups, derived stride schedule and decoder-mid (all default to upstream behavior; legacy serde JSON unaffected). Param formulas pin to trained ground truth first try: 843,834 / 338,600 / 56,290; default 2,225,042 pin and 1.192e-7 parity unchanged. 248 tests green. Tiny edge artifact (tiny_edge_bench.py): ONNX fp32 = 295 KB, 0.66 ms/win (~1,500/s CPU), 94.11% PCK@20 (matches sweep clean-test exactly; parity 1.49e-7). Static int8 is a bad trade at this scale (-1.43pt, +19% MPJPE, -16% size, slower) — recorded as negative result. Export note: width-16 breaks AdaptiveAvgPool((15,1)) TorchScript export; replaced by exact mean+matmul equivalent, proven by parity. Co-Authored-By: claude-flow <ruv@ruv.net> * fix: resolve all 10 confirmed code-review findings (7-angle review, 20/20 verified) wiflow_std: min_feature_width (default 15) replaces the keypoints->stride coupling — for_keypoints(17) now provably builds the trained [2,2,2,2] graph and pools 15->17, matching the validated Python protocol (pinned by tests); param_count() total on invalid configs; random_mask returns Result and rejects non-finite/out-of-range ratios; trainer checkpoints switched to safetensors (.pt VarStore roundtrip broken on Windows torch 2.11). ieee80211bf: SBP proxy now re-triggers instances and relays reports via Action::RelaySbpReport -> SensingFrame::SbpReport (clients consume via their existing path); missed_instances reset on success = consecutive semantics; SessionTable gains a guarded SBP entry point + unknown-id drop counter; initiator-role sessions reject inbound setup/SBP requests (RejectedNotSupported) closing the idle hijack; StartSetup/StartSbp outside Idle return InvalidStateForCommand; SBP validation unified through evaluate_setup with a 1:1 SetupStatus->SbpStatus mapping. events.rs split out to honor the 500-line cap. calibration/cli: enrollment geometry now actually reaches trained banks — both production call sites attach .with_geometry; --geometry flag on train-room and POST /enroll/geometry + train-body geometry on calibrate-serve give production a recording surface; geometry-free banks log the ADR-152 §2.1.2 note. benchmarks: corruption masks committed as ground truth (unregenerable after in-place cleaning; verified bit-identical regeneration from the pristine copy) + generate_corruption_masks.py producer; _bench_common.py dedups the 5x-copied shim/evaluate/seed/remap (post-refactor PCK@20 re-verified equal to the last digit); remote scripts get the mmap patch; tiny_edge --calib validated multiple-of-64; onnx_bench --help no longer executes (and overwrote) the export — artifact restored byte-exact. Workspace: 2,963 tests passed, 0 failed; Python proof PASS. Co-Authored-By: claude-flow <ruv@ruv.net> * ci: build workspace tests without debuginfo — runner disk exhaustion The combined 38-crate debug target exceeds the GitHub runner's disk ('final link failed: No space left on device'); the same tree measured 151GB locally with full debuginfo. CARGO_PROFILE_{DEV,TEST}_DEBUG=0 shrinks the target ~5-10x; debuginfo serves no purpose in CI test runs. Co-Authored-By: claude-flow <ruv@ruv.net>
This commit is contained in:
rUv
@@ -0,0 +1,300 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Two-checkerboard camera-room calibration for WiFi pose training (ADR-152 S2.1.3).
|
||||
|
||||
Aligns the ADR-079 ground-truth camera and the ESP32 WiFi transceivers in
|
||||
one shared 3D room frame -- the PerceptAlign (arXiv 2601.12252) defense
|
||||
against "coordinate overfitting", where CSI-to-camera-coordinate regression
|
||||
memorizes the deployment layout and collapses cross-layout.
|
||||
|
||||
Procedure (<5 minutes):
|
||||
1. Print a checkerboard (default 9x6 inner corners, 25 mm squares).
|
||||
2. Tape one board flat on the ORIGIN WALL, tape-measure its top-left inner
|
||||
corner position in room coordinates (+x along wall, +y into room, +z up).
|
||||
3. Lay the second board flat on the FLOOR, measure its near-left inner corner.
|
||||
4. With the collection camera in its final position, photograph each board.
|
||||
5. Run this script; tape-measure each ESP32 node position when prompted
|
||||
(or pass --geometry nodes.json).
|
||||
|
||||
Output: a calibration bundle JSON consumed by
|
||||
scripts/collect-ground-truth.py --calibration <bundle.json>
|
||||
|
||||
Usage:
|
||||
python scripts/calibrate-camera-room.py \\
|
||||
--wall-image photos/wall.jpg --wall-origin 0.50,0.0,1.60 \\
|
||||
--floor-image photos/floor.jpg --floor-origin 1.00,1.00,0.0 \\
|
||||
--calib-images "photos/intrinsics/*.jpg" \\
|
||||
--geometry config/transceivers.json \\
|
||||
--output data/calibration/camera-room.json
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import glob
|
||||
import json
|
||||
import sys
|
||||
from datetime import datetime
|
||||
from pathlib import Path
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
|
||||
sys.path.insert(0, str(Path(__file__).resolve().parent))
|
||||
import calibration_lib as cal # noqa: E402
|
||||
|
||||
INTRINSICS_CACHE = Path("data") / ".cache" / "camera_intrinsics.json"
|
||||
|
||||
|
||||
def parse_vec3(text: str) -> np.ndarray:
|
||||
parts = [float(p) for p in text.replace(",", " ").split()]
|
||||
if len(parts) != 3:
|
||||
raise argparse.ArgumentTypeError(f"Expected 3 comma-separated numbers, got {text!r}")
|
||||
return np.array(parts, dtype=np.float64)
|
||||
|
||||
|
||||
def detect_corners(image_path: Path, cols: int, rows: int) -> tuple[np.ndarray, tuple[int, int]]:
|
||||
image = cv2.imread(str(image_path))
|
||||
if image is None:
|
||||
print(f"ERROR: Cannot read image {image_path}", file=sys.stderr)
|
||||
sys.exit(1)
|
||||
corners = cal.find_board_corners(image, cols, rows)
|
||||
if corners is None:
|
||||
print(
|
||||
f"ERROR: No {cols}x{rows} checkerboard found in {image_path}. "
|
||||
"Check lighting, focus, and the --board-cols/--board-rows flags.",
|
||||
file=sys.stderr,
|
||||
)
|
||||
sys.exit(1)
|
||||
h, w = image.shape[:2]
|
||||
return corners, (w, h)
|
||||
|
||||
|
||||
def resolve_intrinsics(args, repo_root: Path, board_args: tuple[int, int, float]) -> dict:
|
||||
"""Pre-computed file > cached > computed from --calib-images >
|
||||
last-resort 2-view estimate from the wall+floor photos themselves."""
|
||||
cols, rows, square_m = board_args
|
||||
|
||||
if args.intrinsics:
|
||||
print(f"Intrinsics: loading {args.intrinsics}")
|
||||
return cal.load_intrinsics(Path(args.intrinsics))
|
||||
|
||||
cache_path = repo_root / INTRINSICS_CACHE
|
||||
if cache_path.exists() and not args.recalibrate_intrinsics:
|
||||
print(f"Intrinsics: using cached {cache_path} (pass --recalibrate-intrinsics to redo)")
|
||||
intr = cal.load_intrinsics(cache_path)
|
||||
intr["source"] = "cached"
|
||||
return intr
|
||||
|
||||
if args.calib_images:
|
||||
paths = sorted(glob.glob(args.calib_images))
|
||||
if len(paths) < 3:
|
||||
print(
|
||||
f"ERROR: --calib-images matched only {len(paths)} file(s); "
|
||||
"need >= 3 checkerboard views for stable intrinsics.",
|
||||
file=sys.stderr,
|
||||
)
|
||||
sys.exit(1)
|
||||
corner_sets, image_size = [], None
|
||||
for p in paths:
|
||||
corners, size = detect_corners(Path(p), cols, rows)
|
||||
if image_size is None:
|
||||
image_size = size
|
||||
elif size != image_size:
|
||||
print(f"ERROR: {p} has size {size}, expected {image_size}.", file=sys.stderr)
|
||||
sys.exit(1)
|
||||
corner_sets.append(corners)
|
||||
print(f" corners found: {p}")
|
||||
intr = cal.compute_intrinsics(corner_sets, image_size, cols, rows, square_m)
|
||||
print(f"Intrinsics: computed from {len(paths)} views, "
|
||||
f"reprojection RMS {intr['reprojection_error_px']:.3f} px")
|
||||
cal.save_bundle(intr, cache_path) # plain JSON write; reused on next run
|
||||
print(f" cached to {cache_path}")
|
||||
return intr
|
||||
|
||||
# Last resort: 2-view calibration from the extrinsic photos. Workable but
|
||||
# weak -- warn loudly and recommend a proper multi-view pass.
|
||||
print(
|
||||
"WARNING: no --intrinsics / cache / --calib-images; estimating intrinsics "
|
||||
"from the wall+floor photos alone (2 views, low quality). Prefer "
|
||||
"--calib-images with 5-10 varied board views.",
|
||||
file=sys.stderr,
|
||||
)
|
||||
corner_sets, image_size = [], None
|
||||
for p in (args.wall_image, args.floor_image):
|
||||
corners, size = detect_corners(Path(p), cols, rows)
|
||||
image_size = image_size or size
|
||||
corner_sets.append(corners)
|
||||
intr = cal.compute_intrinsics(corner_sets, image_size, cols, rows, square_m)
|
||||
intr["source"] = "two-view-fallback"
|
||||
return intr
|
||||
|
||||
|
||||
def prompt_transceiver_geometry() -> dict:
|
||||
"""Tape-measure entry of ESP32 node positions in room coordinates."""
|
||||
print()
|
||||
print("Transceiver geometry -- enter one node per line:")
|
||||
print(" <node-id> <x> <y> <z> [yaw_deg] (meters, room frame; blank line to finish)")
|
||||
print(" example: esp32-s3-a 0.10 2.40 1.10 180")
|
||||
nodes = []
|
||||
while True:
|
||||
try:
|
||||
line = input("node> ").strip()
|
||||
except EOFError:
|
||||
break
|
||||
if not line:
|
||||
break
|
||||
parts = line.split()
|
||||
if len(parts) not in (4, 5):
|
||||
print(" expected: <node-id> <x> <y> <z> [yaw_deg]", file=sys.stderr)
|
||||
continue
|
||||
try:
|
||||
node = {"id": parts[0], "position_m": [float(parts[1]), float(parts[2]), float(parts[3])]}
|
||||
if len(parts) == 5:
|
||||
node["antenna_yaw_deg"] = float(parts[4])
|
||||
except ValueError:
|
||||
print(" positions must be numeric", file=sys.stderr)
|
||||
continue
|
||||
nodes.append(node)
|
||||
if not nodes:
|
||||
print("WARNING: no transceiver nodes entered; bundle will carry empty geometry.",
|
||||
file=sys.stderr)
|
||||
return {"nodes": nodes, "units": "meters", "source": "tape-measure-prompt"}
|
||||
|
||||
|
||||
def load_geometry_file(path: Path) -> dict:
|
||||
with open(path, "r", encoding="utf-8") as f:
|
||||
data = json.load(f)
|
||||
nodes = data.get("nodes", data if isinstance(data, list) else None)
|
||||
if nodes is None:
|
||||
raise ValueError(f"{path}: expected {{'nodes': [...]}} or a top-level list")
|
||||
for node in nodes:
|
||||
if "id" not in node or "position_m" not in node:
|
||||
raise ValueError(f"{path}: each node needs 'id' and 'position_m' [x,y,z]")
|
||||
return {"nodes": nodes, "units": "meters", "source": "file"}
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(
|
||||
description="Two-checkerboard camera-room calibration (ADR-152 S2.1.3 / ADR-079)."
|
||||
)
|
||||
parser.add_argument("--wall-image", required=True,
|
||||
help="Photo of the checkerboard on the origin wall")
|
||||
parser.add_argument("--floor-image", required=True,
|
||||
help="Photo of the checkerboard on the floor (camera NOT moved)")
|
||||
parser.add_argument("--wall-origin", type=parse_vec3, default="0.5,0.0,1.6",
|
||||
help="Room xyz (m) of the wall board's first inner corner "
|
||||
"(default: 0.5,0.0,1.6)")
|
||||
parser.add_argument("--floor-origin", type=parse_vec3, default="1.0,1.0,0.0",
|
||||
help="Room xyz (m) of the floor board's first inner corner "
|
||||
"(default: 1.0,1.0,0.0)")
|
||||
parser.add_argument("--wall-axes", default="+x,-z",
|
||||
help="Wall board column,row directions in room frame (default: +x,-z)")
|
||||
parser.add_argument("--floor-axes", default="+x,+y",
|
||||
help="Floor board column,row directions in room frame (default: +x,+y)")
|
||||
parser.add_argument("--board-cols", type=int, default=cal.DEFAULT_BOARD_COLS,
|
||||
help=f"Inner corners per row (default: {cal.DEFAULT_BOARD_COLS})")
|
||||
parser.add_argument("--board-rows", type=int, default=cal.DEFAULT_BOARD_ROWS,
|
||||
help=f"Inner corners per column (default: {cal.DEFAULT_BOARD_ROWS})")
|
||||
parser.add_argument("--square-size-mm", type=float, default=cal.DEFAULT_SQUARE_SIZE_MM,
|
||||
help=f"Checkerboard square size in mm (default: {cal.DEFAULT_SQUARE_SIZE_MM})")
|
||||
parser.add_argument("--intrinsics", help="Pre-computed intrinsics JSON (skips computation)")
|
||||
parser.add_argument("--calib-images",
|
||||
help="Glob of >=3 checkerboard photos for intrinsics computation")
|
||||
parser.add_argument("--recalibrate-intrinsics", action="store_true",
|
||||
help="Ignore the cached intrinsics and recompute")
|
||||
parser.add_argument("--geometry",
|
||||
help="Transceiver geometry JSON ({nodes:[{id,position_m,[antenna_yaw_deg]}]}); "
|
||||
"omit to be prompted for tape-measure entry")
|
||||
parser.add_argument("--output", default=None,
|
||||
help="Bundle output path (default: data/calibration/camera-room-<ts>.json)")
|
||||
args = parser.parse_args()
|
||||
|
||||
if isinstance(args.wall_origin, str):
|
||||
args.wall_origin = parse_vec3(args.wall_origin)
|
||||
if isinstance(args.floor_origin, str):
|
||||
args.floor_origin = parse_vec3(args.floor_origin)
|
||||
|
||||
repo_root = Path(__file__).resolve().parent.parent
|
||||
cols, rows = args.board_cols, args.board_rows
|
||||
square_m = args.square_size_mm / 1000.0
|
||||
|
||||
# --- Intrinsics ---
|
||||
intrinsics = resolve_intrinsics(args, repo_root, (cols, rows, square_m))
|
||||
camera_matrix = np.asarray(intrinsics["camera_matrix"], dtype=np.float64)
|
||||
dist_coeffs = np.asarray(intrinsics["dist_coeffs"], dtype=np.float64)
|
||||
|
||||
# --- Corner detection on the two placed boards ---
|
||||
wall_corners, wall_size = detect_corners(Path(args.wall_image), cols, rows)
|
||||
floor_corners, floor_size = detect_corners(Path(args.floor_image), cols, rows)
|
||||
if wall_size != floor_size:
|
||||
print(f"ERROR: wall image {wall_size} and floor image {floor_size} differ in size; "
|
||||
"both must come from the fixed collection camera.", file=sys.stderr)
|
||||
sys.exit(1)
|
||||
print(f"Corners detected: wall + floor boards ({cols}x{rows}, {args.square_size_mm} mm)")
|
||||
|
||||
# Re-scale intrinsics if they were computed at a different resolution
|
||||
# than the extrinsic photos (the bundle always stores K at wall_size).
|
||||
intr_size = tuple(intrinsics["image_size"])
|
||||
if intr_size != wall_size:
|
||||
sx, sy = wall_size[0] / intr_size[0], wall_size[1] / intr_size[1]
|
||||
camera_matrix[0, 0] *= sx
|
||||
camera_matrix[0, 2] *= sx
|
||||
camera_matrix[1, 1] *= sy
|
||||
camera_matrix[1, 2] *= sy
|
||||
print(f" intrinsics scaled {intr_size} -> {wall_size}")
|
||||
intrinsics = {**intrinsics, "camera_matrix": camera_matrix.tolist(),
|
||||
"image_size": list(wall_size)}
|
||||
|
||||
# --- Room-frame corner positions from the measured placements ---
|
||||
wall_u, wall_v = (cal.parse_axis(t) for t in args.wall_axes.split(","))
|
||||
floor_u, floor_v = (cal.parse_axis(t) for t in args.floor_axes.split(","))
|
||||
wall_room = cal.board_room_points(cols, rows, square_m, args.wall_origin, wall_u, wall_v)
|
||||
floor_room = cal.board_room_points(cols, rows, square_m, args.floor_origin, floor_u, floor_v)
|
||||
|
||||
# --- Extrinsics: joint two-board solve (resolves per-board corner-order
|
||||
# ambiguity -- a single planar board is centrosymmetric; the pair is not) ---
|
||||
extrinsics = cal.solve_two_board_extrinsics(
|
||||
wall_room, wall_corners, floor_room, floor_corners, camera_matrix, dist_coeffs
|
||||
)
|
||||
wall_rmse = extrinsics["per_board"]["wall"]["rmse_px"]
|
||||
floor_rmse = extrinsics["per_board"]["floor"]["rmse_px"]
|
||||
print(f" joint solve: RMSE {extrinsics['rmse_px']:.3f} px "
|
||||
f"(wall {wall_rmse:.3f} / floor {floor_rmse:.3f})")
|
||||
print(f" camera at room {np.round(extrinsics['translation_m'], 3).tolist()} m")
|
||||
if max(wall_rmse, floor_rmse) > 3.0:
|
||||
print(
|
||||
"WARNING: high per-board reprojection error -- re-check the measured "
|
||||
"board origins/axes and that the camera did not move between photos.",
|
||||
file=sys.stderr,
|
||||
)
|
||||
|
||||
# --- Transceiver geometry ---
|
||||
if args.geometry:
|
||||
geometry = load_geometry_file(Path(args.geometry))
|
||||
print(f"Transceiver geometry: {len(geometry['nodes'])} node(s) from {args.geometry}")
|
||||
else:
|
||||
geometry = prompt_transceiver_geometry()
|
||||
|
||||
# --- Bundle ---
|
||||
bundle = cal.make_bundle(
|
||||
camera_intrinsics=intrinsics,
|
||||
camera_to_room_extrinsics=extrinsics,
|
||||
checkerboard_spec={"cols": cols, "rows": rows, "square_size_mm": args.square_size_mm},
|
||||
transceiver_geometry=geometry,
|
||||
)
|
||||
if args.output:
|
||||
out_path = Path(args.output)
|
||||
else:
|
||||
ts = datetime.now().strftime("%Y%m%d_%H%M%S")
|
||||
out_path = repo_root / "data" / "calibration" / f"camera-room-{ts}.json"
|
||||
cal.save_bundle(bundle, out_path)
|
||||
|
||||
print()
|
||||
print("=== Calibration bundle written ===")
|
||||
print(f" path: {out_path}")
|
||||
print(f" calibration_id: {cal.calibration_id(bundle)}")
|
||||
print(f" next: python scripts/collect-ground-truth.py --calibration {out_path}")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,416 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Camera-room calibration library for WiFi pose ground truth (ADR-152 S2.1.3).
|
||||
|
||||
Implements the PerceptAlign-style two-checkerboard alignment adopted in
|
||||
ADR-152 S2.1.3 to defend the ADR-079 camera-supervised pipeline against
|
||||
"coordinate overfitting" (arXiv 2601.12252, MobiCom'26): models regressing
|
||||
CSI to raw camera-frame coordinates memorize the deployment layout and
|
||||
collapse cross-layout. The fix is to express camera AND WiFi transceivers
|
||||
in one shared 3D room frame, and stamp every training label with the
|
||||
calibration + transceiver geometry that produced it.
|
||||
|
||||
Used by:
|
||||
scripts/calibrate-camera-room.py (produces the calibration bundle)
|
||||
scripts/collect-ground-truth.py (consumes it via --calibration)
|
||||
|
||||
Room frame convention (right-handed, meters):
|
||||
origin = a designated wall/floor corner of the room
|
||||
+x = along the origin wall
|
||||
+y = into the room (away from the origin wall)
|
||||
+z = up
|
||||
|
||||
No-depth limitation (IMPORTANT): a single 2D camera keypoint constrains
|
||||
only a *ray* in the room frame, not a 3D point. The transform helpers here
|
||||
therefore return unit bearing rays from the camera center -- a projective
|
||||
alignment. Consumers that need metric 3D points must supply a depth
|
||||
assumption downstream (floor-plane intersection, known subject height,
|
||||
multi-view triangulation, ...). Raw image coordinates are always preserved
|
||||
alongside the room-frame rays so training can choose either representation.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import hashlib
|
||||
import json
|
||||
from datetime import datetime, timezone
|
||||
from pathlib import Path
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
|
||||
BUNDLE_SCHEMA_VERSION = 1
|
||||
BUNDLE_METHOD = "two-checkerboard"
|
||||
|
||||
# Default checkerboard: 9x6 inner corners, 25 mm squares (a common print).
|
||||
DEFAULT_BOARD_COLS = 9
|
||||
DEFAULT_BOARD_ROWS = 6
|
||||
DEFAULT_SQUARE_SIZE_MM = 25.0
|
||||
|
||||
_AXIS_TOKENS = {
|
||||
"+x": (1.0, 0.0, 0.0), "-x": (-1.0, 0.0, 0.0),
|
||||
"+y": (0.0, 1.0, 0.0), "-y": (0.0, -1.0, 0.0),
|
||||
"+z": (0.0, 0.0, 1.0), "-z": (0.0, 0.0, -1.0),
|
||||
}
|
||||
|
||||
|
||||
def parse_axis(token: str) -> np.ndarray:
|
||||
"""Parse an axis token like '+x' or '-z' into a room-frame unit vector."""
|
||||
key = token.strip().lower()
|
||||
if key in _AXIS_TOKENS:
|
||||
return np.array(_AXIS_TOKENS[key], dtype=np.float64)
|
||||
raise ValueError(f"Invalid axis token {token!r}; expected one of {sorted(_AXIS_TOKENS)}")
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Checkerboard geometry
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def board_object_points(cols: int, rows: int, square_size_m: float) -> np.ndarray:
|
||||
"""Inner-corner positions in the board's own frame (z=0 plane), row-major.
|
||||
|
||||
Matches the corner ordering of cv2.findChessboardCorners for a
|
||||
(cols, rows) pattern: cols varies fastest.
|
||||
"""
|
||||
pts = np.zeros((rows * cols, 3), dtype=np.float64)
|
||||
grid = np.mgrid[0:cols, 0:rows].T.reshape(-1, 2) # (rows*cols, 2), cols fastest
|
||||
pts[:, :2] = grid * square_size_m
|
||||
return pts
|
||||
|
||||
|
||||
def board_room_points(
|
||||
cols: int,
|
||||
rows: int,
|
||||
square_size_m: float,
|
||||
origin: np.ndarray,
|
||||
u_axis: np.ndarray,
|
||||
v_axis: np.ndarray,
|
||||
) -> np.ndarray:
|
||||
"""Inner-corner positions in ROOM coordinates for a board placed at a
|
||||
known position: first corner at `origin`, columns stepping along
|
||||
`u_axis`, rows stepping along `v_axis` (both room-frame unit vectors).
|
||||
"""
|
||||
local = board_object_points(cols, rows, square_size_m)
|
||||
origin = np.asarray(origin, dtype=np.float64)
|
||||
u = np.asarray(u_axis, dtype=np.float64)
|
||||
v = np.asarray(v_axis, dtype=np.float64)
|
||||
return origin[None, :] + local[:, 0:1] * u[None, :] + local[:, 1:2] * v[None, :]
|
||||
|
||||
|
||||
def find_board_corners(image: np.ndarray, cols: int, rows: int) -> np.ndarray | None:
|
||||
"""Detect and sub-pixel-refine checkerboard inner corners.
|
||||
|
||||
Returns (cols*rows, 2) float64 pixel coordinates, or None if not found.
|
||||
"""
|
||||
gray = image if image.ndim == 2 else cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
|
||||
flags = cv2.CALIB_CB_ADAPTIVE_THRESH | cv2.CALIB_CB_NORMALIZE_IMAGE
|
||||
found, corners = cv2.findChessboardCorners(gray, (cols, rows), flags=flags)
|
||||
if not found:
|
||||
return None
|
||||
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 1e-3)
|
||||
corners = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
|
||||
return corners.reshape(-1, 2).astype(np.float64)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Intrinsics
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def compute_intrinsics(
|
||||
corner_sets: list[np.ndarray],
|
||||
image_size: tuple[int, int],
|
||||
cols: int,
|
||||
rows: int,
|
||||
square_size_m: float,
|
||||
) -> dict:
|
||||
"""Camera intrinsics from N checkerboard views via cv2.calibrateCamera.
|
||||
|
||||
corner_sets: list of (cols*rows, 2) pixel corner arrays.
|
||||
image_size: (width, height) of the calibration images.
|
||||
"""
|
||||
obj = board_object_points(cols, rows, square_size_m).astype(np.float32)
|
||||
obj_pts = [obj for _ in corner_sets]
|
||||
img_pts = [c.reshape(-1, 1, 2).astype(np.float32) for c in corner_sets]
|
||||
rms, camera_matrix, dist_coeffs, _, _ = cv2.calibrateCamera(
|
||||
obj_pts, img_pts, tuple(image_size), None, None
|
||||
)
|
||||
return {
|
||||
"image_size": [int(image_size[0]), int(image_size[1])],
|
||||
"camera_matrix": camera_matrix.tolist(),
|
||||
"dist_coeffs": dist_coeffs.ravel().tolist(),
|
||||
"reprojection_error_px": float(rms),
|
||||
"source": "computed",
|
||||
}
|
||||
|
||||
|
||||
def load_intrinsics(path: Path) -> dict:
|
||||
"""Load a pre-computed intrinsics JSON ({camera_matrix, dist_coeffs, image_size})."""
|
||||
with open(path, "r", encoding="utf-8") as f:
|
||||
data = json.load(f)
|
||||
# Accept either a bare intrinsics dict or a full calibration bundle.
|
||||
intr = data.get("camera_intrinsics", data)
|
||||
for key in ("camera_matrix", "dist_coeffs", "image_size"):
|
||||
if key not in intr:
|
||||
raise ValueError(f"Intrinsics file {path} missing key {key!r}")
|
||||
intr = dict(intr)
|
||||
intr["source"] = "file"
|
||||
return intr
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Extrinsics (camera -> room rigid transform)
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def reprojection_rmse(
|
||||
room_points: np.ndarray,
|
||||
image_points: np.ndarray,
|
||||
rvec: np.ndarray,
|
||||
tvec: np.ndarray,
|
||||
camera_matrix: np.ndarray,
|
||||
dist_coeffs: np.ndarray,
|
||||
) -> float:
|
||||
proj, _ = cv2.projectPoints(room_points, rvec, tvec, camera_matrix, dist_coeffs)
|
||||
err = proj.reshape(-1, 2) - image_points.reshape(-1, 2)
|
||||
return float(np.sqrt(np.mean(np.sum(err**2, axis=1))))
|
||||
|
||||
|
||||
def _solve_pnp(
|
||||
room_points: np.ndarray,
|
||||
image_points: np.ndarray,
|
||||
camera_matrix: np.ndarray,
|
||||
dist_coeffs: np.ndarray,
|
||||
) -> dict | None:
|
||||
"""One solvePnP run (room->camera), inverted to camera->room. Returns
|
||||
{rotation (3x3 camera->room), translation_m (camera center in room
|
||||
frame), rmse_px} or None on failure.
|
||||
"""
|
||||
ok, rvec, tvec = cv2.solvePnP(
|
||||
room_points.reshape(-1, 1, 3),
|
||||
image_points.reshape(-1, 1, 2),
|
||||
camera_matrix,
|
||||
dist_coeffs,
|
||||
flags=cv2.SOLVEPNP_ITERATIVE,
|
||||
)
|
||||
if not ok:
|
||||
return None
|
||||
rmse = reprojection_rmse(room_points, image_points, rvec, tvec, camera_matrix, dist_coeffs)
|
||||
r_room_to_cam, _ = cv2.Rodrigues(rvec)
|
||||
r_cam_to_room = r_room_to_cam.T
|
||||
camera_center_room = (-r_cam_to_room @ tvec).ravel()
|
||||
return {
|
||||
"rotation": r_cam_to_room.tolist(),
|
||||
"translation_m": camera_center_room.tolist(),
|
||||
"rmse_px": rmse,
|
||||
}
|
||||
|
||||
|
||||
def solve_extrinsics(
|
||||
room_points: np.ndarray,
|
||||
image_points: np.ndarray,
|
||||
camera_matrix: np.ndarray,
|
||||
dist_coeffs: np.ndarray,
|
||||
) -> dict:
|
||||
"""Solve the camera->room rigid transform from 3D room-frame points and
|
||||
their 2D pixel observations.
|
||||
|
||||
NOTE: the corner grid of a single planar checkerboard is centrosymmetric,
|
||||
so the corner ordering returned by findChessboardCorners (which may
|
||||
enumerate from either board end) cannot be disambiguated from one board
|
||||
alone -- the reversed ordering fits a ghost pose with identical
|
||||
reprojection error. Use solve_two_board_extrinsics for the full
|
||||
two-checkerboard procedure, where the joint point set breaks the symmetry.
|
||||
"""
|
||||
ext = _solve_pnp(room_points, image_points, camera_matrix, dist_coeffs)
|
||||
if ext is None:
|
||||
raise RuntimeError("solvePnP failed")
|
||||
return ext
|
||||
|
||||
|
||||
def solve_two_board_extrinsics(
|
||||
wall_room: np.ndarray,
|
||||
wall_image: np.ndarray,
|
||||
floor_room: np.ndarray,
|
||||
floor_image: np.ndarray,
|
||||
camera_matrix: np.ndarray,
|
||||
dist_coeffs: np.ndarray,
|
||||
) -> dict:
|
||||
"""Joint camera->room solve over both checkerboards (the ADR-152 S2.1.3
|
||||
two-checkerboard method).
|
||||
|
||||
Tries all 4 per-board corner-ordering combinations: each board's ordering
|
||||
is individually ambiguous (centrosymmetric grid), but the combined
|
||||
wall+floor point set is not, so exactly one combination reaches minimal
|
||||
reprojection error. Returns the solve_extrinsics dict plus
|
||||
{wall_flipped, floor_flipped, per_board: {wall|floor: {rmse_px}}}.
|
||||
"""
|
||||
best = None
|
||||
for wall_flipped in (False, True):
|
||||
for floor_flipped in (False, True):
|
||||
wi = wall_image[::-1].copy() if wall_flipped else wall_image
|
||||
fi = floor_image[::-1].copy() if floor_flipped else floor_image
|
||||
room = np.concatenate([wall_room, floor_room], axis=0)
|
||||
img = np.concatenate([wi, fi], axis=0)
|
||||
ext = _solve_pnp(room, img, camera_matrix, dist_coeffs)
|
||||
if ext is None:
|
||||
continue
|
||||
if best is None or ext["rmse_px"] < best[0]["rmse_px"]:
|
||||
ext["wall_flipped"] = wall_flipped
|
||||
ext["floor_flipped"] = floor_flipped
|
||||
rvec, _ = cv2.Rodrigues(np.asarray(ext["rotation"]).T)
|
||||
tvec = -np.asarray(ext["rotation"]).T @ np.asarray(ext["translation_m"])
|
||||
ext["per_board"] = {
|
||||
"wall": {"rmse_px": reprojection_rmse(
|
||||
wall_room, wi, rvec, tvec, camera_matrix, dist_coeffs)},
|
||||
"floor": {"rmse_px": reprojection_rmse(
|
||||
floor_room, fi, rvec, tvec, camera_matrix, dist_coeffs)},
|
||||
}
|
||||
best = (ext,)
|
||||
if best is None:
|
||||
raise RuntimeError("solvePnP failed for all corner-ordering combinations")
|
||||
return best[0]
|
||||
|
||||
|
||||
def extrinsics_consistency(ext_a: dict, ext_b: dict) -> dict:
|
||||
"""Angular + translational disagreement between two extrinsic solutions
|
||||
(the two single-board solves). Large values mean a mis-entered board
|
||||
placement or a bad corner detection.
|
||||
"""
|
||||
ra = np.asarray(ext_a["rotation"])
|
||||
rb = np.asarray(ext_b["rotation"])
|
||||
r_delta = ra.T @ rb
|
||||
angle = float(np.degrees(np.arccos(np.clip((np.trace(r_delta) - 1.0) / 2.0, -1.0, 1.0))))
|
||||
t_delta = float(
|
||||
np.linalg.norm(np.asarray(ext_a["translation_m"]) - np.asarray(ext_b["translation_m"]))
|
||||
)
|
||||
return {"rotation_deg": angle, "translation_m": t_delta}
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Calibration bundle (the artifact written to disk)
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def make_bundle(
|
||||
camera_intrinsics: dict,
|
||||
camera_to_room_extrinsics: dict,
|
||||
checkerboard_spec: dict,
|
||||
transceiver_geometry: dict,
|
||||
) -> dict:
|
||||
return {
|
||||
"schema_version": BUNDLE_SCHEMA_VERSION,
|
||||
"method": BUNDLE_METHOD,
|
||||
"calibrated_at": datetime.now(timezone.utc).isoformat(),
|
||||
"room_frame": {
|
||||
"description": "right-handed; origin at wall/floor corner; "
|
||||
"+x along origin wall, +y into room, +z up",
|
||||
"units": "meters",
|
||||
},
|
||||
"checkerboard_spec": checkerboard_spec,
|
||||
"camera_intrinsics": camera_intrinsics,
|
||||
"camera_to_room_extrinsics": camera_to_room_extrinsics,
|
||||
"transceiver_geometry": transceiver_geometry,
|
||||
}
|
||||
|
||||
|
||||
def calibration_id(bundle: dict) -> str:
|
||||
"""Stable content hash of a bundle -- stamped onto every emitted sample
|
||||
so a label can always be traced to the exact calibration that framed it.
|
||||
"""
|
||||
canonical = json.dumps(bundle, sort_keys=True, separators=(",", ":"))
|
||||
return "sha256:" + hashlib.sha256(canonical.encode("utf-8")).hexdigest()
|
||||
|
||||
|
||||
def save_bundle(bundle: dict, path: Path) -> None:
|
||||
path = Path(path)
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
with open(path, "w", encoding="utf-8") as f:
|
||||
json.dump(bundle, f, indent=2)
|
||||
f.write("\n")
|
||||
|
||||
|
||||
def load_bundle(path: Path) -> dict:
|
||||
with open(path, "r", encoding="utf-8") as f:
|
||||
bundle = json.load(f)
|
||||
for key in ("camera_intrinsics", "camera_to_room_extrinsics", "transceiver_geometry"):
|
||||
if key not in bundle:
|
||||
raise ValueError(f"Calibration bundle {path} missing key {key!r}")
|
||||
return bundle
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Keypoint transform (image -> room-frame bearing rays)
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
class CalibrationContext:
|
||||
"""Pre-computed transform state for a collection session.
|
||||
|
||||
Scales the bundle's intrinsics to the live capture resolution (MediaPipe
|
||||
keypoints are normalized [0,1], so we need the actual frame size to get
|
||||
back to pixels before undistorting).
|
||||
"""
|
||||
|
||||
def __init__(self, bundle: dict, frame_w: int, frame_h: int):
|
||||
self.bundle = bundle
|
||||
self.calibration_id = calibration_id(bundle)
|
||||
self.transceiver_geometry = bundle["transceiver_geometry"]
|
||||
self.frame_w = int(frame_w)
|
||||
self.frame_h = int(frame_h)
|
||||
|
||||
intr = bundle["camera_intrinsics"]
|
||||
k = np.asarray(intr["camera_matrix"], dtype=np.float64)
|
||||
cal_w, cal_h = intr["image_size"]
|
||||
sx = self.frame_w / float(cal_w)
|
||||
sy = self.frame_h / float(cal_h)
|
||||
k = k.copy()
|
||||
k[0, 0] *= sx
|
||||
k[0, 2] *= sx
|
||||
k[1, 1] *= sy
|
||||
k[1, 2] *= sy
|
||||
self.camera_matrix = k
|
||||
self.dist_coeffs = np.asarray(intr["dist_coeffs"], dtype=np.float64)
|
||||
|
||||
ext = bundle["camera_to_room_extrinsics"]
|
||||
self.r_cam_to_room = np.asarray(ext["rotation"], dtype=np.float64)
|
||||
self.origin_room = np.asarray(ext["translation_m"], dtype=np.float64)
|
||||
|
||||
def transform_keypoints(self, keypoints_norm: list[list[float]]) -> tuple[np.ndarray, np.ndarray]:
|
||||
"""Normalized [0,1] image keypoints -> unit bearing rays in the room
|
||||
frame, anchored at the camera center.
|
||||
|
||||
Projective alignment ONLY (no depth): each returned ray is the locus
|
||||
of room positions consistent with the 2D observation. Returns
|
||||
(camera_origin_room (3,), ray_dirs (N, 3) unit vectors).
|
||||
"""
|
||||
pts = np.asarray(keypoints_norm, dtype=np.float64)
|
||||
pts_px = pts * np.array([self.frame_w, self.frame_h], dtype=np.float64)
|
||||
undist = cv2.undistortPoints(
|
||||
pts_px.reshape(-1, 1, 2), self.camera_matrix, self.dist_coeffs
|
||||
).reshape(-1, 2)
|
||||
rays_cam = np.concatenate([undist, np.ones((len(undist), 1))], axis=1)
|
||||
rays_cam /= np.linalg.norm(rays_cam, axis=1, keepdims=True)
|
||||
rays_room = (self.r_cam_to_room @ rays_cam.T).T
|
||||
return self.origin_room, rays_room
|
||||
|
||||
|
||||
def load_calibration_context(path: Path, frame_w: int, frame_h: int) -> CalibrationContext:
|
||||
return CalibrationContext(load_bundle(path), frame_w, frame_h)
|
||||
|
||||
|
||||
def augment_record(record: dict, ctx: CalibrationContext | None) -> dict:
|
||||
"""Stamp a ground-truth record with room-frame rays + calibration metadata.
|
||||
|
||||
With ctx=None this is the identity -- the record (and hence the emitted
|
||||
JSONL line) is byte-identical to the pre-calibration ADR-079 format.
|
||||
Raw image-coordinate keypoints are kept untouched in both cases; the
|
||||
room-frame representation is ADDED, never substituted, so training can
|
||||
choose either (ADR-152 S2.1.3).
|
||||
"""
|
||||
if ctx is None:
|
||||
return record
|
||||
if record.get("keypoints"):
|
||||
_, rays = ctx.transform_keypoints(record["keypoints"])
|
||||
record["keypoints_room"] = [[round(float(v), 5) for v in ray] for ray in rays]
|
||||
else:
|
||||
record["keypoints_room"] = []
|
||||
record["camera_origin_room"] = [round(float(v), 5) for v in ctx.origin_room]
|
||||
record["calibration_id"] = ctx.calibration_id
|
||||
record["transceiver_geometry"] = ctx.transceiver_geometry
|
||||
return record
|
||||
@@ -6,9 +6,19 @@ synchronizes with ESP32 CSI recording from the sensing server.
|
||||
|
||||
Output: JSONL file in data/ground-truth/ with per-frame 17-keypoint COCO poses.
|
||||
|
||||
With --calibration <bundle.json> (produced by scripts/calibrate-camera-room.py,
|
||||
ADR-152 S2.1.3), every record is additionally stamped with room-frame bearing
|
||||
rays for each keypoint, the calibration_id, and the transceiver geometry --
|
||||
the PerceptAlign-style defense against coordinate overfitting. Raw image
|
||||
coordinates are always kept; without depth the room-frame representation is
|
||||
a projective alignment (rays, not 3D points) -- see scripts/calibration_lib.py.
|
||||
Without --calibration the output is byte-identical to the original ADR-079
|
||||
format.
|
||||
|
||||
Usage:
|
||||
python scripts/collect-ground-truth.py --preview --duration 60
|
||||
python scripts/collect-ground-truth.py --server http://192.168.1.10:3000
|
||||
python scripts/collect-ground-truth.py --calibration data/calibration/camera-room.json
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
@@ -168,8 +178,23 @@ def main():
|
||||
default="data/ground-truth",
|
||||
help="Output directory (default: data/ground-truth)",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--calibration",
|
||||
default=None,
|
||||
help="Camera-room calibration bundle JSON from scripts/calibrate-camera-room.py "
|
||||
"(ADR-152 S2.1.3); adds room-frame keypoint rays + transceiver geometry "
|
||||
"to every record",
|
||||
)
|
||||
args = parser.parse_args()
|
||||
|
||||
if not args.calibration:
|
||||
print(
|
||||
"WARNING: no --calibration bundle; labels stay in raw camera coordinates "
|
||||
"and are layout-brittle (coordinate overfitting, ADR-152 S2.1.3) -- run "
|
||||
"scripts/calibrate-camera-room.py first.",
|
||||
file=sys.stderr,
|
||||
)
|
||||
|
||||
# --- Resolve paths relative to repo root ---
|
||||
repo_root = Path(__file__).resolve().parent.parent
|
||||
output_dir = repo_root / args.output
|
||||
@@ -193,6 +218,25 @@ def main():
|
||||
frame_h = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
|
||||
print(f"Camera opened: {frame_w}x{frame_h}")
|
||||
|
||||
# --- Load calibration bundle (ADR-152 S2.1.3) ---
|
||||
calib_ctx = None
|
||||
if args.calibration:
|
||||
# Lazy import keeps the no-calibration path identical to the original.
|
||||
sys.path.insert(0, str(Path(__file__).resolve().parent))
|
||||
import calibration_lib
|
||||
|
||||
try:
|
||||
calib_ctx = calibration_lib.load_calibration_context(
|
||||
Path(args.calibration), frame_w, frame_h
|
||||
)
|
||||
except (OSError, ValueError, json.JSONDecodeError) as exc:
|
||||
print(f"ERROR: Cannot load calibration bundle {args.calibration}: {exc}",
|
||||
file=sys.stderr)
|
||||
sys.exit(1)
|
||||
n_nodes = len(calib_ctx.transceiver_geometry.get("nodes", []))
|
||||
print(f"Calibration: {calib_ctx.calibration_id[:23]}... "
|
||||
f"({n_nodes} transceiver node(s)); emitting room-frame keypoint rays")
|
||||
|
||||
# --- Create PoseLandmarker ---
|
||||
options = PoseLandmarkerOptions(
|
||||
base_options=BaseOptions(model_asset_path=str(model_path)),
|
||||
@@ -287,6 +331,10 @@ def main():
|
||||
"n_visible": n_visible,
|
||||
"n_persons": n_persons,
|
||||
}
|
||||
if calib_ctx is not None:
|
||||
# Adds keypoints_room (bearing rays), camera_origin_room,
|
||||
# calibration_id, transceiver_geometry (ADR-152 S2.1.3).
|
||||
record = calibration_lib.augment_record(record, calib_ctx)
|
||||
out_file.write(json.dumps(record) + "\n")
|
||||
frame_count += 1
|
||||
total_confidence += confidence
|
||||
|
||||
@@ -0,0 +1,80 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Segmented overnight empty-room CSI capture (ADR-135 baseline / MAE corpus).
|
||||
|
||||
Binds UDP once and writes fixed-duration JSONL segments with explicit names —
|
||||
no post-hoc renaming, no glob collisions with other recordings.
|
||||
|
||||
Usage:
|
||||
python scripts/overnight-empty-capture.py --segments 8 --segment-seconds 3300
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import os
|
||||
import socket
|
||||
import struct
|
||||
import time
|
||||
|
||||
|
||||
def parse_csi_packet(data):
|
||||
"""ADR-018 binary CSI packet → dict (same layout as record-csi-udp.py)."""
|
||||
if len(data) < 8:
|
||||
return None
|
||||
node_id = data[4]
|
||||
rssi = struct.unpack("b", bytes([data[6]]))[0]
|
||||
channel = data[7]
|
||||
iq = data[8:]
|
||||
amplitudes = []
|
||||
for i in range(0, len(iq) - 1, 2):
|
||||
I = struct.unpack("b", bytes([iq[i]]))[0]
|
||||
Q = struct.unpack("b", bytes([iq[i + 1]]))[0]
|
||||
amplitudes.append(round((I * I + Q * Q) ** 0.5, 2))
|
||||
return {
|
||||
"type": "raw_csi",
|
||||
"ts_ns": time.time_ns(),
|
||||
"node_id": node_id,
|
||||
"rssi": rssi,
|
||||
"channel": channel,
|
||||
"subcarriers": len(iq) // 2,
|
||||
"amplitudes": amplitudes,
|
||||
"iq_hex": iq.hex(),
|
||||
}
|
||||
|
||||
|
||||
def main():
|
||||
ap = argparse.ArgumentParser()
|
||||
ap.add_argument("--port", type=int, default=5005)
|
||||
ap.add_argument("--segments", type=int, default=8)
|
||||
ap.add_argument("--segment-seconds", type=int, default=3300)
|
||||
ap.add_argument("--output", default="data/recordings")
|
||||
ap.add_argument("--prefix", default="overnight-empty")
|
||||
args = ap.parse_args()
|
||||
|
||||
os.makedirs(args.output, exist_ok=True)
|
||||
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||
sock.bind(("0.0.0.0", args.port))
|
||||
sock.settimeout(2.0)
|
||||
|
||||
for seg in range(1, args.segments + 1):
|
||||
path = os.path.join(
|
||||
args.output, f"{args.prefix}-seg{seg}-{int(time.time())}.csi.jsonl"
|
||||
)
|
||||
n = 0
|
||||
t_end = time.time() + args.segment_seconds
|
||||
with open(path, "w", encoding="utf-8") as f:
|
||||
while time.time() < t_end:
|
||||
try:
|
||||
data, _ = sock.recvfrom(4096)
|
||||
except socket.timeout:
|
||||
continue
|
||||
rec = parse_csi_packet(data)
|
||||
if rec is not None:
|
||||
f.write(json.dumps(rec) + "\n")
|
||||
n += 1
|
||||
print(f"segment {seg}: {n} frames -> {path}", flush=True)
|
||||
|
||||
print("capture complete", flush=True)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,8 @@
|
||||
"""Make scripts/ importable for the calibration tests (ADR-152 S2.1.3)."""
|
||||
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
SCRIPTS_DIR = Path(__file__).resolve().parents[1]
|
||||
if str(SCRIPTS_DIR) not in sys.path:
|
||||
sys.path.insert(0, str(SCRIPTS_DIR))
|
||||
@@ -0,0 +1,326 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Headless tests for the camera-room calibration pipeline (ADR-152 S2.1.3).
|
||||
|
||||
Covers calibration_lib.py end to end on synthetic data -- no camera, no
|
||||
display, no MediaPipe:
|
||||
* known extrinsics recovered from synthetic two-checkerboard corners
|
||||
* calibration bundle JSON round-trip + stable content hash
|
||||
* image->room keypoint transform correctness (rays pass through the
|
||||
original 3D points -- the projective, no-depth alignment of ADR-079
|
||||
labels into the shared room frame)
|
||||
* collect-ground-truth's no-calibration record path is byte-identical
|
||||
(augment_record with ctx=None is the identity)
|
||||
|
||||
Run: python -m pytest scripts/tests/ -q
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
import calibration_lib as cal
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Synthetic scene fixtures
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
IMG_W, IMG_H = 1280, 720
|
||||
K_GT = np.array(
|
||||
[[800.0, 0.0, 640.0],
|
||||
[0.0, 800.0, 360.0],
|
||||
[0.0, 0.0, 1.0]]
|
||||
)
|
||||
DIST_ZERO = np.zeros(5)
|
||||
DIST_MILD = np.array([-0.10, 0.02, 0.001, -0.001, 0.0])
|
||||
|
||||
BOARD_COLS, BOARD_ROWS = 9, 6
|
||||
SQUARE_M = 0.025
|
||||
|
||||
|
||||
def look_at_pose(camera_pos, target):
|
||||
"""Ground-truth camera pose: returns (R_cam_to_room, camera_center_room).
|
||||
|
||||
Camera convention: +z forward (optical axis), +x right, +y down.
|
||||
"""
|
||||
c = np.asarray(camera_pos, dtype=np.float64)
|
||||
fwd = np.asarray(target, dtype=np.float64) - c
|
||||
fwd /= np.linalg.norm(fwd)
|
||||
up_room = np.array([0.0, 0.0, 1.0])
|
||||
x_cam = np.cross(fwd, -up_room)
|
||||
x_cam /= np.linalg.norm(x_cam)
|
||||
y_cam = np.cross(fwd, x_cam)
|
||||
r_cam_to_room = np.stack([x_cam, y_cam, fwd], axis=1) # columns = camera axes in room
|
||||
return r_cam_to_room, c
|
||||
|
||||
|
||||
def room_to_cam(r_cam_to_room, center):
|
||||
"""Invert to the solvePnP (room->camera) convention: rvec, tvec."""
|
||||
r_room_to_cam = r_cam_to_room.T
|
||||
tvec = -r_room_to_cam @ center
|
||||
rvec, _ = cv2.Rodrigues(r_room_to_cam)
|
||||
return rvec, tvec.reshape(3, 1)
|
||||
|
||||
|
||||
def project_room_points(points_room, r_cam_to_room, center, k=K_GT, dist=DIST_ZERO):
|
||||
rvec, tvec = room_to_cam(r_cam_to_room, center)
|
||||
proj, _ = cv2.projectPoints(np.asarray(points_room, dtype=np.float64), rvec, tvec, k, dist)
|
||||
return proj.reshape(-1, 2)
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def scene():
|
||||
"""A camera in the room looking at the wall + floor checkerboards."""
|
||||
r_gt, c_gt = look_at_pose(camera_pos=[1.5, 3.0, 1.3], target=[1.0, 0.5, 0.8])
|
||||
wall_room = cal.board_room_points(
|
||||
BOARD_COLS, BOARD_ROWS, SQUARE_M,
|
||||
origin=[0.5, 0.0, 1.6], u_axis=cal.parse_axis("+x"), v_axis=cal.parse_axis("-z"),
|
||||
)
|
||||
floor_room = cal.board_room_points(
|
||||
BOARD_COLS, BOARD_ROWS, SQUARE_M,
|
||||
origin=[1.0, 1.0, 0.0], u_axis=cal.parse_axis("+x"), v_axis=cal.parse_axis("+y"),
|
||||
)
|
||||
return r_gt, c_gt, wall_room, floor_room
|
||||
|
||||
|
||||
def make_bundle(r_gt, c_gt, dist=DIST_ZERO):
|
||||
return cal.make_bundle(
|
||||
camera_intrinsics={
|
||||
"image_size": [IMG_W, IMG_H],
|
||||
"camera_matrix": K_GT.tolist(),
|
||||
"dist_coeffs": dist.tolist(),
|
||||
"reprojection_error_px": 0.0,
|
||||
"source": "synthetic",
|
||||
},
|
||||
camera_to_room_extrinsics={
|
||||
"rotation": r_gt.tolist(),
|
||||
"translation_m": c_gt.tolist(),
|
||||
"rmse_px": 0.0,
|
||||
},
|
||||
checkerboard_spec={"cols": BOARD_COLS, "rows": BOARD_ROWS, "square_size_mm": 25.0},
|
||||
transceiver_geometry={
|
||||
"nodes": [
|
||||
{"id": "esp32-s3-a", "position_m": [0.1, 2.4, 1.1], "antenna_yaw_deg": 180.0},
|
||||
{"id": "esp32-c6-b", "position_m": [3.2, 0.3, 0.9]},
|
||||
],
|
||||
"units": "meters",
|
||||
"source": "file",
|
||||
},
|
||||
)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Extrinsics recovery from synthetic checkerboard corners
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
class TestExtrinsicsRecovery:
|
||||
def test_two_board_combined_recovers_known_pose(self, scene):
|
||||
r_gt, c_gt, wall_room, floor_room = scene
|
||||
room_pts = np.concatenate([wall_room, floor_room], axis=0)
|
||||
img_pts = project_room_points(room_pts, r_gt, c_gt)
|
||||
|
||||
ext = cal.solve_extrinsics(room_pts, img_pts, K_GT, DIST_ZERO)
|
||||
|
||||
assert ext["rmse_px"] < 1e-3
|
||||
np.testing.assert_allclose(np.asarray(ext["translation_m"]), c_gt, atol=1e-4)
|
||||
r_delta = np.asarray(ext["rotation"]).T @ r_gt
|
||||
angle_deg = np.degrees(np.arccos(np.clip((np.trace(r_delta) - 1) / 2, -1, 1)))
|
||||
assert angle_deg < 0.01
|
||||
|
||||
def test_single_board_solves_agree(self, scene):
|
||||
# With correct corner ordering, each board alone recovers the same pose.
|
||||
r_gt, c_gt, wall_room, floor_room = scene
|
||||
ext_wall = cal.solve_extrinsics(
|
||||
wall_room, project_room_points(wall_room, r_gt, c_gt), K_GT, DIST_ZERO)
|
||||
ext_floor = cal.solve_extrinsics(
|
||||
floor_room, project_room_points(floor_room, r_gt, c_gt), K_GT, DIST_ZERO)
|
||||
consistency = cal.extrinsics_consistency(ext_wall, ext_floor)
|
||||
assert consistency["rotation_deg"] < 0.1
|
||||
assert consistency["translation_m"] < 1e-3
|
||||
|
||||
def test_reversed_corner_order_auto_recovered(self, scene):
|
||||
# findChessboardCorners may enumerate from either board end. A single
|
||||
# board cannot disambiguate that flip (centrosymmetric grid), but the
|
||||
# joint two-board solve can -- feed it a reversed wall ordering and
|
||||
# require the true pose back.
|
||||
r_gt, c_gt, wall_room, floor_room = scene
|
||||
wall_img = project_room_points(wall_room, r_gt, c_gt)
|
||||
floor_img = project_room_points(floor_room, r_gt, c_gt)
|
||||
ext = cal.solve_two_board_extrinsics(
|
||||
wall_room, wall_img[::-1].copy(), floor_room, floor_img,
|
||||
K_GT, DIST_ZERO)
|
||||
assert ext["wall_flipped"] is True
|
||||
assert ext["floor_flipped"] is False
|
||||
assert ext["rmse_px"] < 1e-3
|
||||
np.testing.assert_allclose(np.asarray(ext["translation_m"]), c_gt, atol=1e-3)
|
||||
|
||||
def test_joint_solver_matches_unflipped(self, scene):
|
||||
r_gt, c_gt, wall_room, floor_room = scene
|
||||
ext = cal.solve_two_board_extrinsics(
|
||||
wall_room, project_room_points(wall_room, r_gt, c_gt),
|
||||
floor_room, project_room_points(floor_room, r_gt, c_gt),
|
||||
K_GT, DIST_ZERO)
|
||||
assert ext["wall_flipped"] is False and ext["floor_flipped"] is False
|
||||
assert ext["per_board"]["wall"]["rmse_px"] < 1e-3
|
||||
assert ext["per_board"]["floor"]["rmse_px"] < 1e-3
|
||||
|
||||
def test_intrinsics_recovered_from_synthetic_views(self):
|
||||
# Several board views from different poses -> calibrateCamera should
|
||||
# get focal length / principal point close to ground truth.
|
||||
obj = cal.board_object_points(BOARD_COLS, BOARD_ROWS, SQUARE_M)
|
||||
poses = [
|
||||
([0.05, 1.2, 0.05], [0.10, 0.0, 0.06]),
|
||||
([-0.25, 1.0, 0.20], [0.10, 0.0, 0.06]),
|
||||
([0.45, 0.9, -0.15], [0.10, 0.0, 0.06]),
|
||||
([0.10, 1.4, 0.30], [0.10, 0.0, 0.06]),
|
||||
([-0.15, 0.8, -0.20], [0.10, 0.0, 0.06]),
|
||||
]
|
||||
corner_sets = []
|
||||
for cam_pos, target in poses:
|
||||
r, c = look_at_pose(cam_pos, target)
|
||||
# Embed the board rigidly in the y=0 plane (u=+x, v=+z) and view it.
|
||||
board_in_room = np.column_stack([obj[:, 0], obj[:, 2], obj[:, 1]])
|
||||
corner_sets.append(project_room_points(board_in_room, r, c))
|
||||
intr = cal.compute_intrinsics(corner_sets, (IMG_W, IMG_H),
|
||||
BOARD_COLS, BOARD_ROWS, SQUARE_M)
|
||||
k = np.asarray(intr["camera_matrix"])
|
||||
assert abs(k[0, 0] - K_GT[0, 0]) / K_GT[0, 0] < 0.05
|
||||
assert abs(k[1, 1] - K_GT[1, 1]) / K_GT[1, 1] < 0.05
|
||||
assert intr["reprojection_error_px"] < 1.0
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Bundle round-trip + content hash
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
class TestBundle:
|
||||
def test_save_load_roundtrip(self, scene, tmp_path):
|
||||
r_gt, c_gt, _, _ = scene
|
||||
bundle = make_bundle(r_gt, c_gt)
|
||||
path = tmp_path / "camera-room.json"
|
||||
cal.save_bundle(bundle, path)
|
||||
loaded = cal.load_bundle(path)
|
||||
assert loaded == bundle
|
||||
assert cal.calibration_id(loaded) == cal.calibration_id(bundle)
|
||||
|
||||
def test_bundle_schema_fields(self, scene):
|
||||
r_gt, c_gt, _, _ = scene
|
||||
bundle = make_bundle(r_gt, c_gt)
|
||||
for key in ("schema_version", "method", "calibrated_at", "room_frame",
|
||||
"checkerboard_spec", "camera_intrinsics",
|
||||
"camera_to_room_extrinsics", "transceiver_geometry"):
|
||||
assert key in bundle
|
||||
assert bundle["method"] == "two-checkerboard"
|
||||
|
||||
def test_calibration_id_changes_with_content(self, scene):
|
||||
r_gt, c_gt, _, _ = scene
|
||||
bundle_a = make_bundle(r_gt, c_gt)
|
||||
bundle_b = json.loads(json.dumps(bundle_a))
|
||||
bundle_b["transceiver_geometry"]["nodes"][0]["position_m"] = [0.2, 2.4, 1.1]
|
||||
assert cal.calibration_id(bundle_a) != cal.calibration_id(bundle_b)
|
||||
assert cal.calibration_id(bundle_a).startswith("sha256:")
|
||||
|
||||
def test_load_bundle_rejects_missing_keys(self, tmp_path):
|
||||
path = tmp_path / "bad.json"
|
||||
path.write_text('{"camera_intrinsics": {}}', encoding="utf-8")
|
||||
with pytest.raises(ValueError, match="missing key"):
|
||||
cal.load_bundle(path)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Keypoint transform: image -> room-frame bearing rays (projective alignment)
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
class TestKeypointTransform:
|
||||
PERSON_POINTS = np.array([
|
||||
[1.2, 1.5, 1.7], # head height
|
||||
[1.1, 1.5, 1.4], # shoulder
|
||||
[1.3, 1.6, 0.9], # hip
|
||||
[1.2, 1.5, 0.1], # ankle
|
||||
])
|
||||
|
||||
@pytest.mark.parametrize("dist", [DIST_ZERO, DIST_MILD], ids=["no-distortion", "mild-distortion"])
|
||||
def test_rays_pass_through_original_points(self, scene, dist):
|
||||
r_gt, c_gt, _, _ = scene
|
||||
img = project_room_points(self.PERSON_POINTS, r_gt, c_gt, dist=dist)
|
||||
kps_norm = (img / np.array([IMG_W, IMG_H])).tolist()
|
||||
|
||||
ctx = cal.CalibrationContext(make_bundle(r_gt, c_gt, dist=dist), IMG_W, IMG_H)
|
||||
origin, rays = ctx.transform_keypoints(kps_norm)
|
||||
|
||||
np.testing.assert_allclose(origin, c_gt, atol=1e-9)
|
||||
np.testing.assert_allclose(np.linalg.norm(rays, axis=1), 1.0, atol=1e-9)
|
||||
for point, ray in zip(self.PERSON_POINTS, rays):
|
||||
v = point - origin
|
||||
# Distance from the true 3D point to the recovered ray ~ 0, and
|
||||
# the point sits in FRONT of the camera along the ray.
|
||||
dist_to_ray = np.linalg.norm(v - np.dot(v, ray) * ray)
|
||||
assert dist_to_ray < 1e-4
|
||||
assert np.dot(v, ray) > 0
|
||||
|
||||
def test_resolution_scaling(self, scene):
|
||||
# Collection camera runs 640x360 while the bundle was made at
|
||||
# 1280x720 -- normalized keypoints must land on the same rays.
|
||||
r_gt, c_gt, _, _ = scene
|
||||
img = project_room_points(self.PERSON_POINTS, r_gt, c_gt)
|
||||
kps_norm = (img / np.array([IMG_W, IMG_H])).tolist()
|
||||
|
||||
ctx = cal.CalibrationContext(make_bundle(r_gt, c_gt), 640, 360)
|
||||
origin, rays = ctx.transform_keypoints(kps_norm)
|
||||
for point, ray in zip(self.PERSON_POINTS, rays):
|
||||
v = point - origin
|
||||
assert np.linalg.norm(v - np.dot(v, ray) * ray) < 1e-4
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# collect-ground-truth record path (import-level; no camera loop)
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
class TestRecordAugmentation:
|
||||
LEGACY_RECORD = {
|
||||
"ts_ns": 1775300000000000000,
|
||||
"keypoints": [[0.45, 0.12]] * 17,
|
||||
"confidence": 0.92,
|
||||
"n_visible": 14,
|
||||
"n_persons": 1,
|
||||
}
|
||||
|
||||
def test_no_calibration_is_byte_identical(self):
|
||||
# The collector's no---calibration path must emit exactly the
|
||||
# original ADR-079 JSONL line (back-compat guarantee).
|
||||
record = json.loads(json.dumps(self.LEGACY_RECORD))
|
||||
before = json.dumps(record)
|
||||
out = cal.augment_record(record, None)
|
||||
assert out is record
|
||||
assert json.dumps(out) == before
|
||||
assert set(out.keys()) == {"ts_ns", "keypoints", "confidence",
|
||||
"n_visible", "n_persons"}
|
||||
|
||||
def test_calibrated_record_gains_room_fields(self, scene):
|
||||
r_gt, c_gt, _, _ = scene
|
||||
bundle = make_bundle(r_gt, c_gt)
|
||||
ctx = cal.CalibrationContext(bundle, IMG_W, IMG_H)
|
||||
|
||||
record = json.loads(json.dumps(self.LEGACY_RECORD))
|
||||
out = cal.augment_record(record, ctx)
|
||||
|
||||
# Raw image coords preserved untouched; room representation added.
|
||||
assert out["keypoints"] == self.LEGACY_RECORD["keypoints"]
|
||||
assert len(out["keypoints_room"]) == 17
|
||||
assert all(len(ray) == 3 for ray in out["keypoints_room"])
|
||||
assert out["calibration_id"] == cal.calibration_id(bundle)
|
||||
assert out["transceiver_geometry"] == bundle["transceiver_geometry"]
|
||||
assert len(out["camera_origin_room"]) == 3
|
||||
json.dumps(out) # remains JSONL-serializable
|
||||
|
||||
def test_empty_keypoints_record(self, scene):
|
||||
r_gt, c_gt, _, _ = scene
|
||||
ctx = cal.CalibrationContext(make_bundle(r_gt, c_gt), IMG_W, IMG_H)
|
||||
record = {"ts_ns": 1, "keypoints": [], "confidence": 0.0,
|
||||
"n_visible": 0, "n_persons": 0}
|
||||
out = cal.augment_record(record, ctx)
|
||||
assert out["keypoints_room"] == []
|
||||
assert "calibration_id" in out
|
||||
Reference in New Issue
Block a user