diff --git a/.github/workflows/cibuildwheel.yml b/.github/workflows/cibuildwheel.yml index 8c71da3f..2ce2a956 100644 --- a/.github/workflows/cibuildwheel.yml +++ b/.github/workflows/cibuildwheel.yml @@ -75,7 +75,7 @@ jobs: # regression tests unsuited to wheel smoke. CIBW_TEST_COMMAND: > python {project}/scripts/ci_test_setup.py {project} && - python -c "import openptv2.algorithms.track_kernels_tracking, openptv2.algorithms.track3d; print('openmp import OK')" && + python -c "import openptv2.algorithms.track_kernels_corr, openptv2.algorithms.track3d; print('openmp import OK')" && pytest tests/unit/test_vec_utils.py tests/unit/test_correspondences.py diff --git a/docs/index.md b/docs/index.md index d8d55295..ba1aa091 100644 --- a/docs/index.md +++ b/docs/index.md @@ -13,6 +13,8 @@ Welcome to the openptv2 documentation. ### User Documentation - [Tracking Pipeline & Results Guide](tracking_guide.md) - Pipeline workflow, parameter guide, multi-pass tracking, and ptv_is.# output format +- [Particle Trackers](trackers.md) - Which tracker to use and why: basics, all eight engines, caveats, tips, upstream credits +- [Two-Phase Tracking](two-phase-tracking.md) - Two-Phase usage and parameters in depth - [Lid-Driven Cavity Flow Tutorial](tutorials/cavity_flow_tutorial.md) - End-to-end 3D-PTV case study: Autocalibration, Tracer Shaking, Warmup, and 3D Trajectories - [Aortic Pulsatile Flow Tutorial](aorta_tutorial.md) - Cloud-native 3D-PTV on complex aortic flow - [Auto-Calibration with `openptv warmup`](tutorials/warmup_tutorial.md) - Standalone parameter/engine auto-tuning before tracking diff --git a/docs/plans/2026-09-20-trackcorr-vs-3dptv-link-gap-plan.md b/docs/plans/2026-09-20-trackcorr-vs-3dptv-link-gap-plan.md new file mode 100644 index 00000000..c29d6183 --- /dev/null +++ b/docs/plans/2026-09-20-trackcorr-vs-3dptv-link-gap-plan.md @@ -0,0 +1,579 @@ +# Plan: locating the trackcorr vs 3dptv link gap (69.5% vs 59.0%) + +**Date**: 2026-09-20 +**Status**: CLOSED — cause located (config mismatch, T0.1). See "Resolution". +**Dataset**: `C:\Users\alex\Downloads\HiDImaging\wp1_10_images` +(4 cameras, 512x512, ~1300 particles/frame, 10 frames, `dvxmax=1.9`, +`dacc=1.9`, `dangle=270` gon, `add=1`) +**Reference**: `res_ground_truth_backup/` — legacy `3dptv.exe` output, per +`scripts/make_ground_truth_zarr.py` ("regression oracle for the old binary") + +## Resolution (2026-09-20, evening session) + +**Root cause: openptv2 ran with `dacc=0.8` while 3dptv ran with `dacc=1.9`.** +`scratch/_wp1_ab/parameters_Run1.yaml` (`track.dacc: 0.8`) overrides the +work folder's `parameters/track.par` (line 8: `1.9`, byte-identical to the +dataset's). `py_start_proc_c` returns the YAML value — every openptv2 number +in this plan was measured at `dacc=0.8`, i.e. the two engines were never +compared under the same configuration. + +Evidence (all on clean frames 100003–100010, pristine inputs restored before +every run — see methodology note below): + +| config | test links | exact vs ref (7387) | recall | +|---|---|---|---| +| baseline `dacc=0.8` (track.c parity flags) | 6329 | 6181 | 83.7% | +| baseline `dacc=0.8` (openptv2 defaults) | 6335 | 6184 | 83.7% | +| **`dacc=1.9`** (3dptv value) | 7384 | 7238 | **98.0%** | +| `dacc=1.9` + backward pass | 7418 | 7251 | 98.2% | +| `dvxmax` 1.9 → 2.5 (dacc=0.8) | 6328 | 6099 | 82.6% (hurts slightly) | + +Mechanism check: 17.9% of reference links have 3-point `acc >= 0.8` (only +0.25% have `acc >= 1.9`), vs a 16.3% exact-link deficit — the missing mass +sits exactly in the `[0.8, 1.9)` acceleration band the tighter gate rejects. +This also explains the displacement ramp (faster links scatter more in +acceleration) that was mistaken for a search/candidate defect. + +Residual 2% (149 → 136 links after backward): spread across all frames +(12–30/frame, no blowup), disproportionately fast (mean disp 0.97 mm vs 0.39 +overall) — cascade + conflict noise, no second systematic cause. The backward +pass adds only +13 links, so the reference is essentially forward-made. + +**Recommendation**: decide whether `dacc: 0.8` in the work YAML is an +intentional retune or an accident, and either align it with `track.par` +(`1.9`) or document the divergence where the comparison is reported. The +`~2% dacc/dangle rejects` and `dacc/sqrt(18)` noise-ceiling entries in the +"Suspects eliminated" table below were measured/computed under mismatched +assumptions (rejects at dacc=0.8; ceiling at dacc=1.9) — both are superseded +by this section. + +Methodology notes (learned the hard way): +- A tracking run REWRITES `res/rt_is.*` (added particles appended) and + updates target-file `tnr` columns in place. Re-running in the same folder + without restoring inputs silently changes the inputs. `scripts/ + sweep_track_params.py` and `scripts/run_dacc19.py` restore pristine + `rt_is.*` (from `res_ground_truth_backup`) + targets (from dataset + `img_3dptv`) before every run and hash them. +- Never run two tracking processes in one work folder concurrently (steps + re-read inputs per step; races produce garbage link counts and mystery + deaths). One early `dvxmax=2.5` reading (673 links/step-1 + vanished + process) was such a race artifact; clean rerun gives 82.6%. + +## Addendum: dataset-folder verification (same session) + +Re-ran everything directly in `C:\Users\alex\Downloads\HiDImaging\ +wp1_10_images` (whose own `parameters_Run1.yaml` also carries `dacc: 0.8`) +with an in-memory `dacc=1.9` override, forward + backward +(`scripts/verify_same_trajectories.py`): + +| level | result | +|---|---| +| links, clean frames 100003–100010 | 7251/7387 = **98.16%** exact | +| links, all frames 100002–100010 | 7873/8018 = 98.19% | +| full trajectory chains end-to-end | 4463/4793 = **93.1%**; length distrib. identical (mean 2.67 vs 2.69, max 10) | +| displacement overlay | ramp gone: %lost per bin ~0±5% at all speeds; mean step 0.395 vs 0.403 (was 0.394 vs 0.311) | + +Residual-miss fingerprint at matched parameters (in-kernel probe, step +100005, 26 misses): 73% partner PRESENT but lost in conflict resolution, +19% partner gated downstream, 8% inlist empty — versus 96% inlist-EMPTY at +`dacc=0.8`. No systematic blindness remains; the rest is greedy-resolution +divergence + cascade. + +Tooling fixes while verifying: `scripts/diag_inlist_probe.py` gained +`--dacc`/`--dv` overrides and now walks the buffer from `first` to `--step` +(it previously processed frame `first` while labeling/comparing `--step`, +and mislabeled output files for any `--step != first`). + +Dataset left pristine: `res/` back to `{run.zarr}`, target files +byte-identical to pre-run snapshot (rt↔tnr consistent on clean frames), +`parameters_Run1.yaml` untouched (still ships `dacc: 0.8`). + +## Addendum 2: single-trajectory comparison (`scripts/ +compare_single_trajectories.py`, plots in `scratch/single_traj/`) + +At matched parameters the residual is greedy contest resolution in dense +crossing regions, illustrated frame-by-frame: + +- **case2 (crossing, len-10):** at 100006→100007 ref jumps 0.93 mm (acc + 1.02) to 1268 and continues violently (acc up to 1.88); test links 842→822 + (acc 0.27, smoother) and stops. Cross-check: ref runs 822→1237 as a len-2 + stub while test runs 1268→1237 — a genuine 2-particle tangle 1268/822 → + 1237/1238 resolved oppositely. Undecidable without ground truth. +- **case5 (ghost stitch, len-10):** ref leaves 100004:1207 a singleton; test + threads it into a full-span chain assembled from three ref chains (len-3 + head + singleton + interiors of a len-8 and a len-6), including a 1.50 mm + step and an acc-1.899 link at the dacc edge. Sensitive vs conservative — + likewise undecidable from outputs alone. +- **case1 (tail miss):** test follows 8/8 frames then stops where ref + continues through acc 1.295 — conservative tail. +- **case3/4:** 2.67 mm cold-start jump at the corrupt frame 100001 — + artifact of the known data defect, not algorithmic. + +Net: no global bias (smoother/longer/ghost-ridden in neither direction). +Every remaining difference is a local contest outcome. Pushing past 98% +requires ground truth adjudication — the synthetic replica testbed (T3.1). + +## Addendum 3: crossing laboratory (`scripts/synth_crossing.py`) + +Minimal scenes with known truth on the real wp1 optics (4 cams, σ=0.07 mm +noise): two particles cross between frames 3→4 of 7. Result table +(recall / crossing-decision-correct over reps): + +| scenario | parity (lr0) | default (lr1) | +|---|---|---| +| S1 head-on, S2/S3 near-miss, S5 fast, S7 asymmetric | 100% 1/1 | 100% 1/1 | +| S4 head-on+noise (8 reps), S6 three-way (8) | 100% 8/8 | 100% 8/8 | +| S8 kick-at-cross (0.15→1.0, no noise) | 91.7% 1/1 | **100%** 1/1 | +| S9 brake-at-cross | 100% | 100% | +| S10 convoy pair (8) | 100% 8/8 | 100% 8/8 | +| S11 kick+noise (8) | 75% **2/8** | 75% **2/8** | +| S12 convoy+noise (8) | 96.9% 7/8 | **100%** 8/8 | +| S11 with dacc 2.5 / 3.5 | 2/8, 2/8 | 2/8, 2/8 (gate not binding) | + +Mechanism, read from the kernel's own arrays (S8): the kick makes truth +expensive (rr 0.039) while the smooth theft is cheap (rr 0.027); both claim +the same row, truth's owner loses the contest (0.027 vs 0.009), parity drops +it (`next=-2`), `loser_retry` recovers via second choice. Failure modes in +S11 are swaps and stalls varying per noise seed. + +Lessons: (1) for steady motion through crossings the smooth choice IS +correct and openptv2 never misses it — so real case-2's violent-vs-smooth +split likely means the particle truly maneuvered and 3dptv was right; +(2) keep `loser_retry=1` (default): strictly dominates parity on maneuvers +(S8, S12), free elsewhere; (3) maneuver+noise is structural — no parameter +rescues it; fixes would be longer temporal context, noise-aware costs, or +global (non-greedy) assignment. + +## Addendum 4: 4BE on the hard crossings (`scripts/bench_4be_crossing.py`) + +4BE (four-frame best estimate: candidate scored by predicting a real +particle two frames ahead) does NOT solve the maneuver crossing — it is +worse: S8 cross-ok 0/3 (vs parity 3/3, default 3/3), S11 0/8 (vs 2/8), +S12 7/8, S1/S4 100%. Reason: longer context cannot fix a violated motion +model — constant-velocity extrapolation underprices the steady lie versus +maneuvering truth at every horizon, so more context just confirms the lie +more confidently. Two-directional (fwd+bwd trackcorr) was already measured +on real data: +13 links (98.0→98.2%), same mirrored greedy logic, no help +on contests. What would help: acceleration-aware prediction, +multi-hypothesis tracking (keep both options alive 2 frames — the kick IS +resolved by f5/f6), or global assignment; none is in the codebase. + +## Addendum 5: two-phase tracker on crossings (`scripts/bench_two_phase.py`) + +`plugins/two_phase_tracking.py`: phase 1 = 3D KD-tree candidates within +v_max; phase 2 = Hungarian assignment on 2D pixel-distance costs. Memoryless +(no velocity, no history). Result: systematic **bounce bias** — at an +X-crossing the swap always has lower total cost than the cross, so raw +two-phase scores 0 cross-ok on EVERYTHING incl. clean S1 (83.3% recall = +one swapped step). It cannot solve hard crossings as-is. +Fix tested in-bench: constant-velocity prediction injected into phase 1 +(predict points, re-project for leaf costs) + Hungarian (`two-phase+vel`): +S1/S8/S4 → 100% cross-ok, **S11 → 5/8 vs trackcorr's 2/8**. Global +assignment + prediction beats greedy + prediction on the killer. Not yet +productized (needs velocity state, cold start, gap handling in the plugin ++ wp1 validation) — candidate next step. + +## Addendum 6: productized two-phase+vel (plugin) + wp1 verdict + +Implemented in `plugins/two_phase_tracking.py`: velocity state per track, +zero-velocity cold start, gap bridging to `max_gap`, `project_fn` callback +built in `do_tracking` from `exp.cals`/`exp.cpar` (falls back to 3D costs), +new knobs `use_velocity` (default True), `cost_mode`, `v_max` (separate from +`dvxmax`), `max_gap`. `tests/unit/test_two_phase_tracking.py`: 5 passed. +`scripts/bench_two_phase.py` productized engine == prototype on every scene ++ S13 occlusion gap bridged 3/3. + +wp1 (`scripts/verify_two_phase_wp1.py`, v_max × max_gap grid): + +| config | recall | links (ref 7418) | test-only step p50/p90 | +|---|---|---|---| +| v2.0 gap1 | **96.26%** | 8443 | 0.81 / 1.95 | +| v2.0 gap2 | 95.93% | 8988 (+573 gap) | 0.82 / 1.95 | +| v2.5 gap1 | 94.98% | 8734 | 1.49 / 2.51 | +| v3.3 gap1 | 86.92% | 9246 | 2.52 / 3.96 | + +vs trackcorr@dacc19: 98.16%, 7418 links. Verdict: viable ALTERNATIVE +engine (maneuver-sensitive: S11 5/8 vs 2/8), not a replacement — it lacks +trackcorr's selectivity (no acc gate / two-hop confirmation), so it +over-links plausible-but-unconfirmed tracks, and gap bridging slightly +hurts on wp1 (95.93 < 96.26: stale-velocity gap claims steal consecutive +links). Natural next step per the original vision (3D forward, then 2D): +cascade — trackcorr first, two-phase+vel only on unlinked heads/tails. + +## The gap + +Both engines run on identical `rt_is.*`, so a link-by-link comparison is exact. +Measured on clean frames 100003–100010: + +``` +3dptv 69.5% openptv2 59.0% reproduced 6113/7387 = 82.8% +``` + +The loss is monotonic in link displacement, with **no cliff**: + +| link displacement | 3dptv links | openptv2 | % openptv2 misses | +|---|---|---|---| +| 0.00–0.22 mm | 3326 | 3263 | 2% | +| 0.44–0.55 | 608 | 495 | 19% | +| 0.77–0.88 | 231 | 116 | 50% | +| 1.21–1.32 | 72 | 20 | 72% | +| 1.87–1.98 | 34 | 7 | 79% | + +Mean step: 3dptv 0.394 mm, openptv2 0.311 mm — openptv2's trajectories are +measurably slower, which is the originally reported symptom. + +Split of the 1256 missing links: 96% the **predecessor got no link at all**, +4% it linked elsewhere. Only 0.7% of reference links are mislinked. So this is +genuine dropping, not preference reordering. + +## Suspects eliminated (all by direct measurement) + +| Suspect | Verdict | Evidence | +|---|---|---| +| Search-volume clamp | ✗ | no cliff; both runs have links past `dvxmax` | +| Candidate shortlist depth (top-4) | ✗ | true partner at **rank 1** median, 0.2% beyond top-4 | +| Multi-camera `freq<2` cull | ✗ | 1.2% | +| Two-hop lookahead | ✗ | 0.2% `HOP2_NOT_FOUND`; `X5` predictor byte-identical to `track.c:223` | +| `dacc`/`dangle` gate | ✗ | ~2% rejects; `dangle=270` gon = 243° is inert | +| Position noise | ✗ | sigma = 0.070 mm vs ceiling `dacc/sqrt(18)` = 0.448 mm | +| Phase-3 loser retry | ✗ | +1.6 links/frame of ~760 | +| Neighbour-velocity cold start | ✗ | 6705 -> 6704 links | +| Compiled vs pure-Python | ✗ | **byte-identical** (534 links, 523 reproduced, both) | +| Camera permutation | ✗ | cross-camera residual matrix is diagonal | +| Quadrant vs full-frame convention | ✗ | baseline 90 px vs 330–400 px for every offset variant | +| mmlut / multimedia model | ✗ | clearing the LUT gives identical residuals; projection is **0.53 px** median on clean frames | +| Frame-100001 corruption | ✗ (as gap cause) | gap persists at 69.5/59.0 on clean frames | +| Prediction cascade | ✗ | autopsy with test-run link history unchanged (69.1% GATE_PASSED) | + +Faithful to `track.c`, verified line-by-line: `rr` cost formula, the +`(acc 4.0), `dacc` + (1.9 -> 6.0), `dangle` (270 -> 400), `add` (0/1) — plotting openptv2's link + rate against 3dptv's fixed 69.5%. +- **Decision**: whichever parameter closes the gap names the binding constraint + empirically. If none closes it, the loss is not a gate and T1.1 should show a + structural cause. + +### T2.2 — One-step isolation with seeded history + +Removes cascade entirely. + +- **Method**: seed `path_prev`/`path_next` from 3dptv's `ptv_is` for frames + N-1 and N, run exactly one step, compare that step's links. +- **Decision**: matching means the gap is cascade (a small per-step deficit + compounding). Still missing means a genuine per-step difference. + +## Phase 3 — Make iteration fast + +### T3.1 — Synthetic replica at real statistics + +The earlier synthetic (`scripts/ab_loser_retry.py synthetic`) was too sparse +and too clean to reproduce this regime. + +- **Method**: ground-truth trajectories matching wp1 — ~1300 particles/frame, + 0.36 mm mean step, 0.07 mm position noise, this calibration, 4 cameras, same + density distribution. +- **Decision**: ~100% recall means the gap is specific to real data and + Phase 0/1 explains it. ~59% means you have a fast, fully-observable testbed + with a known right answer — iterate there, not on real data. + +## Phase 4 — The comparison not yet done + +### T4.1 — Instrumented `track.c` harness + +Everything so far compares openptv2's internals against 3dptv's **final +output**. Nobody has looked at 3dptv's intermediates. + +- **Method**: extract `trackcorr_c` from `C:\Users\alex\Downloads\3dptv\src_c\` + into a standalone C harness reading the same `rt_is`/targets/`track.par`, + with the same counters as T1.1. Compare stage-by-stage on one frame. +- **Cost**: highest — a real build project (Tcl dependencies need stripping). +- **Decision**: reserve for the case where Phases 0–3 leave it ambiguous. + +## Ordering and exit criteria + +| Phase | Cost | Could end it outright | +|---|---|---| +| 0 | hours | **Yes** — parameter mismatch or stale reference dissolves the premise | +| 1 | ~1 day | **Yes** — the census names the stage | +| 2 | hours | Localises empirically without any mechanism hypothesis | +| 3 | ~1 day | Converts it into a controlled, ground-truth problem | +| 4 | days | Definitive, only if needed | + +**Run Phase 0 first.** Three dead ends in the originating session would have +been avoided by checking whether the two runs were configured the same way. + +**Stop condition**: a stage from T1.1 accounting for the majority of missed +links, confirmed by T1.2 traces, whose removal or widening moves the link rate +toward 69.5% in T2.1. + +## Tooling already built + +| Script | Measures | +|---|---| +| `scripts/diag_conflict_parity.py` | `track.c` vs openptv2 link resolution, self-checking | +| `scripts/diag_speed_ceiling.py` | measured vs configured speed ceiling | +| `scripts/ab_loser_retry.py` | `synthetic` / `folder` / `gate` modes, paired statistics | +| `scripts/diag_position_noise.py` | position noise vs the `dacc/sqrt(18)` ceiling | +| `scripts/diag_link_displacement.py` | the displacement ramp table + plot | +| `scripts/diag_candidate_autopsy.py` | per-link two-stage autopsy (**external replay — see the open contradiction above; trust T1.1 over this**) | +| `scripts/diag_inlist_probe.py` | reads the kernel's real `inlist`/`linkdecis` arrays | +| `scripts/_run_one_step.py` | one step + link comparison; used for the compiled/interpreted check | + +## Addendum 7: cascade (trackcorr base + additive two-phase merge) + +`scripts/cascade_track.py` (`merge_links`: accept a two-phase link iff +BOTH ends are free; never steal) + `scripts/run_cascade.py` (synth/wp1 +driver). Load-bearing bug found by the +0 result: trackcorr marks dropped +tails `-2`, not `-1` — the first guard (`== -1`) accepted nothing. + +- synth: S13 occlusion gap fixed 94.1% → 100%; S11 unchanged (swaps have + taken ends — additive merge fixes incompleteness, never incorrectness). +- wp1: base 7251/7387 (98.16%) → merged 7295/7387 (**98.75%**), +444 links. + Added split: consecutive exact=44/extra=328 (11.8% added-precision); + gap exact=0/extra=0 (no occlusions in this data). Added links are + high-acc by construction (p50 1.46 vs ref 0.31); acc-filtering destroys + the gain (they're maneuvers or ghosts — undecidable vs 3dptv). +- Verdict: works as designed, but 44 recovered vs 328 unconfirmed fails + the no-ghost utility — ship as opt-in second pass, default OFF. The + adjudication the 328 needs is a dense-data synthetic testbed with truth. + +## Addendum 8: Willneff program — pipeline completion + core limits + +Willneff & Grün 2002 (the algorithm track.c implements): new-method +efficiency eff_3D = links/particles 76.2/89.4/91.1% (datasets A/B/C), +gains +25/+28/+13% over object-space-only. Ingredients: image+object +space, prediction, gap bridging, bidirectional. Thesis headline: yield of +LONG trajectories for Lagrangian analysis. + +wp1 chain census — ref vs ours fwd+bwd are IDENTICAL (eff 69.9/70.2%, +fullspan 334/335, len≥8 533/539): the port is faithful; the gap to +Willneff's numbers is dataset difficulty + missing pipeline stages. + +Postprocess (`scripts/run_postprocess_wp1.py`, already wired into plugin +flows): cold_start +8, gap bridges +138, reciprocity severs 0 → +eff 70.2→71.5%, fullspan 335→373 (+11%), len≥8 539→608, exact unchanged +98.16% (ref lacks relinked links). Gap bridging validated with truth: +1- and 2-frame occlusions → 100% (`max_gap=2`). Note: 2-frame gaps only +bridge with sequence room — the last 2 steps kill lookahead (track.c +behavior), so tail cold-starts need the backward pass. + +Global (Hungarian) Phase-2 (`scripts/proto_global_resolve.py`, no kernel +change, 20 reps): S11 20/20 (vs 7-10/20) BUT S4 13/20 and S12 7/20 +cross-ok (vs 19-20/20). Drill (`scripts/drill_s4_global.py`): identical +histories, pure resolution-level loss — min-total overfits noise. +REJECTED as replacement; greedy+retry stays the best average rule. + +Bottom line for trackcorr: pipeline-complete (fwd+bwd+postprocess) + +loser_retry=1 is the evidence-backed operating point (98.16% links, +71.5% eff, fullspan 373 on wp1). The S11 maneuver+noise core has no +single-step fix — needs MHT / maneuver detection (future work). + +## Addendum 9: out-of-box program — profiler, appearance, ensemble + +Miss profiler (`scripts/miss_profiler.py`, 145 misses vs 7873 controls): +misses are faster (step 1.02 vs 0.38), 5x more crowded (crowd≥2: 15.2 vs +3.0%), shorter history (5.3 vs 7.5), higher acc (1.14 vs 0.44); no frame +clustering except the tail frame (30 = last-frame no-lookahead boundary, +vanishes on long sequences). Cold start is NOT the problem (0% starts). +Appearance probe (sumg continuity at 26 contested divergences): truth +more similar in 65% — real but weak; tiebreak-grade, not gate-grade. + +Ensemble (`scripts/collect_ensemble_wp1.py`: parity / default-lr1 / 4BE / +two-phase+vel, identical inputs): singles 98.16 / **98.33** / 88.70 / +96.26% (default-lr1 beats parity by 13 links — last missing cell). +Unanimous (4/4) links: **99.74% precision** (n=6440) — the ghost-free set. +Majority (≥3): 99.40% precision but only 96.01% recall (2-2 ties abstain). +Union (any engine right): **99.54%** — only 34/7387 links are hard for +ALL engines. Contested (engines disagree): 947 links (12.8%), majority +right 90.1% (wins: default 841, parity 828, 2p 688, 4be 129). + +Shippable: per-link agreement flag (4/4 → high-confidence; 2-2 → +review) + default-lr1 as the recall engine. Kernel rebuild verified +feasible (setup.py, ~87 s) if an in-kernel change ever earns it. + +## Addendum 11: brightness in cost + pulsatile freedom (kernel work) + +track.c verified: tracking cost is purely kinematic +(`rr=(dl/lmax+acc/dacc+angle/dangle)/quali`) — brightness was never used. +(a) Implemented: `targ_sumg` SoA plumbing (Frame→kernel), per-particle +mean-grey signatures, `rr += w_app·|Δ|/(sum)` at all 4 cost sites, +`app_weight` flag (Tracker/TrackingRun/kernel, default 0.0), rebuilt. +S15 (4:1 brightness kick+noise): 83%→100% at w≥0.1; 18 other +scenario-variants unchanged (incl. uniform-brightness S11: proves +inertness when uninformative). wp1: -4 links at w=0.1 → default 0.0, +opt-in ≈0.1 for distinct populations. Shape/size (n/nx/ny) same pattern, +not yet plumbed. +(b) Pulsatile: mild (acc=100), veto skipped in sparse data +where a lone bystander loses on cost (S14c). A kernel starvation-gate +experiment was tried and REMOVED (redundant once the veto works; it +blocked genuine shell rescues). Final, all on rebuilt kernel: +wp1 ppar-acc-hi2.5 fwd = 7238/97.98% EXACT fixed parity; S14c-vel = 100% +(61% fixed); S15-app0.1 = 100%; S11 fixed=app=ppar = 79.6%; +S1-ppar = 100%. wp1 fwd+bwd: ppar+app0.1 == fixed+app0.1 link-for-link +(7234/4439) -- backward absorbs the forward delta; app0.1 alone costs +-17/-24 vs app0 on natural brightness (7251/4463), so default app=0, +opt-in 0.1. run_adaptive_pp gained `yaml`, `mode`, `backward` params. + +## Addendum 10: manual vote on the 34 union-miss links (moved intact -- +was accidentally folded into Addendum 11 during editing) + +Prototype `scripts/vote_links.py` (agree. sidecars: flag 4/3/2/1 ++ per-engine prev) + `scripts/plot_hard_links.py` (6 mosaics in +`scratch/hard_links/`, X-Y/X-Z neighborhoods with per-engine arrows) + +`scripts/judge_hard_links.py` (same-target options table) + +`scripts/trace_divergence.py` (upstream walk) + validated +`scripts/probe_search.py` (48/48 control agreement) and per-axis legality +(31/34 legal; 3 ref dv-violations: 100005/488, 100010/860, 100010/1026). + +Verdict by class (34): +- TAIL (8, all frame 100010): no-lookahead strict gate; vanishes on long + sequences. 860/1026 are ref dv-violations — engines defensible. +- LOCAL contested, same history (14): genuine ties; options table decides + each. Exhibits: 100006/875 (ref smoother on step AND acc AND appearance + 4-vs-139 — test steal; best case FOR appearance tiebreak), 100006/334 + (ref acc 2.13 OVER dacc — engines right; possible 3dptv overreach), + 100007/1268 (eng smoother 0.62 vs 1.02 — toss-up), 100004/1247 + REF-FAST (ref steady 2.39mm/acc 0.41, all abstain — smoothness bias + drops fast tracks;needs velocity-alignment reward, not just acc penalty). +- LOCAL all-abstain, small-step legal (535: kernel inlist=3 incl. truth — + S8-style maneuver contest loss, rr 0.039 vs 0.027; 421: parity blind via + broken forward history (21.6mm prediction), default contest loss — SAME + vote, THREE mechanisms across engines). +- UPSTREAM (10): 2 one-step swaps adjudicated (100003/1167 lean-test: + smooth+history vs cold 2.25 jump; 100006/875 for REF, agrees with + appearance), 1 head-formation (421), 7 birth-differences in the corrupt + frame-100001 zone (excluded from judging — data defect). + +Critical missing decisions, ranked: (1) cost function has no +velocity-alignment reward and no appearance tiebreak (875 + 1247 would +flip); (2) single-step horizon can't see maneuvers resolving (S11); +(3) upstream swaps cascade into downstream blindness backward can't +repair (needs next>=0 seeds). (1) is implementable in-kernel; (2)-(3) +need MHT. diff --git a/docs/plans/2026-09-20-tracking-kernels-dedup.md b/docs/plans/2026-09-20-tracking-kernels-dedup.md new file mode 100644 index 00000000..3e4cd6a5 --- /dev/null +++ b/docs/plans/2026-09-20-tracking-kernels-dedup.md @@ -0,0 +1,89 @@ +# Tracking-kernels dedup (ponytail-audit items 1-9) + +Branch: `refactor/dedup-tracking-kernels` (off `main`). Committed and pushed (0e1200a0, 1b83cfa6); no PR yet. +Goal: remove duplicated / orphaned code in `src/openptv2/algorithms/track_kernels_*.py` +without changing behavior. + +## What was found + +The same Cython `cdef`/`nogil` kernels were copy-pasted across sibling modules +(cross-module C calls need a `.pxd`, so people copied instead). Bodies were +identical or differed only in decorators/comments. `track_kernels_pixel.pxd` and +`track_kernels_position.pxd` already existed as the sharing mechanism, so I +extended them rather than inventing anything. + +## Done + +Items 1-5, dedup. Owners: +- `track_kernels_pixel.py`: `_multimed_r_nlay_1layer` (added `exceptval(check=False)` + pxd entry), + `_point_to_pixel_out`, `_candsearch_in_pix_rest_nogil`, `_pixel_to_metric_out`, + `_dist_to_flat_out`, `_sorted_candidates_fast_out_nogil`, `candsearch_in_pix_fast_nogil` + (the pixel copy is the newer one with `max_cands`/`out_dists`; search's was a stale, dead copy). +- `track_kernels_position.py`: `_ray_tracing_out` (added exceptval + pxd), `_angle_acc_out` + (moved here from geom/corr, added exceptval + pxd), `_point_position_out`, + `assess_new_position_fast_nogil`. +- Copies deleted from geom, search, transform, corr. Importers use the existing + `if cython.compiled: cimport ... else: from .x import ...` pattern. + Import direction is acyclic: pixel <- position <- geom/transform/corr/batch. + +Item 6: deleted the `track_kernels_tracking.py` shim. Repointed `track_kernels.py`, +`setup.py` (`ALGORITHMS_MODULES`), `.github/workflows/cibuildwheel.yml` (import smoke test), +and `tests/unit/test_track_kernels_tracking_coverage.py` (now imports from owners; shim +constants defined locally; `_mod` = the pixel module, which keeps the old inert-patch behavior). + +Item 9: deleted 16 wrappers with no non-test callers, plus their tests and the two +re-exports in `track_kernels.py`: +`angle_acc_fast`, `_ray_tracing_fast`, `pixel_to_metric_fast`, `dist_to_flat_fast`, +`metric_to_pixel_fast`, `_metric_to_pixel_out`, `_flat_image_coord_fast`, `_img_coord_fast`, +`img_coord_batch_fast`, `flat_image_coord_batch_fast`, `point_position_fast`, +`ray_tracing_batch_fast`, `point_position_batch_fast`, `pixel_to_metric_batch_fast`, +`metric_to_pixel_batch_fast`, `sort_candidates_by_freq_fast`. +Also dropped two search test classes that only tested the deleted dead copies. + +Net so far: roughly -2,200 lines from dedup, about -840 from item 9, plus tests. + +## Verification status (updated 2026-09-21) + +Clean Cython rebuild of the final tree: OK. +- Hot-path tests (`test_track`, `test_track3d`, `test_correspondences`, `test_track4be`): 49 passed = baseline. +- Full suite `uv run --no-sync pytest tests`: 2017 passed, 86 skipped, 39 deselected (12 min). +- Pure-Python fallback, kernel coverage files only + (`test_track_kernels_*_coverage.py`, 6 files): 266 passed. +- Pure-Python fallback over the whole `tests/unit/test_*_coverage.py` glob: 1227 passed, 24 failed + (38 min; the glob now matches 29 files, not the 16 CLAUDE.md mentions, so it is slow). + The failures I inspected (`test_epi_coverage`, `test_correspondences_coverage`, 14 of the 24) + are all `Coord2d.__init__() got an unexpected keyword argument 'pnr'` / + `Candidate.__init__() ... 'pnr'`: interpreted-mode constructor mismatch in modules this + branch does not touch. Not confirmed on `main`; the other 10 were not inspected. +- Running the suite rewrites tracked `test_data/test_cavity/img/*_targets`; `git checkout -- test_data` before committing. + +## Remaining + +1. Optional: confirm the 24 fallback failures also occur on `main` (build `main`, run + `tests/unit/test_epi_coverage.py tests/unit/test_correspondences_coverage.py` interpreted). +2. Optional perf sanity: `_angle_acc_out` was `ccall inline` inside corr and is now a cross-module + C call (lost inlining). Time a tracking run before/after; if it regressed, keep a private copy in corr. +3. Cosmetic: stray banner comments in `test_track_kernels_batch_coverage.py` (about lines 84-102) and + `test_track_kernels_transform_coverage.py` (lines 22, 352). +4. Open a PR. + +## Deliberately skipped + +- **Item 7** (merge forward/backward tracking loops in `track_kernels_corr.py`): not a clean + dedup. Normalised diff of `trackcorr_loop_fast` vs `trackback_loop_fast` shows about 670 of about 900 + lines differ; forward uses the `_trackcorr_particle_fast` worker, backward is inline. Merging + changes core tracking logic and there is no golden-output regression data. Only attempt + with a recorded before/after trajectory comparison on a real dataset. +- **Item 8** (delete 4BE tracker: `track4be_loop_fast`, `track4be.py`, `plugins/four_be_tracking.py`): + it is registered in `tracking_registry.py` and benchmarked in about 10 scripts/notebooks + (`bench_*`, `benchmark_*`, `tracker_tutorial_dashboard.py`). Needs an explicit product decision. + Do not delete without Alex saying so. + +## Gotchas + +- Cimported names are not importable from Python: tests and non-cimporting modules must import + the `cpdef` names from the owner module (pixel/position), not from geom/transform. +- Cython pure mode + `.pxd`: `noexcept nogil` in the pxd requires `@cython.exceptval(check=False)` + on the `def`, or the signatures mismatch. +- Shell cwd drift: `cd` to the repo root at the start of every command. +- Use `git grep`, not `grep -r`, at the repo root (huge build dirs; a plain grep timed out). diff --git a/docs/plans/2026-09-24-cloud-wp1-twophase-run-plan.md b/docs/plans/2026-09-24-cloud-wp1-twophase-run-plan.md new file mode 100644 index 00000000..8cad71f7 --- /dev/null +++ b/docs/plans/2026-09-24-cloud-wp1-twophase-run-plan.md @@ -0,0 +1,118 @@ +# Plan: Cloud Run of wp1 with Bidirectional Two-Phase Tracker & Postprocessing + +**Date:** 2026-09-24 +**Target Dataset:** `C:\Users\alex\Downloads\HiDImaging\CompleteTest\wp1\test` +**GCP Environment:** Project `iucc-alex-liberzon`, Region `europe-west3`, Buckets `openptv-uploads` / `openptv-results` +**Objective:** Re-run tracking on the full 5,005 frames directly in the cloud using the new bidirectional Two-Phase tracker, execute flowtracks repair and smoothing, generate Eulerian and phase-averaged Zarr datasets, and fetch trajectories and results locally for analysis against `res_orig` (3dptv.exe `xuap`). + +--- + +## Background & Rationale + +1. **Why rerun in Cloud:** + The full correspondences and target intermediate files for the 5,005 frames already reside in `openptv-results` on GCS (`CompleteTest-track-20260915-065041/wp1/test/res.tar`). Running tracking on Cloud Run directly where the correspondence data is located avoids pulling tens of gigabytes across the internet and executes in parallel. + +2. **Why the new Two-Phase Bidirectional Tracker:** + - Previous runs suffered from overly conservative kinematic limits (`dacc: 0.4`, `dvxmax: 1.4`), which gate-dropped fast systolic bursts in the aorta. + - The new Two-Phase tracker with **Forward-First Bidirectional Tracking** (`bidirectional: true`): + - Forward pass with $v_{\max} = 2.2\text{ mm}$ establishes the clean diastolic core. + - Backward pass with wider reach ($v_{\max}^{\text{bwd}} = 3.5\text{ mm}$) recovers high-velocity systolic bursts without risking swaps in dense regions. + - Closed 75% of the link gap to 4-frame trackcorr on wp1 (reaching 97.48% exact recall in 3.0 s vs 15.0 s). + - Incorporates canonical `TargetArray.sort_y()` and auto-grid safety. + +--- + +## Step-by-Step Execution Plan + +### Step 1: Merge PR #39 in `openptv2` +- **Current Status:** PR #39 (`refactor/dedup-tracking-kernels` -> `main`) is open, mergeable, and all 8 GitHub Actions CI checks are **GREEN** (cross-platform wheels on Linux/macOS/Windows, ruff linting, multi-Python tests). +- **Action:** + ```bash + gh pr merge 39 --merge + git switch main + git pull --ff-only + ``` + +### Step 2: Build & Push Docker Image in `openptv-cloud` +- Update `openptv-cloud` dependency reference to include the merged `openptv2` `main`. +- Build the container image and push to Google Artifact Registry: + ```bash + cd C:\Users\alex\projects\openptv-cloud + uv run openptv-cloud build + ``` +- Image target: + `europe-west3-docker.pkg.dev/iucc-alex-liberzon/openptv/openptv-cloud-job:latest` + +### Step 3: Verify & Configure `parameters_twophase.yaml` +- Parameter file already prepared at: + `C:\Users\alex\Downloads\HiDImaging\CompleteTest\wp1\test\parameters_twophase.yaml` +- Config details: + ```yaml + plugins: + selected_sequence: default + selected_tracking: two_phase + + track: + selected_tracking: two_phase + preset: two_phase + use_velocity: true + cost_mode: projected + leaf_weight: 1.0 + v_max: 2.2 # Diastolic core search radius + bidirectional: true # Forward-first locked bidirectional tracking + bwd_v_max: 3.5 # Systolic burst recovery radius + max_gap: 2 # Missing-frame bridge capacity + allow_shared: true # Occlusion clustering + share_tol: 1.0 + confirm_tol: 1.9 # Kinematic acceleration gate matching dacc=1.9 + confirm_ends: false + max_group_size: 128 + postprocess: false + + sequence: + first: 1 + last: 5005 + ``` + +### Step 4: Configure `experiment.yaml` for wp1 Single-Folder Run +In `C:\Users\alex\Downloads\HiDImaging\CompleteTest\experiment.yaml`: +- Scope run folders strictly to `wp1/test`: + ```yaml + runs: + folders: [wp1/test] + + frames: + first: 1 + last: 5005 + + trajectories: + repair: true # flowtracks.repair (cut bad links, join pieces across gaps) + smoothing_window: 7 # Savitzky-Golay filter + smoothing_order: 2 + ``` + +### Step 5: Launch Tracking on GCP Cloud Run +- Run the cloud pipeline: + ```bash + uv run openptv-cloud run C:\Users\alex\Downloads\HiDImaging\CompleteTest + ``` +- Monitor progress: + ```bash + uv run openptv-cloud status C:\Users\alex\Downloads\HiDImaging\CompleteTest --watch + ``` + +### Step 6: Fetch Trajectories & Postprocess Locally +- Download the resulting trajectory table and phase-binned Zarr datasets: + ```bash + uv run openptv-cloud fetch C:\Users\alex\Downloads\HiDImaging\CompleteTest + ``` +- Confirm output files in `C:\Users\alex\Downloads\HiDImaging\CompleteTest\wp1\test\res\`: + - `run.zarr/trajectories` (repaired & smoothed positions, velocities, accelerations) + - `run.zarr/eulerian` (Eulerian velocity, TKE, MKE fields) + - `phase_binned.zarr` (24 cardiac phase bins) + +### Step 7: Validation against `res_orig/xuap` +- Run comparison script between newly generated trajectories in `res/run.zarr` vs ground-truth `res_orig`: + - Track length distribution (mean length $\ge 14$ frames). + - Trajectory coverage and sample counts. + - Phase-averaged velocity profiles in peak systole vs diastole. diff --git a/docs/trackers.md b/docs/trackers.md new file mode 100644 index 00000000..2ce7f169 --- /dev/null +++ b/docs/trackers.md @@ -0,0 +1,156 @@ +# Particle Trackers in openptv2 + +Start here if you want to know which tracker to use and why. For measured +numbers on a reference dataset, see [Tracker Tutorials](tracker-tutorials.md); +for pipeline and file formats, see the [Tracking Guide](tracking_guide.md); +for Two-Phase usage in depth, see [Two-Phase Tracking](two-phase-tracking.md). + +## 1. The basics: what tracking is + +A camera takes a picture every frame. In each picture, every particle is a +dot. In the next picture the dots have moved a little. **Tracking means +deciding which dot in picture 2 is the same particle as each dot in +picture 1.** + +When particles are far apart and slow, this is easy: take the nearest dot. +It gets hard when particles cross each other, when one is hidden for a +frame, or when positions are noisy. Every tracker below is a different set +of rules for that decision. They differ in only four things: + +1. **Guess** — where do you expect the particle next? (stay put, constant + velocity, smoothed curve, look at future frames, bigger time step) +2. **Score** — how do you rank candidates? (distance, acceleration, angle, + camera-image agreement) +3. **Conflicts** — two particles want the same dot: who wins? (first come, + cheapest first, best total pairing, nobody) +4. **Evidence** — what do you look at? Only 3D points, or also the original + camera images? Trackers that check the images survive bad 3D points; + trackers that ignore them are faster. + +Two facts shape everything on this page. First, measured on synthetic flow +(see `docs/tracking-benchmark-results.md`), most wrong links come from +**missing detections across gaps (~60%)** and **cold starts (~20%)** — not +from the scoring rule. So gap handling matters more than clever costs. +Second, particles only ever **enter and exit** the volume; a track that ends +mid-volume is almost always an occlusion or a dropout, not a real exit. + +## 2. All trackers at a glance + +Select with `plugins.selected_tracking` in your parameters (GUI: Plugins +page). The `name` column is the exact preset string. + +| preset (`name`) | idea in one line | looks at | +|---|---|---| +| `default`, `standard_forward`, `full_multipass`, `two_directional` (trackcorr) | Guess forward, confirm in every camera, accept smooth links | 2D targets + 3D | +| `priority_segment_3d` (Fast 3D / 3MA) | Cheapest (smoothest) links first, globally | 3D only | +| `4be` | Peek at frame n+2 before accepting; conflicts link to nobody | 3D only | +| `nearest_hungarian_3d` (MyPTV 3D) | Best total pairing per frame pair (Hungarian), survives gaps | 3D only | +| `myptv_2d_tracking` (MyPTV 2D) | Each camera tracks its own images, then cameras vote | 2D per camera | +| `predictive_gmm_3d` (proPTV) | Fit a smooth curve through history, predict from it | 3D only | +| `two_phase` (Two-Phase) | 3D search for candidates, per-camera images for ranking | 3D + 2D | +| `hybrid_deltat_3d` (Hybrid) | Match every N-th frame where motion beats noise, fill between | 3D only | + +Details per tracker live in `src/openptv2/tracking_registry.py` +(`TRACKER_REGISTRY`) — the machine-readable version of this page. + +## 3. Each tracker: caveats, tips, tricks + +### trackcorr (`default`, `standard_forward`, …) — the original engine +The safest default. Predicts forward, checks candidates in every camera +image, accepts links with small acceleration and turning angle, and can run +backward plus gap relinking. **Caveat:** slowest of the bunch, and three +interacting parameters (`dvxmax`, `dacc`, `angle`). **Tip:** on noisy slow +flow, switch the angle limit off — the measured turning angle is mostly +noise then. If a particle is hidden in one camera, trackcorr usually still +gets it through the others. + +### Fast 3D / 3MA (`priority_segment_3d`) — the fast one +Hundreds of thousands of particles per second, simplest mental model +(smoothest join wins). **Caveat:** blind to the images, so a ghost particle +sitting where the guess expects is accepted without question. **Tip:** use +it for clean, dense data and high throughput; distrust it where ghosts are +likely (poor calibration, few cameras). + +### 4BE (`4be`) — the careful one +Looks one frame into the future before committing, and refuses contested +dots outright. **Caveat:** built for sparse clean data; on noisy/dense data +it produces the worst accelerations of all engines and leaves gaps +deliberately (gap bridging is off for its preset on purpose — it would +rebuild exactly the links 4BE declined). **Tip:** sparse lab data with +reliable detection; not turbulence. + +### MyPTV 3D (`nearest_hungarian_3d`) — the fair one +Nobody grabs the nearest dot first: it finds the pairing with the lowest +*total* distance, so nobody is paired badly. Tracks survive short gaps +(`max_gap`), code is plain readable Python. **Caveat:** one frame pair at a +time — it cannot use what happens next. **Tip:** good first alternative to +the default; easiest engine to modify (`src/openptv2/plugins/myptv_3d_tracking.py`). + +### MyPTV 2D (`myptv_2d_tracking`) — the per-camera voter +Each camera tracks its own movie; links with the most camera votes win. +**Caveat:** a link needs only one vote, so a single confused camera can +still create a bad link. **Tip:** reaches for it when 3D triangulation is +unreliable but the raw images are clean. + +### proPTV (`predictive_gmm_3d`) — the smoother +Fits smooth curves through each path, so speeds and accelerations stay +sensible under noise. **Caveat (read first):** this port predicts from the +*smoothed current position*, not from an extrapolated one, and its search +radius is the same with or without history — so it currently behaves closer +to smoothed nearest-neighbour than to the predictive scheme of the paper. +Also dense data gets expensive. **Tip:** check the plugin README before +trusting its "predictive" label; fix the extrapolation first if you build +on it. + +### Two-Phase (`two_phase`) — the hybrid +3D search lists candidates, per-camera image distances rank them, Hungarian +per connected group decides. No motion model — immune to bad guesses, but +fails once motion outruns particle spacing. Reported +74% multi-frame +trajectories over the default on a poorly-conditioned aorta dataset. +**Caveat:** needs Cet 2D targets per camera in the store; falls back to +pure 3D (`leaf_weight=0`) without them. **Tips:** start with +`leaf_weight=1`, `v_max` at ~3× your typical step; see +[Two-Phase Tracking](two-phase-tracking.md) for the full parameter guide +including the shared-observation prototype flags. + +### Hybrid multi-Δt (`hybrid_deltat_3d`) — the slow-flow specialist +When particles crawl, frame-to-frame steps drown in noise — so it matches +every N-th frame (where displacement beats noise) and fills the middle with +a smooth curve. **Caveat:** the only engine that changes the signal-to-noise +ratio instead of fighting it, but the smooth fill is wrong for fast or +curved motion. **Tip:** high frame rate + slow flow; set `stride` so the +coarse step clearly exceeds your 3D noise floor. + +## 4. Upstream credit: MyPTV and proPTV are plugins, not forks + +Two engines borrow ideas from outside projects. **We use them as plugins — +adapted concepts on openptv2's own data structures, not modified copies of +their code.** The full frameworks (triangulation pipelines, calibration, +backtracking/repair, smoothing toolboxes) live only in their own +repositories — use those projects directly if you need them. Both are +permissively MIT-licensed. + +- **MyPTV** by Ron Shnapp — open-source Python 3D-PTV library. + Repository: · + Paper: Shnapp, R. (2022). *MyPTV: A Python Package for 3D Particle + Tracking.* Journal of Open Source Software, 7(75), 4398. + · + What we adapted: per-camera 2D image-space tracking with multi-camera + consensus (`myptv_2d_tracking`), and kinematic prediction + assignment + matching in 3D (`nearest_hungarian_3d`). +- **proPTV** by Robin Barta and colleagues (DLR) — probabilistic PTV + framework, Python. + Repository: · + Paper: Barta, R. et al. (2024). *proPTV: A probabilistic particle + tracking velocimetry framework.* Journal of Computational Physics, 514, + 113212. · + What we adapted: the small pure-numpy core (Gaussian-mixture / basis + approximation and Savitzky–Golay smoothing, vendored under + `src/openptv2/plugins/proptv/`), wired into the `predictive_gmm_3d` + plugin. The original's triangulation, probability model, backtracking + and repair are *not* ported. +- The classic engines descend from the OpenPTV/liboptv lineage + (). + +If you publish with these methods, please cite the original authors above +in addition to openptv2. diff --git a/docs/two-phase-tracking.md b/docs/two-phase-tracking.md new file mode 100644 index 00000000..97a9f933 --- /dev/null +++ b/docs/two-phase-tracking.md @@ -0,0 +1,88 @@ +# Two-Phase Tracking: usage and parameters + +Two-Phase (`selected_tracking: two_phase`, +`src/openptv2/plugins/two_phase_tracking.py`) links particles in two steps: +**Phase 1** finds candidates with a 3D KD-tree around each track's predicted +position; **Phase 2** ranks them by per-camera 2D image distance and solves a +Hungarian assignment per connected group. 3D proposes, the images dispose. + +## 1. How to use it + +**GUI:** Plugins page → tracking plugin → `two_phase`. Parameters live in +the `track` section (same names as below). + +**Batch/YAML:** minimal setup — +```yaml +plugins: + selected_tracking: two_phase +track: + v_max: 5.0 # mm/frame; or dvxmax, same meaning here + leaf_weight: 1.0 + max_gap: 2 +``` + +**Plain Python** (no experiment needed): +```python +import numpy as np +from openptv2.plugins.two_phase_tracking import ( + TwoPhaseTracker, TwoPhaseTrackerConfig) + +cfg = TwoPhaseTrackerConfig(v_max=5.0, leaf_weight=1.0, max_gap=2) +links = TwoPhaseTracker(cfg).track_frames( + frame_particles, # list of (N_i, 3) arrays, mm + frame_leaves, # list of (N_i, 2*C) arrays, px (optional) + project_fn, # (N,3) -> (N,2*C) re-projection (optional) +) +# links: list of (t0, row0, t1, row1); use return_chains=True to also get +# per-track histories: links, chains = ...track_frames(..., return_chains=True) +``` + +Without leaves/`project_fn` it falls back to pure 3D distance costs +(`cost_mode="3d"`); pass `leaf_weight=0` to force that explicitly. + +## 2. Parameters + +All live in the `track` YAML section (batch/GUI) or on +`TwoPhaseTrackerConfig` (Python). Units in brackets. + +| parameter | default | what it does | how to set it | +|---|---|---|---| +| `v_max` (or `dvxmax`) [mm/frame] | 15.5 | 3D search radius around each prediction | ~3× your typical per-frame step. Too small: true links never become candidates. Too big: everything connects into giant groups (slow, sloppy) | +| `leaf_weight` [–] | 1.0 | weight of 2D image distance in the cost | 1.0 normally; 0 = pure 3D (no leaves needed). Lower it when calibration is poor — bad projection poisons the ranking | +| `use_velocity` [bool] | true | match predictions (`pos + vel·dt`), not positions | Keep on. Off = every crossing resolves as a bounce | +| `cost_mode` [str] | `projected` | `projected`: rank by re-projected 2D distance (needs `project_fn`/cals); `3d`: rank by 3D distance | `projected` with good calibration, `3d` otherwise | +| `max_gap` [frames] | 2 | a track survives this many unmatched frames | 2 covers single-frame dropouts (the biggest measured failure source). Higher = longer bridges, more impostors | +| `dt` [–] | 1.0 | time step for velocity | 1.0 for consecutive frames | +| `max_group_size` [nodes] | 128 | groups bigger than this skip the cubic Hungarian, greedy inside | Raise only if you can afford it; at production density frames percolate and this cap is what keeps a run from stalling | +| `allow_shared` [bool] | false | **prototype:** in groups with more tracks than detections (occlusion), losers share the winner's detection instead of dying | Enable where occlusions matter; validated on synthetic crossings (0 switches). Shared points move position but never velocity | +| `max_shared` [frames] | 2 | max consecutive shared frames per track | 2 covers brief overlaps; higher risks twin tracks that never separate | +| `share_tol` [cost] | 1.0 | a shared claim needs an edge cost below this (mutual-prediction gate) | Without it, a stranded track hijacks strangers' detections (observed live). ~5–10× your position noise; `null` disables the gate (not advised) | + +## 3. How it behaves (caveats) + +- **No motion model to be wrong** — but also none to help: if particles move + farther per frame than the typical spacing, every tracker fails, this one + first. Check step-vs-spacing before blaming parameters. +- **Gap survival is prediction-based:** a track coasts on `pos + vel` + through gaps up to `max_gap`; a maneuver inside the gap is lost. That is + by design — see `max_gap` above. +- **Occlusions:** with `allow_shared`, one detection may serve two tracks + for up to `max_shared` frames (marked in chains when + `return_chains=True`). Owners of the other engines: this is the reference + implementation of the shared-observation rule — same idea ports to + `track3d_loop_fast` (recorded, not claimed) and to linkage postprocess + (mark, then bridge). +- **Speed:** ~linear in particles (KD-tree search, small per-group + Hungarians); the `max_group_size` cap bounds the worst case. + +## 4. Tuning recipe + +1. Measure your typical step `s` (median linked displacement) and 3D noise + `n` (second-difference statistics — see `docs/algorithms/tracking.md`). +2. `v_max` ≈ 3·s. `leaf_weight` = 1 with decent calibration, 0 without. +3. Run; count short tracks (cold starts) vs gaps. More gaps than tracks → + raise `max_gap` to 3. More fragments at crossings → enable + `allow_shared`. +4. Validate on synthetic ground truth with the same spacing/noise before + trusting a production run (`tests/helpers/synthetic_scene.py` generates + scenes; `scripts/proto_shared_validate.py` shows the scoring pattern). diff --git a/pyproject.toml b/pyproject.toml index 6312bee5..0f7667f8 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -232,7 +232,7 @@ ignore = ["D1", "E501"] # E741: `l`, `I` mirror the matrix names in the C originals under test. # E402: imports after a sys.path tweak or an importorskip guard. "tests/**" = ["E402", "E741", "F401"] -"scripts/**" = ["E402"] +"scripts/**" = ["E402", "E741", "F401", "F841"] "skills/**" = ["E402", "F401"] "docs/**" = ["E402"] "test_data/**" = ["E402"] diff --git a/scripts/_run_one_step.py b/scripts/_run_one_step.py new file mode 100644 index 00000000..1c7407b7 --- /dev/null +++ b/scripts/_run_one_step.py @@ -0,0 +1,66 @@ +"""Run one forward tracking step and report links vs a reference run. + +Used to compare the COMPILED kernels against the pure-Python fallback on the +same input: run it once normally, once with the .pyd/.so files moved aside. + + uv run python scripts/_run_one_step.py --work --ref +""" + +from __future__ import annotations + +import argparse +import os +from pathlib import Path + + +def read_links(res: Path, frame: int): + p = res / f"ptv_is.{frame}" + lines = p.read_text().strip().splitlines() + n = int(lines[0]) + return [int(line.split()[0]) for line in lines[1 : n + 1]] + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument("--work", type=Path, required=True) + ap.add_argument("--ref", type=Path, required=True) + ap.add_argument("--frame", type=int, default=100002) + args = ap.parse_args() + + work = args.work.resolve() + ref = args.ref.resolve() + + import openptv2.algorithms.track as trk + + compiled = trk.is_compiled() + print(f"compiled kernels: {compiled}") + + from openptv2.gui.parameter_manager import ParameterManager + from openptv2.gui.ptv import py_start_proc_c + from openptv2.tracker import Tracker, default_naming + + old = os.getcwd() + os.chdir(work) + try: + pm = ParameterManager() + pm.from_yaml(work / "parameters_Run1.yaml") + cpar, spar, vpar, track_par, tpar, cals, epar = py_start_proc_c(pm) + for i, s in enumerate(pm.get_target_filenames()): + spar.set_img_base_name(i, str(Path(s).resolve()) + ".") + Tracker(cpar, vpar, track_par, spar, cals, default_naming).full_forward() + finally: + os.chdir(old) + + r = read_links(ref, args.frame) + t = read_links(work / "res", args.frame) + refn = sum(1 for p in r if p >= 0) + testn = sum(1 for p in t if p >= 0) + match = sum(1 for i in range(min(len(r), len(t))) if r[i] >= 0 and t[i] == r[i]) + print(f"\nframe {args.frame}") + print(f" reference (3dptv) links : {refn}") + print(f" this run links : {testn}") + print(f" reproduced exactly : {match} ({match / refn:.1%} of reference)") + + +if __name__ == "__main__": + main() diff --git a/scripts/ab_loser_retry.py b/scripts/ab_loser_retry.py new file mode 100644 index 00000000..1c9b75c3 --- /dev/null +++ b/scripts/ab_loser_retry.py @@ -0,0 +1,408 @@ +"""A/B the Phase-3 "loser retry" link resolution, with statistics. + +Phase 3 (track_kernels_corr.py) lets a particle that lost a contested candidate +claim its next-best free candidate. The original 3dptv track.c drops that +particle instead. This script runs BOTH on identical input and asks whether the +difference is real and which way it points. + +Two modes: + + synthetic curved trajectories + 3D position noise + ground truth. + Noise is the point: real PTV positions jitter either side of the + smooth path, so per-frame angle/acceleration are noise-dominated + and the acc/angle gate fires on noise, not physics. Straight clean + lines hide exactly the regime we care about. + Verdict metric: link precision / recall vs truth. + + folder a real working folder (res/rt_is.*, cal/, parameters_Run1.yaml). + No ground truth, so "better" is not directly observable -- we + report paired per-frame differences in link count and in the + acceleration tail (a wrong link injects a spurious large + acceleration, so a growing tail is evidence of bad links). + +Run from repo root: + uv run python scripts/ab_loser_retry.py synthetic + uv run python scripts/ab_loser_retry.py folder --path +""" + +from __future__ import annotations + +import argparse +import os +import shutil +from pathlib import Path + +import numpy as np + +from openptv2.algorithms.calibration import Calibration +from openptv2.algorithms.imgcoord import img_coord_batch +from openptv2.algorithms.parameters import ControlPar + +REPO = Path(__file__).resolve().parent.parent +SRC = REPO / "test_data" / "track" +NCAMS = 2 +FIRST = 10001 + + +# ----------------------------------------------------------------- synthetic + + +def make_tracks(n, n_frames, noise_mm, seed, speed=4.0, curve=0.35): + """Curved trajectories + isotropic 3D position noise. + + Returns (truth_clean, observed) as {pid: {frame: (x,y,z)}}. `observed` is + what goes into rt_is -- the noisy positions a real triangulation produces. + """ + rng = np.random.default_rng(seed) + truth, obs = {}, {} + for pid in range(n): + # Start on a loose grid so search volumes overlap and contests happen. + x0 = 10.0 + 14.0 * (pid % 5) + y0 = -16.0 + 7.0 * (pid // 5) + z0 = 35.0 + 4.0 * ((pid * 7) % 3) + phase = rng.uniform(0, 2 * np.pi) + truth[pid], obs[pid] = {}, {} + for f in range(n_frames): + t = float(f) + x = x0 + speed * t + y = y0 + curve * speed * np.sin(0.45 * t + phase) * 3.0 + z = z0 + curve * speed * np.cos(0.30 * t + phase) * 2.0 + truth[pid][FIRST + f] = (x, y, z) + obs[pid][FIRST + f] = ( + x + rng.normal(0, noise_mm), + y + rng.normal(0, noise_mm), + z + rng.normal(0, noise_mm), + ) + return truth, obs + + +def write_scene(work: Path, obs, n_frames): + """Project observed positions to targets + rt_is. Row order == pid order.""" + import sys + + sys.path.insert(0, str(Path(__file__).resolve().parent)) + from diag_speed_ceiling import write_rt_is, write_targets + + cpar = ControlPar.from_file(str(SRC / "parameters" / "ptv.par")) + cals = [ + Calibration.from_file( + str(SRC / "cal" / f"cam{c}.tif.ori"), + str(SRC / "cal" / f"cam{c}.tif.addpar"), + ) + for c in range(1, NCAMS + 1) + ] + img, res = work / "img", work / "res" + img.mkdir(exist_ok=True) + res.mkdir(exist_ok=True) + + pids = sorted(obs) + for f in range(n_frames): + frame = FIRST + f + per_cam = {c: [] for c in range(NCAMS)} + rows = [] + for pid in pids: + x, y, z = obs[pid][frame] + pnrs = [] + for ci in range(NCAMS): + xy = img_coord_batch( + np.array([[x, y, z]], dtype=np.float64), cals[ci], cpar.mm + )[0] + px = xy[0] / cpar.pix_x + cpar.imx / 2 + py = cpar.imy / 2 - xy[1] / cpar.pix_y + idx = len(per_cam[ci]) + per_cam[ci].append((idx, px, py, 50, 5, 5, 1000, len(rows))) + pnrs.append(idx) + while len(pnrs) < 4: + pnrs.append(-1) + rows.append((len(rows) + 1, x, y, z, *pnrs)) + write_rt_is(res / f"rt_is.{frame}", rows) + for ci in range(NCAMS): + write_targets(img / f"cam{ci + 1}.{frame}_targets", per_cam[ci]) + return pids + + +# ---------------------------------------------------------------- run / read + + +def run(work: Path, loser_retry: int, n_frames: int): + from openptv2.gui.parameter_manager import ParameterManager + from openptv2.gui.ptv import py_start_proc_c + from openptv2.tracker import Tracker, default_naming + + old = os.getcwd() + os.chdir(work) + try: + pm = ParameterManager() + pm.from_yaml(work / "parameters_Run1.yaml") + cpar, spar, vpar, track_par, tpar, cals, epar = py_start_proc_c(pm) + for cam_id, short in enumerate(pm.get_target_filenames()): + spar.set_img_base_name(cam_id, str(Path(short).resolve()) + ".") + t = Tracker( + cpar, vpar, track_par, spar, cals, default_naming, + loser_retry=loser_retry, + ) + t.full_forward() + finally: + os.chdir(old) + + +def read_links(res: Path, frame: int): + """prev column of ptv_is.: prev[i] is row i's parent in frame-1.""" + p = res / f"ptv_is.{frame}" + if not p.exists(): + return [] + lines = p.read_text().strip().splitlines() + if not lines: + return [] + n = int(lines[0]) + return [int(line.split()[0]) for line in lines[1 : n + 1]] + + +def read_xyz(res: Path, frame: int): + p = res / f"rt_is.{frame}" + lines = p.read_text().strip().splitlines() + n = int(lines[0]) + return np.array( + [[float(v) for v in line.split()[1:4]] for line in lines[1 : n + 1]] + ) + + +# ------------------------------------------------------------------ scoring + + +def score_truth(res: Path, n_frames): + """Link precision/recall. Row i is pid i in every frame by construction.""" + tp = fp = 0 + for f in range(1, n_frames): + prev = read_links(res, FIRST + f) + for i, p in enumerate(prev): + if p < 0: + continue + if p == i: + tp += 1 + else: + fp += 1 + return tp, fp + + +def per_frame_stats(res: Path, n_frames): + """Per frame: link count, mean step, 95th-pct acceleration magnitude.""" + nlink, mstep, a95 = [], [], [] + for f in range(1, n_frames): + prev = read_links(res, FIRST + f) + cur = read_xyz(res, FIRST + f) + old = read_xyz(res, FIRST + f - 1) + steps = [ + np.linalg.norm(cur[i] - old[p]) + for i, p in enumerate(prev) + if 0 <= p < len(old) and i < len(cur) + ] + nlink.append(len(steps)) + mstep.append(float(np.mean(steps)) if steps else 0.0) + + accs = [] + if f >= 2: + prev2 = read_links(res, FIRST + f - 1) + older = read_xyz(res, FIRST + f - 2) + for i, p in enumerate(prev): + if not (0 <= p < len(old) and i < len(cur)): + continue + pp = prev2[p] if p < len(prev2) else -1 + if 0 <= pp < len(older): + accs.append( + float(np.linalg.norm(cur[i] - 2 * old[p] + older[pp])) + ) + a95.append(float(np.percentile(accs, 95)) if accs else 0.0) + return np.array(nlink), np.array(mstep), np.array(a95) + + +def paired(name, a, b, unit=""): + """Paired difference B-A with a bootstrap CI and a Wilcoxon p.""" + from scipy import stats + + d = b - a + if np.allclose(d, 0): + print(f" {name:<28} identical") + return + rng = np.random.default_rng(0) + boots = [ + np.mean(rng.choice(d, size=len(d), replace=True)) for _ in range(4000) + ] + lo, hi = np.percentile(boots, [2.5, 97.5]) + try: + p = stats.wilcoxon(a, b).pvalue + except ValueError: + p = float("nan") + sig = "significant" if p < 0.05 else "n.s." + print( + f" {name:<28} retry-ON minus OFF = {np.mean(d):+8.4f}{unit} " + f"95% CI [{lo:+.4f}, {hi:+.4f}] p={p:.4g} ({sig})" + ) + + +# --------------------------------------------------------------------- main + + +def prepare(work: Path): + if work.exists(): + shutil.rmtree(work) + work.mkdir(parents=True) + shutil.copytree(SRC / "cal", work / "cal") + shutil.copytree(SRC / "parameters", work / "parameters") + shutil.copy(SRC / "parameters_Run1.yaml", work / "parameters_Run1.yaml") + + +def set_frames(work: Path, n_frames): + import yaml + + y = yaml.safe_load((work / "parameters_Run1.yaml").read_text()) + for key in ("sequence", "Sequence", "sequence_par"): + if key in y: + y[key]["first"] = FIRST + y[key]["last"] = FIRST + n_frames - 1 + (work / "parameters_Run1.yaml").write_text(yaml.safe_dump(y)) + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument("mode", choices=["synthetic", "folder", "gate"]) + ap.add_argument("--path", type=Path, help="working folder for mode=folder") + ap.add_argument("--frames", type=int, default=12) + ap.add_argument("--n", type=int, default=25, help="particles (synthetic)") + ap.add_argument("--noise", type=float, default=0.25, help="mm, 3D position") + ap.add_argument("--reps", type=int, default=8, help="noise seeds (synthetic)") + args = ap.parse_args() + + if args.mode == "gate": + # How much position noise can the per-frame acc gate survive? + # + # acc is |x[n+1] - 2x[n] + x[n-1]|. With independent noise sigma on + # each position, each component of that combination has variance + # (1+4+1)*sigma^2 = 6 sigma^2, so the 3D magnitude of the PURE NOISE + # acceleration is about sqrt(3*6)*sigma = 4.24*sigma -- before the + # particle has actually accelerated at all. Once 4.24*sigma reaches + # dacc, the gate is rejecting noise, not physics. + dacc = float((SRC / "parameters" / "track.par").read_text().split()[7]) + predicted = dacc / np.sqrt(18.0) + levels = [0.0, 0.1, 0.25, 0.4, 0.5, 0.75, 1.0, 1.5] + print(f"\ndacc = {dacc} predicted noise ceiling = dacc/sqrt(18) = " + f"{predicted:.3f} mm") + print(f"\n{'noise sigma':>12}{'4.24*sigma':>12}{'recall':>9}" + f"{'precision':>11} (vs dacc={dacc})") + xs, ys = [], [] + for nz in levels: + recs, precs = [], [] + for rep in range(args.reps): + work = REPO / "scratch" / f"_gate_{rep}" + prepare(work) + _, obs = make_tracks(args.n, args.frames, nz, seed=200 + rep) + write_scene(work, obs, args.frames) + set_frames(work, args.frames) + run(work, 1, args.frames) + tp, fp = score_truth(work / "res", args.frames) + recs.append(tp / (args.n * (args.frames - 1))) + precs.append(tp / (tp + fp) if tp + fp else 0.0) + r, p = float(np.mean(recs)), float(np.mean(precs)) + xs.append(nz) + ys.append(r) + flag = " <-- gate is eating noise" if 4.24 * nz > dacc else "" + print(f"{nz:>12.2f}{4.24 * nz:>12.2f}{r:>9.3f}{p:>11.3f}{flag}") + + import matplotlib + + matplotlib.use("Agg") + import matplotlib.pyplot as plt + + fig, ax = plt.subplots(figsize=(7, 4)) + ax.plot(xs, ys, "o-", color="#2a6ebb", label="link recall") + ax.axvline( + predicted, color="#c0392b", ls="--", + label=f"dacc/$\\sqrt{{18}}$ = {predicted:.2f} mm", + ) + ax.set_xlabel("3D position noise $\\sigma$ (mm)") + ax.set_ylabel("fraction of true links found") + ax.set_title("Per-frame acceleration gate vs position noise") + ax.set_ylim(-0.05, 1.05) + ax.grid(alpha=0.3) + ax.legend() + fig.tight_layout() + out = REPO / "scratch" / "gate_noise_ceiling.png" + fig.savefig(out, dpi=130) + print(f"\nwrote {out}") + return + + if args.mode == "synthetic": + rows = [] + for rep in range(args.reps): + work = REPO / "scratch" / f"_ab_syn_{rep}" + prepare(work) + _, obs = make_tracks(args.n, args.frames, args.noise, seed=100 + rep) + write_scene(work, obs, args.frames) + set_frames(work, args.frames) + + out = {} + for retry in (0, 1): + run(work, retry, args.frames) + tp, fp = score_truth(work / "res", args.frames) + out[retry] = (tp, fp) + rows.append(out) + + poss = args.n * (args.frames - 1) + print(f"\nsynthetic: {args.n} particles, {args.frames} frames, " + f"{args.noise} mm position noise, {args.reps} seeds") + print(f"{'':10}{'OFF (track.c)':>28}{'ON (openptv2)':>28}") + print(f"{'seed':10}{'TP':>7}{'FP':>7}{'prec':>7}{'rec':>7}" + f"{'TP':>7}{'FP':>7}{'prec':>7}{'rec':>7}") + pa, pb, ra, rb = [], [], [], [] + for i, o in enumerate(rows): + line = f"{i:<10}" + for retry in (0, 1): + tp, fp = o[retry] + prec = tp / (tp + fp) if tp + fp else 0.0 + rec = tp / poss + line += f"{tp:>7}{fp:>7}{prec:>7.3f}{rec:>7.3f}" + (pa if retry == 0 else pb).append(prec) + (ra if retry == 0 else rb).append(rec) + print(line) + + print("\npaired over seeds:") + paired("link precision", np.array(pa), np.array(pb)) + paired("link recall", np.array(ra), np.array(rb)) + return + + work = args.path.resolve() + if not (work / "res").is_dir(): + raise SystemExit(f"no res/ in {work}") + import yaml + + y = yaml.safe_load((work / "parameters_Run1.yaml").read_text()) + seq = next(y[k] for k in ("sequence", "Sequence", "sequence_par") if k in y) + first, last = int(seq["first"]), int(seq["last"]) + n_frames = last - first + 1 + + global FIRST + FIRST = first + + stats_by = {} + for retry in (0, 1): + run(work, retry, n_frames) + keep = work / f"res_retry{retry}" + if keep.exists(): + shutil.rmtree(keep) + shutil.copytree(work / "res", keep) + stats_by[retry] = per_frame_stats(keep, n_frames) + + print(f"\nreal folder: {work} frames {first}..{last}") + names = ["links per frame", "mean step (mm)", "95th-pct |accel| (mm)"] + units = ["", " mm", " mm"] + for k, (nm, un) in enumerate(zip(names, units)): + paired(nm, stats_by[0][k], stats_by[1][k], un) + print( + "\nNo ground truth here, so this shows DIFFERENT and which way -- not " + "better.\nA growing acceleration tail with retry ON is evidence the " + "extra links are wrong." + ) + + +if __name__ == "__main__": + main() diff --git a/scripts/analyze_residual.py b/scripts/analyze_residual.py new file mode 100644 index 00000000..664c7215 --- /dev/null +++ b/scripts/analyze_residual.py @@ -0,0 +1,252 @@ +"""Residual autopsy at matched parameters (dacc=1.9): WHAT differs, not how much. + +Compares 3dptv reference chains vs openptv2 chains on the same rt_is.*: + 1. chain census (lengths, singletons, full-span, ghost/added chains) + 2. break classification (head / tail / mid-split / merge / lost) + 3. smoothness (3-point acc distributions along each engine's own chains) + 4. contest analysis (at every divergence, whose link is smoother under + whose history -- is openptv2 wrong, or alternatively-valid?) + +Run from repo root (after a dacc=1.9 run in --work): + uv run python -u scripts/analyze_residual.py [--work scratch/_wp1_ab] +""" + +from __future__ import annotations + +import argparse +from pathlib import Path + +import numpy as np + +REPO = Path(__file__).resolve().parent.parent +DS = Path(r"C:\Users\alex\Downloads\HiDImaging\wp1_10_images") +REF = DS / "res_ground_truth_backup" +FIRST, LAST = 100001, 100010 + + +def read_pn(res: Path, frame: int): + lines = (res / f"ptv_is.{frame}").read_text().strip().splitlines() + n = int(lines[0]) + prev, nxt = [], [] + for line in lines[1: n + 1]: + p = line.split() + prev.append(int(p[0])) + nxt.append(int(p[1])) + return prev, nxt + + +def read_xyz(f: int): + lines = (REF / f"rt_is.{f}").read_text().strip().splitlines() + n = int(lines[0]) + return np.array([[float(v) for v in l.split()[1:4]] + for l in lines[1: n + 1]]) + + +def load(res: Path): + prev, nxt, n = {}, {}, {} + for f in range(FIRST, LAST + 1): + p, x = read_pn(res, f) + prev[f], nxt[f] = p, x + n[f] = len(p) + return prev, nxt, n + + +def chains(prev, nxt, nrows, first, last, row_cap=None): + """Maximal chains from heads (prev<0). Returns list of node tuples.""" + out = [] + for f in range(first, last + 1): + cap = nrows[f] if row_cap is None else min(nrows[f], row_cap[f]) + for i in range(min(cap, len(prev[f]))): + if prev[f][i] < 0: + ch = [(f, i)] + cf, ci = f, i + while (cf <= last and ci < len(nxt[cf]) + and nxt[cf][ci] >= 0): + ci = nxt[cf][ci] + cf += 1 + ch.append((cf, ci)) + out.append(tuple(ch)) + return out + + +def edges_of(ch): + return {(ch[k], ch[k + 1]) for k in range(len(ch) - 1)} + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument("--work", type=Path, default=REPO / "scratch" / "_wp1_ab") + args = ap.parse_args() + work = args.work.resolve() + tst_res = work / "res" + + rp, rn, ref_n = load(REF) + tp, tn, tst_n = load(tst_res) + print(f"rows/frame ref : {[ref_n[f] for f in range(FIRST, LAST + 1)]}") + print(f"rows/frame test: {[tst_n[f] for f in range(FIRST, LAST + 1)]}") + + XYZ = {f: read_xyz(f) for f in range(FIRST, LAST + 1)} + + ref_chains = chains(rp, rn, ref_n, FIRST, LAST) + # test chains restricted to shared rows for head-finding, but allowed to + # wander into added rows when followed + tst_chains = chains(tp, tn, tst_n, FIRST, LAST) + tst_shared = chains(tp, tn, tst_n, FIRST, LAST, + row_cap={f: ref_n[f] for f in range(FIRST, LAST + 1)}) + + # ---- 1. census ---- + print("\n== chain census ==") + for tag, chs in (("ref ", ref_chains), ("test", tst_chains)): + L = np.array([len(c) for c in chs]) + import collections + hist = collections.Counter(len(c) for c in chs) + print(f"{tag}: n={len(chs)} mean_len={L.mean():.2f} " + f"singletons={hist[1]} fullspan={hist[LAST - FIRST + 1]}") + print(f" len histogram: {dict(sorted(hist.items()))}") + + ghost_chains = [c for c in tst_chains + if any(i >= ref_n[f] for f, i in c)] + print(f"test chains touching added rows: {len(ghost_chains)} " + f"({len(ghost_chains) / len(tst_chains):.1%} of test chains)") + + # ---- 2. break classification ---- + ref_edge_set = set() + for c in ref_chains: + ref_edge_set |= edges_of(c) + tst_edge_set = set() + for c in tst_chains: + # only shared-row edges are comparable + for a, b in edges_of(c): + if a[1] < ref_n[a[0]] and b[1] < ref_n[b[0]]: + tst_edge_set.add((a, b)) + + # node -> ref chain id + node2ref = {} + for cid, c in enumerate(ref_chains): + for node in c: + node2ref[node] = cid + + exact = head = tail = mid = lost = 0 + merges = 0 + merge_examples = [] + for c in ref_chains: + E = [(c[k], c[k + 1]) for k in range(len(c) - 1)] + if not E: + continue + hit = [e in tst_edge_set for e in E] + if all(hit): + exact += 1 + continue + if not any(hit): + lost += 1 + continue + # partial: where are the misses? + miss_idx = [k for k, h in enumerate(hit) if not h] + if all(k == 0 for k in miss_idx): + head += 1 + elif all(k == len(E) - 1 for k in miss_idx): + tail += 1 + else: + mid += 1 + # merge: test edge leaving a node of this chain into another chain? + for k in miss_idx: + a = E[k][0] + if a[0] <= LAST and a[1] < len(tn[a[0]]) and tn[a[0]][a[1]] >= 0: + b2 = (a[0] + 1, tn[a[0]][a[1]]) + if (b2[1] < ref_n[b2[0]] and node2ref.get(b2, -1) != cid + and b2 in node2ref): + merges += 1 + if len(merge_examples) < 5: + merge_examples.append( + (a, E[k][1], b2, node2ref[b2])) + break + n_multi = len([c for c in ref_chains if len(c) > 1]) + print(f"\n== ref multi-node chains: {n_multi} ==") + print(f"exact={exact} head-miss={head} tail-miss={tail} mid-split={mid} " + f"lost={lost}") + print(f"chains where test merged away mid-chain: {merges}") + for a, bref, btest, ocid in merge_examples: + print(f" ref {a}->{bref}, test {a}->{btest} " + f"(joins ref chain {ocid})") + + # test-only edges (neither endpoint pair in ref): ghosts + merges + ref_nodes = set(node2ref) + test_only = [e for e in tst_edge_set if e not in ref_edge_set] + print(f"\ntest-only shared-row edges: {len(test_only)} " + f"({len(test_only) / max(len(tst_edge_set), 1):.1%} of test edges)") + + # ---- 3. smoothness: acc along each engine's OWN chains ---- + def acc_list(chs): + out = [] + for c in chs: + for k in range(1, len(c) - 1): + (f0, i0), (f1, i1), (f2, i2) = c[k - 1], c[k], c[k + 1] + if (f1 == f0 + 1 and f2 == f1 + 1 and i0 < ref_n[f0] + and i1 < ref_n[f1] and i2 < ref_n[f2]): + X0, X1, X2 = XYZ[f0][i0], XYZ[f1][i1], XYZ[f2][i2] + out.append(float(np.linalg.norm(X2 - 2 * X1 + X0))) + return np.array(out) + + ra, ta = acc_list(ref_chains), acc_list(tst_chains) + print("\n== smoothness (3-point acc on own chains, shared rows) ==") + for tag, a in (("ref ", ra), ("test", ta)): + print(f"{tag}: n={len(a)} mean={a.mean():.4f} " + f"p50={np.median(a):.4f} p90={np.percentile(a, 90):.4f} " + f"p99={np.percentile(a, 99):.4f} max={a.max():.4f}") + + # ---- 4. contest analysis at divergences ---- + # ref edge (a->b) missing, test has (a->c): compare acc of each choice + # under REF history (grandparent from ref chain) and under TEST history. + def gp_of(node, prev_map): + f, i = node + if f - 1 < FIRST or i >= len(prev_map[f]): + return None + p = prev_map[f][i] + if p < 0 or p >= ref_n[f - 1]: + return None + return (f - 1, p) + + smoother_test = smoother_ref = ties = 0 + inadmissible = 0 # test choice violates dv/dacc under ref history + acc_pairs = [] + for c in ref_chains: + for k in range(len(c) - 1): + a, b = c[k], c[k + 1] + if (a, b) in tst_edge_set: + continue + if a[1] >= len(tn[a[0]]): + continue + cc = tn[a[0]][a[1]] + if cc < 0 or cc >= ref_n[a[0] + 1]: + continue + cnode = (a[0] + 1, cc) + g = gp_of(a, rp) + if g is None: + continue + Xa, Xb, Xc, Xg = XYZ[a[0]][a[1]], XYZ[b[0]][b[1]], \ + XYZ[cnode[0]][cnode[1]], XYZ[g[0]][g[1]] + acc_ref = float(np.linalg.norm(Xb - 2 * Xa + Xg)) + acc_tst = float(np.linalg.norm(Xc - 2 * Xa + Xg)) + acc_pairs.append((acc_ref, acc_tst)) + if acc_tst < acc_ref - 1e-9: + smoother_test += 1 + elif acc_ref < acc_tst - 1e-9: + smoother_ref += 1 + else: + ties += 1 + if acc_tst >= 1.9: + inadmissible += 1 + P = np.array(acc_pairs) + print(f"\n== contests (ref edge missed, test chose elsewhere; n={len(P)}) ==") + print(f"test choice smoother: {smoother_test} " + f"({smoother_test / max(len(P), 1):.1%})") + print(f"ref choice smoother : {smoother_ref} " + f"({smoother_ref / max(len(P), 1):.1%})") + print(f"ties: {ties}; test choice acc>=dacc(1.9): {inadmissible}") + if len(P): + print(f"mean acc: ref-choice {P[:, 0].mean():.4f} vs " + f"test-choice {P[:, 1].mean():.4f}") + + +if __name__ == "__main__": + main() diff --git a/scripts/bench_4be_crossing.py b/scripts/bench_4be_crossing.py new file mode 100644 index 00000000..f09deae8 --- /dev/null +++ b/scripts/bench_4be_crossing.py @@ -0,0 +1,111 @@ +"""Can 4BE (four-frame best estimate) solve the hard crossings? + +4BE scores a candidate by whether it predicts a REAL particle two frames +ahead -- longer temporal context than trackcorr's one-frame lookahead plus +averaged gate. Same S-scenes, same seeds, same scoring as synth_crossing. + +Engines: trackcorr-parity (lr0), trackcorr-default (lr1), 4be (paper +defaults: STRICT_SUPPORT=0, GREEDY_CONFLICTS=0). + +Run from repo root: + uv run python -u scripts/bench_4be_crossing.py [--reps N] +""" + +from __future__ import annotations + +import argparse +import os +import sys +from pathlib import Path + +sys.path.insert(0, str(Path(__file__).resolve().parent)) + +import numpy as np # noqa: E402 +from synth_crossing import ( # noqa: E402 + REPO, + load_optics, + make_truth, + score, + setup_work, + write_scene, +) + + +def run_4be(work: Path): + from openptv2.gui.parameter_manager import ParameterManager + from openptv2.gui.ptv import py_start_proc_c + from openptv2.tracker import Tracker, default_naming + + old = os.getcwd() + os.chdir(work) + try: + pm = ParameterManager() + pm.from_yaml(work / "parameters_Run1.yaml") + cpar, spar, vpar, track_par, tpar, cals, epar = py_start_proc_c(pm) + for cam_id, short in enumerate(pm.get_target_filenames()): + spar.set_img_base_name(cam_id, str(Path(short).resolve()) + ".") + Tracker(cpar, vpar, track_par, spar, cals, + default_naming).full_forward_4be() + finally: + os.chdir(old) + + +def run_corr(work: Path, lr: int, cs: int): + from openptv2.gui.parameter_manager import ParameterManager + from openptv2.gui.ptv import py_start_proc_c + from openptv2.tracker import Tracker, default_naming + + old = os.getcwd() + os.chdir(work) + try: + pm = ParameterManager() + pm.from_yaml(work / "parameters_Run1.yaml") + cpar, spar, vpar, track_par, tpar, cals, epar = py_start_proc_c(pm) + for cam_id, short in enumerate(pm.get_target_filenames()): + spar.set_img_base_name(cam_id, str(Path(short).resolve()) + ".") + Tracker(cpar, vpar, track_par, spar, cals, default_naming, + loser_retry=lr, cold_start_neighbour=cs).full_forward() + finally: + os.chdir(old) + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument("--reps", type=int, default=8) + args = ap.parse_args() + + cpar, cals = load_optics() + scenarios = [ + ("S1 head-on", {"v": 0.5}), + ("S8 kick", {"v": 0.5, "maneuver": "kick"}), + ("S11 kick+noise", {"v": 0.5, "maneuver": "kick", "noise": 0.07}), + ("S12 convoy+noise", {"v": 0.5, "convoy": 0.6, "noise": 0.07}), + ("S4 head-on+noise", {"v": 0.5, "noise": 0.07}), + ] + engines = [ + ("corr-parity", lambda w: run_corr(w, 0, 0)), + ("corr-default", lambda w: run_corr(w, 1, 1)), + ("4be", run_4be), + ] + print(f"{'scenario':18}{'engine':14}{'recall':>9}{'cross-ok':>10}", + flush=True) + for sname, kw in scenarios: + noisy = kw.get("noise", 0.0) > 0 + reps = args.reps if noisy else 3 + for ename, run in engines: + recs, crs = [], [] + for rep in range(reps): + work = REPO / "scratch" / "_synth_4be" + setup_work(work) + truth = make_truth(seed=1000 + rep, **kw) + rows_pf = write_scene(work, truth, cpar, cals) + run(work) + r, c = score(work, rows_pf) + recs.append(r) + crs.append(c) + print(f"{sname:18}{ename:14}{np.mean(recs):>8.1%} " + f"{sum(crs)}/{len(crs)}", flush=True) + + +if __name__ == "__main__": + main() diff --git a/scripts/bench_two_phase.py b/scripts/bench_two_phase.py new file mode 100644 index 00000000..c7f338b4 --- /dev/null +++ b/scripts/bench_two_phase.py @@ -0,0 +1,185 @@ +"""Can the two-phase tracker solve the hard crossings? Plus a velocity-aware fix. + +Two-phase (plugins/two_phase_tracking.py): phase 1 = 3D KD-tree candidates +within v_max; phase 2 = Hungarian assignment on 2D pixel-distance costs. +Memoryless: no velocity prediction, no history. + +Prediction tested here: + (a) two-phase CANNOT do steady crossings (bounce bias: at an X-crossing + the swap assignment always has lower total cost than the cross), and + (b) whether adding constant-velocity prediction to its phase-1 search + points (pred = x + v) fixes crossings AND maneuvers. + +Engines: corr-default (reference), two-phase raw, two-phase+velocity. +Same S-scenes/seeds/scoring as synth_crossing. + +Run from repo root: + uv run python -u scripts/bench_two_phase.py [--reps N] +""" + +from __future__ import annotations + +import argparse +import sys +from pathlib import Path + +sys.path.insert(0, str(Path(__file__).resolve().parent)) + +import numpy as np # noqa: E402 +from synth_crossing import ( # noqa: E402 + REPO, + load_optics, + make_truth, + project, + setup_work, + write_scene, +) + +V_MAX = 3.3 # mirrors the dv +/-1.9 box (1.9*sqrt(3)) + + +def load_rt_positions(work: Path, nf: int, first: int): + frames = [] + for f in range(nf): + lines = (work / "res" / f"rt_is.{first + f}").read_text().splitlines() + n = int(lines[0]) + frames.append(np.array( + [[float(v) for v in l.split()[1:4]] for l in lines[1: n + 1]])) + return frames + + +def project_points(P, cpar, cals): + from synth_crossing import NCAMS + + n = len(P) + xy = np.full((n, NCAMS * 2), np.nan) + for i, (x, y, z) in enumerate(P): + for ci, (px, py) in enumerate(project(cpar, cals, x, y, z)): + xy[i, 2 * ci] = px + xy[i, 2 * ci + 1] = py + return np.nan_to_num(xy) + + +def make_leaves(positions, cpar, cals): + return [project_points(P, cpar, cals) for P in positions] + + +def run_productized(work, cpar, cals): + """The productized plugin class (velocity + gaps + re-projection).""" + from synth_crossing import FIRST as _F + from synth_crossing import NF as _N + + from openptv2.plugins.two_phase_tracking import ( + TwoPhaseTracker, + TwoPhaseTrackerConfig, + ) + + positions = load_rt_positions(work, _N, _F) + leaves = make_leaves(positions, cpar, cals) + cfg = TwoPhaseTrackerConfig(v_max=V_MAX, leaf_weight=1.0, + use_velocity=True, cost_mode="projected", + max_gap=2) + tr = TwoPhaseTracker(cfg) + return tr.track_frames( + [np.asarray(p) for p in positions], leaves, + project_fn=lambda P: project_points(np.asarray(P), cpar, cals)) + + +def run_two_phase(work, cpar, cals, nf, first, use_velocity: bool): + from openptv2.plugins.two_phase_tracking import _match_two_phase_frame + + positions = load_rt_positions(work, nf, first) + leaves = make_leaves(positions, cpar, cals) + n = len(positions[0]) + vel = {i: np.zeros(3) for i in range(n)} + links = [] # (t0, pid0, t1, pid1) + for t in range(nf - 1): + pts0, pts1 = positions[t], positions[t + 1] + if use_velocity: + pred = np.array([pts0[i] + vel[i] for i in range(n)]) + # costs must be evaluated AT the predicted positions: + # re-project predictions to pixels for the leaf signature + pred_leaves = project_points(pred, cpar, cals) + else: + pred, pred_leaves = pts0, leaves[t] + got = _match_two_phase_frame( + pred, pts1, pred_leaves, leaves[t + 1], + np.arange(n, dtype=np.int32), np.arange(n, dtype=np.int32), + V_MAX, 1.0) + for pid0, pid1 in got: + links.append((t, pid0, t + 1, pid1)) + if use_velocity: + new_vel = {} + for pid0, pid1 in got: + new_vel[pid1] = pts1[pid1] - pts0[pid0] + vel = {i: new_vel.get(i, np.zeros(3)) for i in range(n)} + return links + + +def score_links(links, rows_pf, nf, first): + # expected row->row links per pid from per-frame row maps (gap-aware: + # a pid absent at f-1 links from its last-seen row). + expected = set() + for pid_frames in _pid_frames(rows_pf, nf, first): + for (t0, r0), (t1, r1) in zip(pid_frames, pid_frames[1:]): + expected.add((t0, r0, t1, r1)) + got = set(links) + n_ok = len(expected & got) + n_tot = len(expected) + cross_ok = all(e in got for e in expected if e[2] == 4) + return (n_ok / n_tot if n_tot else 1.0), cross_ok + + +def _pid_frames(rows_pf, nf, first): + by_pid = {} + for f in range(nf): + for pid, row in rows_pf[first + f].items(): + by_pid.setdefault(pid, []).append((f, row)) + return list(by_pid.values()) + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument("--reps", type=int, default=8) + args = ap.parse_args() + + from synth_crossing import FIRST, NF + + cpar, cals = load_optics() + scenarios = [ + ("S1 head-on", {"v": 0.5}), + ("S8 kick", {"v": 0.5, "maneuver": "kick"}), + ("S11 kick+noise", {"v": 0.5, "maneuver": "kick", "noise": 0.07}), + ("S4 head-on+noise", {"v": 0.5, "noise": 0.07}), + ("S13 gap-occl", {"v": 0.5, "dropout": True}), + ] + print(f"{'scenario':16}{'engine':16}{'recall':>9}{'cross-ok':>10}", + flush=True) + for sname, kw in scenarios: + noisy = kw.get("noise", 0.0) > 0 + reps = args.reps if noisy else 3 + # prototype harnesses assume constant-N frames; the dropout scene + # (varying N) runs productized-only. + engines = ("productized",) if kw.get("dropout") else ( + "two-phase", "two-phase+vel", "productized") + for ename in engines: + recs, crs = [], [] + for rep in range(reps): + work = REPO / "scratch" / "_synth_2p" + setup_work(work) + truth = make_truth(seed=1000 + rep, **kw) + rows_pf = write_scene(work, truth, cpar, cals) + if ename == "productized": + links = run_productized(work, cpar, cals) + else: + links = run_two_phase(work, cpar, cals, NF, FIRST, + use_velocity=ename.endswith("+vel")) + r, c = score_links(links, rows_pf, NF, FIRST) + recs.append(r) + crs.append(c) + print(f"{sname:16}{ename:16}{np.mean(recs):>8.1%} " + f"{sum(crs)}/{len(crs)}", flush=True) + + +if __name__ == "__main__": + main() diff --git a/scripts/bench_wp1_trackers.py b/scripts/bench_wp1_trackers.py new file mode 100644 index 00000000..c6b490e4 --- /dev/null +++ b/scripts/bench_wp1_trackers.py @@ -0,0 +1,213 @@ +"""Benchmark trackcorr vs two-phase on wp1: speed + accuracy + trajectory quality. + +Phases (each timed): + A. trackcorr fwd+bwd (dacc=1.9 parity config) -> links/chains/smoothness + B. two-phase+vel (v_max=2.0, gap=1) -> links/chains/smoothness + C. trackcorr + postprocess (reciprocity + cold start + gap relink) -> delta +Smoothness = |acc| distribution over consecutive link triples vs reference. + +Run from repo root: + uv run python -u scripts/bench_wp1_trackers.py +""" + +from __future__ import annotations + +import shutil +import sys +import time +from pathlib import Path + +sys.path.insert(0, str(Path(__file__).resolve().parent)) + +import numpy as np # noqa: E402 +from verify_same_trajectories import ( # noqa: E402 + DS, + REF, + compare_links, + compare_tracks, +) + +WORK = (Path(__file__).resolve().parent.parent / "scratch" / "_wp1_ab").resolve() +FIRST, LAST = 100001, 100010 +SFIRST, SLAST = 100003, 100010 +NCAMS = 4 + + +def restore(): + for f in range(FIRST, LAST + 1): + shutil.copyfile(REF / f"rt_is.{f}", WORK / "res" / f"rt_is.{f}") + for cam in range(1, NCAMS + 1): + shutil.copyfile( + DS / "img_3dptv" / f"Cam{cam}.{f}_targets", + WORK / "img_3dptv" / f"Cam{cam}.{f}_targets") + + +def read_prev(d: Path, f: int): + lines = (d / f"ptv_is.{f}").read_text().splitlines() + n = int(lines[0]) + return [int(l.split()[0]) for l in lines[1: n + 1]] + + +def read_xyz(d: Path, f: int): + lines = (d / f"rt_is.{f}").read_text().splitlines() + n = int(lines[0]) + return np.array([[float(v) for v in l.split()[1:4]] for l in lines[1: n + 1]]) + + +def acc_stats(link_dir: Path, tag: str): + """|acc| over consecutive triples following test linkage.""" + accs, n_links = [], 0 + XYZ = {f: read_xyz(WORK / "res", f) for f in range(FIRST, LAST + 1)} + for f in range(SFIRST, SLAST + 1): + pv = read_prev(link_dir, f) + pp = read_prev(link_dir, f - 1) + X0, X1, X2 = XYZ[f - 2], XYZ[f - 1], XYZ[f] + for i in range(min(len(pv), len(X2))): + p = pv[i] + if p < 0 or p >= len(X1): + continue + q = pp[p] if p < len(pp) else -1 + if q < 0 or q >= len(X0): + continue + n_links += 1 + accs.append(float(np.linalg.norm((X2[i] - X1[p]) - (X1[p] - X0[q])))) + accs = np.array(accs) + print(f"[{tag}] triples={len(accs)} acc mean={accs.mean():.3f} " + f"p90={np.percentile(accs, 90):.3f} " + f"max={accs.max():.3f} rough(>1.9)={(accs > 1.9).sum()}", + flush=True) + + +def chain_stats(links, positions, frames, tag: str): + nxt = {} + for (t0, r0, t1, r1) in links: + nxt[(t0, r0)] = (t1, r1) + tgt = set(nxt.values()) + chains, visited = [], set() + for s in sorted(set(nxt) - tgt): + c, k = [s], s + while k in nxt and nxt[k] not in visited: + visited.add(k) + k = nxt[k] + c.append(k) + chains.append(c) + lens = np.array([len(c) for c in chains]) + print(f"[{tag}] chains={len(chains)} len mean={lens.mean():.2f} " + f"median={np.median(lens):.0f} max={lens.max()} " + f"fullspan10={(lens >= 10).sum()}", flush=True) + + +def main(): + import os + + from openptv2.gui.parameter_manager import ParameterManager + from openptv2.gui.ptv import py_start_proc_c + from openptv2.tracker import Tracker, default_naming + + # ---- A. trackcorr ---- + restore() + os.chdir(WORK) + try: + t0 = time.perf_counter() + pm = ParameterManager() + pm.from_yaml(WORK / "parameters_Run_dacc19.yaml") + cpar, spar, vpar, track_par, tpar, cals, epar = py_start_proc_c(pm) + tr = Tracker(cpar, vpar, track_par, spar, cals, default_naming, + loser_retry=0, cold_start_neighbour=0) + tr.full_forward() + tr.full_backward() + t_tc = time.perf_counter() - t0 + finally: + os.chdir(Path.cwd()) + print(f"[trackcorr fwd+bwd] wall={t_tc:.1f}s", flush=True) + compare_links(WORK, SFIRST, SLAST, "trackcorr") + compare_tracks(WORK) + acc_stats(WORK / "res", "trackcorr-smooth") + acc_stats(REF, "ref-smooth") + + # ---- C. postprocess on trackcorr output ---- + os.chdir(WORK) + try: + t0 = time.perf_counter() + stats = tr.postprocess(cold_start=True, reciprocity=True, + gap_relinking=True, max_gap=2) + t_pp = time.perf_counter() - t0 + finally: + os.chdir(Path.cwd()) + print(f"[postprocess] wall={t_pp:.1f}s stats={stats}", flush=True) + compare_links(WORK, SFIRST, SLAST, "trackcorr+post") + compare_tracks(WORK) + acc_stats(WORK / "res", "post-smooth") + + # ---- B. two-phase ---- + from openptv2.algorithms.calibration import Calibration + from openptv2.algorithms.imgcoord import img_coord_batch + from openptv2.algorithms.parameters import ControlPar + from openptv2.plugins.two_phase_tracking import ( + TwoPhaseTracker, + TwoPhaseTrackerConfig, + ) + restore() + cpar = ControlPar.from_file(str(WORK / "parameters" / "ptv.par")) + cals = [Calibration.from_file( + str(WORK / "cal" / f"cam_{c}.tif.ori"), + str(WORK / "cal" / f"cam_{c}.tif.addpar")) for c in range(1, NCAMS + 1)] + frames = list(range(FIRST, LAST + 1)) + t0 = time.perf_counter() + positions, leaves = [], [] + for f in frames: + P = read_xyz(WORK / "res", f) + positions.append(P) + xy = np.full((len(P), NCAMS * 2), np.nan) + for i, (x, y, z) in enumerate(P): + for ci in range(NCAMS): + m = img_coord_batch(np.array([[x, y, z]]), cals[ci], cpar.mm)[0] + xy[i, 2 * ci] = m[0] / cpar.pix_x + cpar.imx / 2 + xy[i, 2 * ci + 1] = cpar.imy / 2 - m[1] / cpar.pix_y + leaves.append(np.nan_to_num(xy)) + + def project_fn(pred): + pred = np.asarray(pred, dtype=np.float64) + out = np.full((len(pred), NCAMS * 2), np.nan) + for i, (x, y, z) in enumerate(pred): + for ci in range(NCAMS): + m = img_coord_batch(np.array([[x, y, z]]), cals[ci], cpar.mm)[0] + out[i, 2 * ci] = m[0] / cpar.pix_x + cpar.imx / 2 + out[i, 2 * ci + 1] = cpar.imy / 2 - m[1] / cpar.pix_y + return np.nan_to_num(out) + + cfg = TwoPhaseTrackerConfig(v_max=2.0, leaf_weight=1.0, use_velocity=True, + cost_mode="projected", max_gap=1) + links = TwoPhaseTracker(cfg).track_frames(positions, leaves, + project_fn=project_fn) + t_tp = time.perf_counter() - t0 + print(f"[two-phase+vel] wall={t_tp:.1f}s links={len(links)}", flush=True) + ref_prev = {} + for k, f in enumerate(frames): + ref_prev[k] = read_prev(REF, f) + ref_total = sum(sum(1 for p in ref_prev[k] if p >= 0) + for k, f in enumerate(frames) if SFIRST <= f <= SLAST) + exact = sum( + 1 for (t0i, r0, t1, r1) in links + if SFIRST <= frames[t1] <= SLAST and t1 - t0i == 1 + and r1 < len(ref_prev[t1]) and ref_prev[t1][r1] == r0) + print(f"[two-phase+vel] ref={ref_total} exact={exact} " + f"recall={exact / ref_total:.2%}", flush=True) + chain_stats(links, positions, frames, "two-phase-chains") + # smoothness over two-phase consecutive links + mp = {(t1, r1): (t0i, r0) for (t0i, r0, t1, r1) in links if t1 - t0i == 1} + accs = [] + for (t1, r1), (t0i, r0) in mp.items(): + if (t0i, r0) in mp and t0i - mp[(t0i, r0)][0] == 1: + t_1, r_1 = mp[(t0i, r0)] + a = positions[t1][r1] - positions[t0i][r0] + b = positions[t0i][r0] - positions[t_1][r_1] + accs.append(float(np.linalg.norm(a - b))) + accs = np.array(accs) + print(f"[two-phase-smooth] triples={len(accs)} acc mean={accs.mean():.3f} " + f"p90={np.percentile(accs, 90):.3f} max={accs.max():.3f} " + f"rough(>1.9)={(accs > 1.9).sum()}", flush=True) + + +if __name__ == "__main__": + main() diff --git a/scripts/cascade_track.py b/scripts/cascade_track.py new file mode 100644 index 00000000..37c8aaf5 --- /dev/null +++ b/scripts/cascade_track.py @@ -0,0 +1,56 @@ +"""Cascade tracker: trackcorr first, two-phase+vel only on leftovers. + +Phase 1: full trackcorr forward (+optional backward) -- high precision base. +Phase 2: productized TwoPhaseTracker (+velocity, re-projected costs) runs +on the same positions; its links are merged ADDITIVELY: a two-phase link +(a -> b) is accepted iff trackcorr left BOTH ends free (next[a] == -1 and +prev[b] == -1). Existing links are never stolen or rewritten -- the merge +can only extend heads/tails and bridge gaps, exactly where maneuvers hide. + +Outputs go to _cascade/ (the trackcorr res/ is untouched). +""" + +from __future__ import annotations + +from pathlib import Path + +import numpy as np + + +def merge_links(corr_prev, corr_next, tp_links, nrows): + """Additive merge of two-phase links into trackcorr linkage. + + corr_prev/next: lists per frame offset of prev/next arrays (row space). + tp_links: iterable of (t0, r0, t1, r1) in the same frame-offset/row space. + nrows: rows per frame offset. + Returns (merged_prev, merged_next, n_added). + """ + mprev = [np.array(p, dtype=np.int32) for p in corr_prev] + mnxt = [np.array(p, dtype=np.int32) for p in corr_next] + added = [] + for t0, r0, t1, r1 in tp_links: + if not (0 <= t0 < len(mnxt) and 0 <= t1 < len(mprev)): + continue + if not (0 <= r0 < nrows[t0] and 0 <= r1 < nrows[t1]): + continue + # Free tail: trackcorr marks "no outgoing link" as -1 (never + # claimed) or -2 (lost contest / dropped); both are linkable. + # Free head: prev == -1. Anything else is taken -- never steal. + if mnxt[t0][r0] < 0 and mprev[t1][r1] < 0: + mnxt[t0][r0] = r1 + mprev[t1][r1] = r0 + added.append((t0, r0, t1, r1)) + return mprev, mnxt, len(added), added + + +def write_ptv_is(out: Path, frames, mprev, mnxt, positions): + """Write merged linkage + positions in ptv_is format.""" + out.mkdir(parents=True, exist_ok=True) + for k, f in enumerate(frames): + n = len(positions[k]) + with open(out / f"ptv_is.{f}", "w") as fh: + fh.write(f"{n}\n") + for i in range(n): + x, y, z = positions[k][i] + fh.write(f"{int(mprev[k][i]):4d} {int(mnxt[k][i]):4d} " + f"{x:10.3f} {y:10.3f} {z:10.3f}\n") diff --git a/scripts/check_dv_hard.py b/scripts/check_dv_hard.py new file mode 100644 index 00000000..35ad988f --- /dev/null +++ b/scripts/check_dv_hard.py @@ -0,0 +1,39 @@ +"""Per-axis legality of the 34 hard links vs dv=1.9.""" + +from pathlib import Path + +import numpy as np + +DS = Path(r"C:\Users\alex\Downloads\HiDImaging\wp1_10_images") +REF = DS / "res_ground_truth_backup" + + +def read_pn(d, f): + lines = (d / f"ptv_is.{f}").read_text().splitlines() + n = int(lines[0]) + return [int(l.split()[0]) for l in lines[1: n + 1]] + + +def read_xyz(f): + lines = (REF / f"rt_is.{f}").read_text().splitlines() + n = int(lines[0]) + return np.array([[float(v) for v in l.split()[1:4]] for l in lines[1: n + 1]]) + + +def main(): + import csv + rows = list(csv.DictReader(open("scratch/hard_links/hard_links.csv"))) + print(" frm row dx dy dz maxax dv") + for r in rows: + f, i = int(r["frame"]), int(r["row"]) + X1, X3 = read_xyz(f - 1), read_xyz(f) + p = int(r["ref_prev"]) + d = np.array(X3[i]) - np.array(X1[p]) + m = float(np.abs(d).max()) + flag = "VIOL" if m >= 1.9 else "ok" + print(f"{f:>7}{i:>5}{d[0]:>7.2f}{d[1]:>7.2f}{d[2]:>7.2f}" + f"{m:>7.2f} {flag}") + + +if __name__ == "__main__": + main() diff --git a/scripts/collect_ensemble_wp1.py b/scripts/collect_ensemble_wp1.py new file mode 100644 index 00000000..d3e8115d --- /dev/null +++ b/scripts/collect_ensemble_wp1.py @@ -0,0 +1,242 @@ +"""Ensemble adjudication on wp1: four diverse engines vote per link. + +Engines (all on identical pristine inputs): + parity : trackcorr loser_retry=0 (existing scratch/_wp1_ab/res) + default : trackcorr loser_retry=1 fwd+bwd (fresh clone) + 4be : four-frame best estimate fwd (fresh clone) + 2p : two-phase+vel v_max=2.0 (arrays -> ptv files) + +Vote per (frame,row): unanimous / majority / union / intersection sets, +each scored vs the 3dptv reference. Answers: is there a ghost-free +high-precision subset, and does voting beat the best single engine? + +Run from repo root (detached, ~10 min): + uv run python -u scripts/collect_ensemble_wp1.py +""" + +from __future__ import annotations + +import os +import shutil +from pathlib import Path + +import numpy as np + +REPO = Path(__file__).resolve().parent.parent +DS = Path(r"C:\Users\alex\Downloads\HiDImaging\wp1_10_images") +REF = DS / "res_ground_truth_backup" +SRC = REPO / "scratch" / "_wp1_ab" +FIRST, LAST = 100001, 100010 +SCORE_FIRST, SCORE_LAST = 100003, 100010 +NCAMS = 4 + + +def setup_clone(tag: str) -> Path: + work = REPO / "scratch" / f"_ens_{tag}" + if work.exists(): + shutil.rmtree(work) + (work / "img_3dptv").mkdir(parents=True) + (work / "res").mkdir(parents=True) + shutil.copytree(SRC / "cal", work / "cal") + shutil.copytree(SRC / "parameters", work / "parameters") + shutil.copy(SRC / "parameters_Run_dacc19.yaml", + work / "parameters_Run1.yaml") + for f in range(FIRST, LAST + 1): + shutil.copyfile(REF / f"rt_is.{f}", work / "res" / f"rt_is.{f}") + for cam in range(1, NCAMS + 1): + shutil.copyfile( + DS / "img_3dptv" / f"Cam{cam}.{f}_targets", + work / "img_3dptv" / f"Cam{cam}.{f}_targets") + return work + + +def run_corr(work: Path, lr: int): + from openptv2.gui.parameter_manager import ParameterManager + from openptv2.gui.ptv import py_start_proc_c + from openptv2.tracker import Tracker, default_naming + + old = os.getcwd() + os.chdir(work) + try: + pm = ParameterManager() + pm.from_yaml(work / "parameters_Run1.yaml") + cpar, spar, vpar, track_par, tpar, cals, epar = py_start_proc_c(pm) + for cam_id, short in enumerate(pm.get_target_filenames()): + spar.set_img_base_name(cam_id, str(Path(short).resolve()) + ".") + tr = Tracker(cpar, vpar, track_par, spar, cals, default_naming, + loser_retry=lr, cold_start_neighbour=0) + tr.full_forward() + tr.full_backward() + finally: + os.chdir(old) + + +def run_4be(work: Path): + from openptv2.gui.parameter_manager import ParameterManager + from openptv2.gui.ptv import py_start_proc_c + from openptv2.tracker import Tracker, default_naming + + old = os.getcwd() + os.chdir(work) + try: + pm = ParameterManager() + pm.from_yaml(work / "parameters_Run1.yaml") + cpar, spar, vpar, track_par, tpar, cals, epar = py_start_proc_c(pm) + for cam_id, short in enumerate(pm.get_target_filenames()): + spar.set_img_base_name(cam_id, str(Path(short).resolve()) + ".") + Tracker(cpar, vpar, track_par, spar, cals, + default_naming).full_forward_4be() + finally: + os.chdir(old) + + +def run_2p(work: Path) -> Path: + from openptv2.algorithms.calibration import Calibration + from openptv2.algorithms.imgcoord import img_coord_batch + from openptv2.algorithms.parameters import ControlPar + from openptv2.plugins.two_phase_tracking import ( + TwoPhaseTracker, + TwoPhaseTrackerConfig, + ) + + cpar = ControlPar.from_file(str(work / "parameters" / "ptv.par")) + cals = [Calibration.from_file( + str(work / "cal" / f"cam_{c}.tif.ori"), + str(work / "cal" / f"cam_{c}.tif.addpar")) + for c in range(1, NCAMS + 1)] + frames = list(range(FIRST, LAST + 1)) + + def proj(P): + P = np.asarray(P, dtype=np.float64) + out = np.full((len(P), NCAMS * 2), np.nan) + for i, (x, y, z) in enumerate(P): + for ci in range(NCAMS): + m = img_coord_batch(np.array([[x, y, z]]), cals[ci], + cpar.mm)[0] + out[i, 2 * ci] = m[0] / cpar.pix_x + cpar.imx / 2 + out[i, 2 * ci + 1] = cpar.imy / 2 - m[1] / cpar.pix_y + return np.nan_to_num(out) + + positions, leaves = [], [] + for f in frames: + lines = (work / "res" / f"rt_is.{f}").read_text().splitlines() + n = int(lines[0]) + P = np.array([[float(v) for v in l.split()[1:4]] + for l in lines[1: n + 1]]) + positions.append(P) + leaves.append(proj(P)) + cfg = TwoPhaseTrackerConfig(v_max=2.0, leaf_weight=1.0, use_velocity=True, + cost_mode="projected", max_gap=1) + links = TwoPhaseTracker(cfg).track_frames(positions, leaves, + project_fn=proj) + nrows = [len(P) for P in positions] + tprev = [np.full(n, -1) for n in nrows] + tnxt = [np.full(n, -1) for n in nrows] + for t0, r0, t1, r1 in links: + if 0 <= t0 < len(frames) and 0 <= t1 < len(frames): + if r0 < nrows[t0] and r1 < nrows[t1]: + tnxt[t0][r0] = r1 + tprev[t1][r1] = r0 + out = work / "res_2p" + out.mkdir(exist_ok=True) + for k, f in enumerate(frames): + with open(out / f"ptv_is.{f}", "w") as fh: + fh.write(f"{nrows[k]}\n") + for i in range(nrows[k]): + x, y, z = positions[k][i] + fh.write(f"{int(tprev[k][i]):4d} {int(tnxt[k][i]):4d} " + f"{x:10.3f} {y:10.3f} {z:10.3f}\n") + return out + + +def read_prev(res: Path, f: int): + lines = (res / f"ptv_is.{f}").read_text().splitlines() + n = int(lines[0]) + return [int(l.split()[0]) for l in lines[1: n + 1]] + + +def main(): + print("collecting engines...", flush=True) + w_def = setup_clone("lr1") + run_corr(w_def, 1) + w_4be = setup_clone("4be") + run_4be(w_4be) + w_2p = setup_clone("2p") + res_2p = run_2p(w_2p) + + engs = { + "parity": SRC / "res", + "default": w_def / "res", + "4be": w_4be / "res", + "2p": res_2p, + } + P = {name: {f: read_prev(res, f) for f in range(FIRST, LAST + 1)} + for name, res in engs.items()} + R = {f: read_pn_ref(f) for f in range(FIRST, LAST + 1)} + + names = list(engs) + single = {} + for name in names: + tot = mt = 0 + for f in range(SCORE_FIRST, SCORE_LAST + 1): + r, t = R[f], P[name][f] + for i in range(min(len(r), len(t))): + if r[i] >= 0: + tot += 1 + mt += t[i] == r[i] + single[name] = mt / tot + print(f"{name:8} recall={mt / tot:.2%} ({mt}/{tot})", flush=True) + + # votes + una_t = una_m = maj_t = maj_m = uni_m = inter_m = 0 + contested = agree_ref = 0 + win_counts = {n: 0 for n in names} + for f in range(SCORE_FIRST, SCORE_LAST + 1): + r = R[f] + T = {n: P[n][f] for n in names} + n = min(len(r), *(len(T[x]) for x in names)) + for i in range(n): + if r[i] < 0: + continue + votes = [T[x][i] for x in names] + vals = [v for v in votes if v is not None] + top = max(set(vals), key=vals.count) + cnt = vals.count(top) + if cnt == len(names): + una_t += 1 + una_m += top == r[i] + if cnt >= 3: + maj_t += 1 + maj_m += top == r[i] + if any(v == r[i] for v in vals): + uni_m += 1 + if all(v == r[i] for v in vals): + inter_m += 1 + if not all(v == vals[0] for v in vals): + contested += 1 + agree_ref += top == r[i] + for x in names: + if T[x][i] == r[i]: + win_counts[x] += 1 + tot = sum(1 for f in range(SCORE_FIRST, SCORE_LAST + 1) + for p in R[f] if p >= 0) + print(f"\nref links scored: {tot}", flush=True) + print(f"unanimous(4/4): n={una_t} precision={una_m / una_t:.2%}", + flush=True) + print(f"majority(>=3) : n={maj_t} precision={maj_m / maj_t:.2%} " + f"recall={maj_m / tot:.2%}", flush=True) + print(f"union (any engine right): recall={uni_m / tot:.2%}", flush=True) + print(f"intersection (all right): recall={inter_m / tot:.2%}", flush=True) + print(f"contested links: {contested} ({contested / tot:.1%}); " + f"majority right: {agree_ref / contested:.1%}", flush=True) + print(f"wins on contested: {win_counts}", flush=True) + + +def read_pn_ref(f: int): + lines = (REF / f"ptv_is.{f}").read_text().splitlines() + n = int(lines[0]) + return [int(l.split()[0]) for l in lines[1: n + 1]] + + +if __name__ == "__main__": + main() diff --git a/scripts/compare_single_trajectories.py b/scripts/compare_single_trajectories.py new file mode 100644 index 00000000..fd6c1e4b --- /dev/null +++ b/scripts/compare_single_trajectories.py @@ -0,0 +1,268 @@ +"""Single-trajectory comparison: what does the residual 2% LOOK like? + +Finds the most illustrative divergent trajectories (mid-splits, merges, +lost chains, ghost extensions), prints frame-by-frame tables (positions, +step, acc for each engine's choice) and renders ref-vs-test overlay plots. + +Run from repo root (uses existing outputs, no tracking run): + uv run python -u scripts/compare_single_trajectories.py \ + --work scratch/_wp1_ab --out scratch/single_traj +""" + +from __future__ import annotations + +import argparse +from pathlib import Path + +import numpy as np + +REPO = Path(__file__).resolve().parent.parent +DS = Path(r"C:\Users\alex\Downloads\HiDImaging\wp1_10_images") +REF = DS / "res_ground_truth_backup" +FIRST, LAST = 100001, 100010 + + +def read_pn(res: Path, frame: int): + lines = (res / f"ptv_is.{frame}").read_text().strip().splitlines() + n = int(lines[0]) + prev, nxt = [], [] + for line in lines[1: n + 1]: + p = line.split() + prev.append(int(p[0])) + nxt.append(int(p[1])) + return prev, nxt + + +def read_xyz(f: int): + lines = (REF / f"rt_is.{f}").read_text().strip().splitlines() + n = int(lines[0]) + return np.array([[float(v) for v in l.split()[1:4]] + for l in lines[1: n + 1]]) + + +def load(res: Path): + prev, nxt, n = {}, {}, {} + for f in range(FIRST, LAST + 1): + p, x = read_pn(res, f) + prev[f], nxt[f] = p, x + n[f] = len(p) + return prev, nxt, n + + +def chains(prev, nxt, nrows): + out = [] + for f in range(FIRST, LAST + 1): + for i in range(nrows[f]): + if prev[f][i] < 0: + ch = [(f, i)] + cf, ci = f, i + while cf <= LAST and ci < len(nxt[cf]) and nxt[cf][ci] >= 0: + ci = nxt[cf][ci] + cf += 1 + ch.append((cf, ci)) + out.append(tuple(ch)) + return out + + +def pos(node, XYZ, nrows): + f, i = node + if i >= nrows[f]: + return None + return XYZ[f][i] + + +def acc(Xg, Xa, Xb): + if Xg is None: + return float("nan") + return float(np.linalg.norm(Xb - 2 * Xa + Xg)) + + +def show_case(title, ref_chain, tst_chain, XYZ, ref_n, rp, tp): + print(f"\n{'=' * 72}\n{title}\n{'=' * 72}") + print(f" ref chain ({len(ref_chain)} nodes): " + + " -> ".join(f"{f}:{i}" for f, i in ref_chain)) + if tst_chain is not None: + print(f" test chain ({len(tst_chain)} nodes): " + + " -> ".join(f"{f}:{i}" for f, i in tst_chain)) + print(f" {'frame':>8} {'ref node':>9} {'x,y,z':>24} " + f"{'step':>6} {'acc':>6} | {'tst node':>9} {'step':>6} {'acc':>6}") + # walk frames covered by either chain + frames = sorted({f for f, _ in ref_chain} | + ({f for f, _ in tst_chain} if tst_chain else set())) + rmap = dict(zip([f for f, _ in ref_chain], ref_chain)) + tmap = dict(zip([f for f, _ in tst_chain], tst_chain)) if tst_chain else {} + rprev_node, tprev_node = None, None + rgp, tgp = None, None + for f in frames: + rnode, tnode = rmap.get(f), tmap.get(f) + Xr = pos(rnode, XYZ, ref_n) if rnode else None + Xt = pos(tnode, XYZ, ref_n) if tnode else None + rstep = (float(np.linalg.norm(Xr - pos(rprev_node, XYZ, ref_n))) + if Xr is not None and rprev_node is not None + and pos(rprev_node, XYZ, ref_n) is not None else float("nan")) + tstep = (float(np.linalg.norm(Xt - pos(tprev_node, XYZ, ref_n))) + if Xt is not None and tprev_node is not None + and pos(tprev_node, XYZ, ref_n) is not None else float("nan")) + racc = acc(pos(rgp, XYZ, ref_n) if rgp else None, + pos(rprev_node, XYZ, ref_n) if rprev_node else None, Xr) \ + if Xr is not None and rprev_node else float("nan") + tacc = acc(pos(tgp, XYZ, ref_n) if tgp else None, + pos(tprev_node, XYZ, ref_n) if tprev_node else None, Xt) \ + if Xt is not None and tprev_node else float("nan") + mark = " " if rnode == tnode else ">>" + print(f"{mark} {f:>8} {str(rnode):>9} " + f"{('%.2f,%.2f,%.2f' % tuple(Xr)) if Xr is not None else '-':>24} " + f"{rstep:6.3f} {racc:6.3f} | {str(tnode):>9} " + f"{tstep:6.3f} {tacc:6.3f}") + if rnode: + rgp, rprev_node = rprev_node, rnode + if tnode: + tgp, tprev_node = tprev_node, tnode + + +def plot_case(title, ref_chain, tst_chain, XYZ, ref_n, path): + import matplotlib + matplotlib.use("Agg") + import matplotlib.pyplot as plt + + fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(11, 4.5)) + fig.suptitle(title, fontsize=10) + R = np.array([XYZ[f][i] for f, i in ref_chain + if i < ref_n[f]]) + ax1.plot(R[:, 0], R[:, 1], "o-", color="#2a6ebb", label="3dptv", + markersize=7) + ax2.plot(R[:, 0], R[:, 2], "o-", color="#2a6ebb", label="3dptv", + markersize=7) + if tst_chain: + T = np.array([XYZ[f][i] for f, i in tst_chain + if i < ref_n[f]]) + ax1.plot(T[:, 0], T[:, 1], "s--", color="#c0392b", + label="openptv2", markersize=6) + ax2.plot(T[:, 0], T[:, 2], "s--", color="#c0392b", + label="openptv2", markersize=6) + # divergence node: first frame where they differ + rm = dict(zip([f for f, _ in ref_chain], ref_chain)) + tm = dict(zip([f for f, _ in tst_chain], tst_chain)) if tst_chain else {} + for f in sorted(set(rm) & set(tm)): + if rm[f] != tm[f]: + for ax, xy in ((ax1, (0, 1)), (ax2, (0, 2))): + P = XYZ[f][rm[f][1]] + ax.annotate("split", (P[xy[0]], P[xy[1]]), + textcoords="offset points", xytext=(8, 8), + fontsize=9, color="black", + arrowprops={"arrowstyle": "->"}) + break + for ax, xl, yl in ((ax1, "x (mm)", "y (mm)"), (ax2, "x (mm)", "z (mm)")): + ax.set_xlabel(xl) + ax.set_ylabel(yl) + ax.legend(fontsize=9) + ax.grid(alpha=0.3) + ax.set_aspect("equal", adjustable="datalim") + fig.tight_layout() + fig.savefig(path, dpi=130) + print(f" wrote {path}") + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument("--work", type=Path, default=REPO / "scratch" / "_wp1_ab") + ap.add_argument("--out", type=Path, default=REPO / "scratch" / "single_traj") + args = ap.parse_args() + out = args.out + out.mkdir(parents=True, exist_ok=True) + tst_res = args.work.resolve() / "res" + + rp, rn, ref_n = load(REF) + tp, tn, tst_n = load(tst_res) + XYZ = {f: read_xyz(f) for f in range(FIRST, LAST + 1)} + + ref_chains = chains(rp, rn, ref_n) + tst_chains = chains(tp, tn, tst_n) + tst_set = set(tst_chains) + node2tst = {} + for c in tst_chains: + for node in c: + node2tst[node] = c + node2ref = {} + for c in ref_chains: + for node in c: + node2ref[node] = c + ref_edges = set() + for c in ref_chains: + ref_edges |= {(c[k], c[k + 1]) for k in range(len(c) - 1)} + tst_edges = set() + for c in tst_chains: + for k in range(len(c) - 1): + a, b = c[k], c[k + 1] + if a[1] < ref_n[a[0]] and b[1] < ref_n[b[0]]: + tst_edges.add((a, b)) + + cases = [] + # 1-2. longest mid-splits + mids = [] + for c in ref_chains: + E = [(c[k], c[k + 1]) for k in range(len(c) - 1)] + if len(E) < 3: + continue + hit = [e in tst_edges for e in E] + if any(hit) and not all(hit): + miss = [k for k, h in enumerate(hit) if not h] + if any(0 < k < len(E) - 1 for k in miss): + mids.append(c) + mids.sort(key=len, reverse=True) + for k, c in enumerate(mids[:2]): + # test chain through the split node + E = [(c[i], c[i + 1]) for i in range(len(c) - 1)] + hit = [e in tst_edges for e in E] + split_at = next(i for i, h in enumerate(hit) if not h) + tchain = node2tst.get(c[split_at], None) + cases.append((f"case{k + 1}-midsplit len{len(c)}", c, tchain)) + + # 3. a merge: test edge joining two ref chains + merged = None + for c in ref_chains: + E = [(c[i], c[i + 1]) for i in range(len(c) - 1)] + for a, b in E: + if (a, b) not in tst_edges and a[1] < len(tn[a[0]]): + cc = tn[a[0]][a[1]] + if cc >= 0: + b2 = (a[0] + 1, cc) + if b2 in node2ref and node2ref[b2] != c: + merged = (c, node2tst.get(a)) + break + if merged: + break + if merged: + cases.append((f"case3-merge len{len(merged[0])}", merged[0], merged[1])) + + # 4. longest fully-lost multi-node chain + lost = [c for c in ref_chains if len(c) > 2 and not any( + (c[i], c[i + 1]) in tst_edges for i in range(len(c) - 1))] + lost.sort(key=len, reverse=True) + if lost: + c = lost[0] + cases.append((f"case4-lost len{len(c)}", c, node2tst.get(c[0]))) + + # 5. ghost extension: ref singleton that test links, longest test chain + ghost = [] + for c in ref_chains: + if len(c) == 1: + (f, i) = c[0] + if i < len(tn[f]) and tn[f][i] >= 0: + ghost.append((c, node2tst.get((f, i)))) + ghost.sort(key=lambda t: len(t[1] or ()), reverse=True) + if ghost: + c, t = ghost[0] + cases.append(("case5-ghost", c, t)) + + print(f"selected {len(cases)} cases") + for title, c, t in cases: + show_case(title, c, t, XYZ, ref_n, rp, tp) + plot_case(title, c, t, XYZ, ref_n, out / (title + ".png")) + + args_t = ", ".join(title for title, _, _ in cases) + print(f"\ncases: {args_t}") + + +if __name__ == "__main__": + main() diff --git a/scripts/diag_candidate_autopsy.py b/scripts/diag_candidate_autopsy.py new file mode 100644 index 00000000..222617e8 --- /dev/null +++ b/scripts/diag_candidate_autopsy.py @@ -0,0 +1,339 @@ +"""Why did openptv2 drop this link? Replay the candidate shortlist per link. + +Takes the links a reference run (legacy 3dptv) made and the run under test did +NOT, and replays the frame-2 candidate stage for each one using the shipped +reference implementations (searchquader + candsearch_in_pix from +openptv2.algorithms.track). For every dropped link it asks, in the order the +tracker asks: + + OUT_OF_BOX the true partner's target is not inside the projected search + window in ANY camera -> search volume / prediction problem + RANK>4 it IS in the window but more than 4 targets sit closer to the + predicted centre, so candsearch_in_pix evicts it before + anything scores it -> shortlist depth problem + FREQ<2 it survives the top-4 in fewer than 2 cameras, so + sortwhatfound culls it (ttools.c:335) -> multi-camera problem + SURVIVED it reached the scoring stage, so the loss is downstream + (two-hop lookahead, acc/angle gate, or conflict resolution) + +Run from repo root: + uv run python scripts/diag_candidate_autopsy.py \ + --ref /res_ground_truth_backup \ + --test /res_retry0 \ + --data +""" + +from __future__ import annotations + +import argparse +from pathlib import Path + +import numpy as np + +from openptv2.algorithms.calibration import Calibration +from openptv2.algorithms.parameters import ControlPar, TrackPar +from openptv2.algorithms.track import ( + angle_acc, + candsearch_in_pix, + point_to_pixel, + searchquader, +) +from openptv2.algorithms.tracking_frame_buf import read_targets + +MAX_SHORTLIST = 4 + + +def read_ptv_is(path: Path): + lines = path.read_text().strip().splitlines() + n = int(lines[0]) + prev = np.empty(n, dtype=int) + for i, line in enumerate(lines[1 : n + 1]): + prev[i] = int(line.split()[0]) + return prev + + +def read_rt_is(path: Path): + """Return (xyz, corres) — corres[i, cam] is the target index, -1 if none.""" + lines = path.read_text().strip().splitlines() + n = int(lines[0]) + xyz = np.empty((n, 3)) + cor = np.full((n, 4), -1, dtype=int) + for i, line in enumerate(lines[1 : n + 1]): + p = line.split() + xyz[i] = [float(p[1]), float(p[2]), float(p[3])] + for c in range(4): + cor[i, c] = int(p[4 + c]) + return xyz, cor + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument("--ref", type=Path, required=True) + ap.add_argument("--test", type=Path, required=True) + ap.add_argument("--data", type=Path, required=True) + ap.add_argument("--first", type=int, default=100002) + ap.add_argument("--last", type=int, default=100010) + ap.add_argument("--targets", default="img_3dptv/Cam{cam}.") + ap.add_argument("--limit", type=int, default=0, help="stop after N drops") + ap.add_argument("--pred-from-test", action="store_true", + help="predict using the RUN-UNDER-TEST link history " + "(what the tracker actually had) instead of the " + "reference's -- isolates cascade effects") + args = ap.parse_args() + + cpar = ControlPar.from_file(str(args.data / "parameters" / "ptv.par")) + tpar = TrackPar.from_file(str(args.data / "parameters" / "track.par")) + ncam = cpar.num_cams + cals = [] + for c in range(1, ncam + 1): + ori = add = None + for stem in (f"cam_{c}.tif", f"cam{c}.tif", f"Cam{c}.tif", f"cam_{c}"): + p = args.data / "cal" / f"{stem}.ori" + if p.exists(): + ori = str(p) + a = args.data / "cal" / f"{stem}.addpar" + add = str(a) if a.exists() else None + break + if ori is None: + raise SystemExit(f"no .ori found for camera {c} in {args.data / 'cal'}") + cals.append(Calibration.from_file(ori, add)) + + verdict = {"OUT_OF_BOX": 0, "RANK>4": 0, "FREQ<2": 0, "SURVIVED": 0} + rank_hist = [] + step_of = {k: [] for k in verdict} + n_drop = 0 + stage2 = { + "checked": 0, "no_true_succ": 0, "DISPL_GATE": 0, + "HOP2_NOT_FOUND": 0, "ACC_ANGLE_GATE": 0, "GATE_PASSED": 0, + } + gate_fail, hop2_step, gate_ok_step = [], [], [] + + for f in range(args.first, args.last + 1): + ref_prev = read_ptv_is(args.ref / f"ptv_is.{f}") + test_prev = read_ptv_is(args.test / f"ptv_is.{f}") + cur_xyz, cur_cor = read_rt_is(args.ref / f"rt_is.{f}") + old_xyz, old_cor = read_rt_is(args.ref / f"rt_is.{f - 1}") + otargs = [ + read_targets(str(args.data / args.targets.format(cam=c + 1)), f - 1) + for c in range(ncam) + ] + # frame f-2, for the constant-velocity prediction + older_xyz = None + if f - 2 >= args.first - 1: + p2 = args.ref / f"rt_is.{f - 2}" + if p2.exists(): + older_xyz, _ = read_rt_is(p2) + src = args.test if args.pred_from_test else args.ref + oprev = read_ptv_is(src / f"ptv_is.{f - 1}") + + targs = [] + for c in range(ncam): + base = str(args.data / args.targets.format(cam=c + 1)) + targs.append(read_targets(base, f)) + + # frame f+1, for the two-hop lookahead stage + nxt_xyz = nxt_cor = ntargs = None + next_of = {} + pn = args.ref / f"rt_is.{f + 1}" + if pn.exists(): + nxt_xyz, nxt_cor = read_rt_is(pn) + nprev = read_ptv_is(args.ref / f"ptv_is.{f + 1}") + for j, pj in enumerate(nprev): + if pj >= 0: + next_of[pj] = j + ntargs = [ + read_targets(str(args.data / args.targets.format(cam=c + 1)), f + 1) + for c in range(ncam) + ] + + for i in range(len(ref_prev)): + p_ref = ref_prev[i] + p_test = test_prev[i] if i < len(test_prev) else -2 + if p_ref < 0 or p_test == p_ref: + continue # no reference link, or we matched it + n_drop += 1 + if args.limit and n_drop > args.limit: + break + + X1 = old_xyz[p_ref] + X2 = X1 + if older_xyz is not None and p_ref < len(oprev) and oprev[p_ref] >= 0: + X0 = older_xyz[oprev[p_ref]] + X2 = 2 * X1 - X0 + + step = float(np.linalg.norm(cur_xyz[i] - X1)) + xr, xl, yd, yu = searchquader(X2, tpar, cpar, cals) + + in_box = 0 + in_top4 = 0 + best_rank = None + for c in range(ncam): + tidx = cur_cor[i, c] + if tidx < 0 or tidx >= len(targs[c]): + continue + # The tracker centres the pixel search on the particle's own + # MEASURED target (track.c:136), and only reprojects for a + # camera with no correspondence. Using point_to_pixel here + # instead silently shifts the window by the reprojection + # residual -- which is what made an earlier version of this + # script disagree with the tracker. + oidx = old_cor[p_ref, c] if p_ref < len(old_cor) else -1 + if 0 <= oidx < len(otargs[c]): + cx, cy = otargs[c][oidx].x, otargs[c][oidx].y + else: + cx, cy = point_to_pixel(X2, cals[c], cpar) + tx = targs[c][tidx].x + ty = targs[c][tidx].y + if not ( + cx - xl[c] < tx < cx + xr[c] and cy - yu[c] < ty < cy + yd[c] + ): + continue + in_box += 1 + # Rank by distance among every target inside the same window. + d_true = np.hypot(cx - tx, cy - ty) + closer = 0 + for k in range(len(targs[c])): + if targs[c][k].tnr == -1: + continue + kx, ky = targs[c][k].x, targs[c][k].y + if not ( + cx - xl[c] < kx < cx + xr[c] + and cy - yu[c] < ky < cy + yd[c] + ): + continue + if np.hypot(cx - kx, cy - ky) < d_true: + closer += 1 + rank = closer + 1 + best_rank = rank if best_rank is None else min(best_rank, rank) + # Confirm against the shipped shortlist implementation. + p = candsearch_in_pix( + targs[c], len(targs[c]), cx, cy, + xl[c], xr[c], yu[c], yd[c], cpar, + ) + if tidx in p: + in_top4 += 1 + + if in_box == 0: + v = "OUT_OF_BOX" + elif in_top4 == 0: + v = "RANK>4" + elif in_top4 < 2: + v = "FREQ<2" + else: + v = "SURVIVED" + verdict[v] += 1 + step_of[v].append(step) + if best_rank is not None: + rank_hist.append(best_rank) + + # ---- stage 2: the two-hop lookahead (track.c lines 204-337) ---- + # The frame-2 candidate is only registered if a valid frame-3 + # continuation is also found. A perfect frame-2 candidate still + # yields NO LINK when the frame-3 search comes up empty. + if v != "SURVIVED" or nxt_xyz is None: + continue + stage2["checked"] += 1 + X3 = cur_xyz[i] + X0v = None + if older_xyz is not None and p_ref < len(oprev) and oprev[p_ref] >= 0: + X0v = older_xyz[oprev[p_ref]] + X5 = 2 * X3 - X1 if X0v is None else 0.5 * (5 * X3 - 4 * X1 + X0v) + + succ = next_of.get(i, -1) + if succ < 0: + stage2["no_true_succ"] += 1 + continue + X4 = nxt_xyz[succ] + + d = X4 - X3 + if not ( + tpar.dvxmin < d[0] < tpar.dvxmax + and tpar.dvymin < d[1] < tpar.dvymax + and tpar.dvzmin < d[2] < tpar.dvzmax + ): + stage2["DISPL_GATE"] += 1 + continue + + xr5, xl5, yd5, yu5 = searchquader(X5, tpar, cpar, cals) + hits = 0 + for c in range(ncam): + tidx = nxt_cor[succ, c] + if tidx < 0 or tidx >= len(ntargs[c]): + continue + cx, cy = point_to_pixel(X5, cals[c], cpar) + p = candsearch_in_pix( + ntargs[c], len(ntargs[c]), cx, cy, + xl5[c], xr5[c], yu5[c], yd5[c], cpar, + ) + if tidx in p: + hits += 1 + if hits < 2: + stage2["HOP2_NOT_FOUND"] += 1 + hop2_step.append(step) + continue + + a1, ac1 = angle_acc(X3, X4, X5) + if X0v is not None: + a0, ac0 = angle_acc(X1, X2, X3) + else: + a0, ac0 = a1, ac1 + acc = (ac0 + ac1) / 2 + ang = (a0 + a1) / 2 + if (acc < tpar.dacc and ang < tpar.dangle) or acc < tpar.dacc / 10: + stage2["GATE_PASSED"] += 1 + gate_ok_step.append(step) + else: + stage2["ACC_ANGLE_GATE"] += 1 + gate_fail.append((acc, ang, step)) + if args.limit and n_drop > args.limit: + break + + total = sum(verdict.values()) + print(f"\ndropped links analysed: {total}\n") + print(f"{'verdict':<14}{'count':>8}{'share':>9}{'mean step':>12}{'max step':>11}") + for k in ("OUT_OF_BOX", "RANK>4", "FREQ<2", "SURVIVED"): + s = step_of[k] + share = verdict[k] / total * 100 if total else 0 + ms = np.mean(s) if s else 0.0 + xs = np.max(s) if s else 0.0 + print(f"{k:<14}{verdict[k]:>8}{share:>8.1f}%{ms:>12.4f}{xs:>11.4f}") + + if rank_hist: + r = np.array(rank_hist) + print(f"\nbest-camera distance rank of the true partner " + f"(shortlist keeps {MAX_SHORTLIST}):") + for k in range(1, 9): + n = int((r == k).sum()) + if n: + print(f" rank {k:<2} {n:>6} {'#' * min(60, n * 60 // len(r))}") + n = int((r > 8).sum()) + if n: + print(f" rank >8 {n:>6}") + print(f" median rank {np.median(r):.1f} " + f"fraction beyond top-4: {(r > MAX_SHORTLIST).mean():.1%}") + + if stage2["checked"]: + print(f"\n--- stage 2: two-hop lookahead, for the {stage2['checked']} " + f"drops whose frame-2 candidate SURVIVED ---") + order = ["no_true_succ", "DISPL_GATE", "HOP2_NOT_FOUND", + "ACC_ANGLE_GATE", "GATE_PASSED"] + for k in order: + share = stage2[k] / stage2["checked"] * 100 + print(f" {k:<18}{stage2[k]:>7}{share:>8.1f}%") + if hop2_step: + print(f"\n HOP2_NOT_FOUND mean step {np.mean(hop2_step):.4f} mm " + f"max {np.max(hop2_step):.4f} mm") + if gate_fail: + a = np.array([g[0] for g in gate_fail]) + g = np.array([g[1] for g in gate_fail]) + print(f" ACC_ANGLE_GATE acc median {np.median(a):.3f} " + f"(dacc={tpar.dacc}) angle median {np.median(g):.1f} " + f"(dangle={tpar.dangle})") + if gate_ok_step: + print(f"\n GATE_PASSED mean step {np.mean(gate_ok_step):.4f} mm " + f"-- these reached scoring, so they were lost in link " + f"resolution (lost a contest).") + + +if __name__ == "__main__": + main() diff --git a/scripts/diag_conflict_parity.py b/scripts/diag_conflict_parity.py new file mode 100644 index 00000000..cd2b01dc --- /dev/null +++ b/scripts/diag_conflict_parity.py @@ -0,0 +1,146 @@ +"""Legacy track.c vs openptv2 link-resolution, side by side. + +Both trackers score candidate links identically (verified: the `rr` formula and +the acc/angle gate in track_kernels_corr.py match track.c line-for-line). They +diverge at the LAST step -- deciding who keeps a contested candidate. + +track.c (lines 598-646): one pass. A particle whose best candidate is already +taken either evicts the holder (if its finaldecis is better) or is set to +next = -2 and is DONE. The evicted holder is also set to -2 and is DONE. Neither +ever falls back to its second choice -- the author left the commented-out +printfs ("zweite Wahl fuer %d waere") at lines 617-623 and 635-641 showing the +second choice was inspected but deliberately not used. + +openptv2 (track_kernels_corr.py:1273-1282) adds a third phase that track.c has +no counterpart for: every loser walks its remaining candidates and claims the +first unclaimed one. + +This script runs both resolvers over the same decision table and reports the +links that exist in one and not the other. + +Run from repo root: + uv run python scripts/diag_conflict_parity.py +""" + +from __future__ import annotations + +NONE = -2 +FREE = -1 + + +def resolve_legacy(inlist, decis, linkdecis, n_next): + """track.c lines 588-646. Losers die; no fallback.""" + nxt = [NONE] * len(inlist) + finaldecis = [1e6] * len(inlist) + prev = [FREE] * n_next + + for h in range(len(inlist)): + if inlist[h] > 0: + order = sorted(range(inlist[h]), key=lambda i: decis[h][i]) + finaldecis[h] = decis[h][order[0]] + nxt[h] = linkdecis[h][order[0]] + + for h in range(len(inlist)): + if inlist[h] <= 0: + continue + cand = nxt[h] + if prev[cand] == FREE: + prev[cand] = h + elif finaldecis[prev[cand]] > finaldecis[h]: + nxt[prev[cand]] = NONE # evicted holder is done, no retry + prev[cand] = h + else: + nxt[h] = NONE # loser is done, no retry + return nxt + + +def resolve_openptv2(inlist, decis, linkdecis, n_next): + """track_kernels_corr.py 1226-1286. Adds phase 3: losers retry.""" + nxt = [NONE] * len(inlist) + finaldecis = [1e6] * len(inlist) + prev = [FREE] * n_next + order_of = {} + + for h in range(len(inlist)): + if inlist[h] > 0: + order = sorted(range(inlist[h]), key=lambda i: decis[h][i]) + order_of[h] = order + finaldecis[h] = decis[h][order[0]] + nxt[h] = linkdecis[h][order[0]] + + for h in range(len(inlist)): + if inlist[h] <= 0: + continue + cand = nxt[h] + if prev[cand] == FREE: + prev[cand] = h + elif finaldecis[prev[cand]] > finaldecis[h]: + nxt[prev[cand]] = NONE + prev[cand] = h + else: + nxt[h] = NONE + + # --- phase 3: the part track.c does not have --- + for h in range(len(inlist)): + if inlist[h] > 1 and nxt[h] == NONE: + for ti in order_of[h][1:]: + cand = linkdecis[h][ti] + if prev[cand] == FREE: + nxt[h] = cand + finaldecis[h] = decis[h][ti] + prev[cand] = h + break + return nxt + + +def report(name, inlist, decis, linkdecis, n_next, speeds=None): + a = resolve_legacy(inlist, decis, linkdecis, n_next) + b = resolve_openptv2(inlist, decis, linkdecis, n_next) + print(f"\n=== {name} ===") + print(f"{'particle':>9} {'track.c':>12} {'openptv2':>12} note") + for h in range(len(inlist)): + note = "" + if a[h] != b[h]: + note = "<-- DIVERGES" + if speeds and b[h] in speeds: + note += f" (openptv2 links a {speeds[b[h]]} step)" + la = "dropped" if a[h] == NONE else f"-> {a[h]}" + lb = "dropped" if b[h] == NONE else f"-> {b[h]}" + print(f"{h:>9} {la:>12} {lb:>12} {note}") + print(f" links: track.c={sum(x != NONE for x in a)} " + f"openptv2={sum(x != NONE for x in b)}") + return a, b + + +def demo(): + # Two frame-1 particles both rank frame-2 candidate 0 as their best. + # Particle 1 loses the contest. Its second choice is candidate 1, which is + # a longer (faster) step -- rr penalises displacement via dl/lmax, so a + # particle's later choices are generally its longer ones. + inlist = [2, 2] + decis = [[0.10, 0.80], [0.40, 0.90]] + linkdecis = [[0, 1], [0, 1]] + speeds = {1: "longer"} + a, b = report("contested candidate", inlist, decis, linkdecis, 2, speeds) + assert a == [0, NONE], a + assert b == [0, 1], b + + # Three-way pile-up on one candidate: track.c drops two, openptv2 salvages + # whatever is still free. + inlist3 = [2, 2, 2] + decis3 = [[0.10, 0.50], [0.20, 0.60], [0.30, 0.70]] + linkdecis3 = [[0, 1], [0, 1], [0, 2]] + a3, b3 = report("three-way pile-up", inlist3, decis3, linkdecis3, 3) + assert a3 == [0, NONE, NONE], a3 + assert b3 == [0, 1, 2], b3 + + # No contest -> identical. Guards against the resolvers differing anywhere + # other than the contested path. + a4, b4 = report("no contest", [1, 1], [[0.1], [0.2]], [[0], [1]], 2) + assert a4 == b4 == [0, 1], (a4, b4) + + print("\nself-check OK: the two resolvers agree unless a candidate is contested.") + + +if __name__ == "__main__": + demo() diff --git a/scripts/diag_inlist_probe.py b/scripts/diag_inlist_probe.py new file mode 100644 index 00000000..1220a5dd --- /dev/null +++ b/scripts/diag_inlist_probe.py @@ -0,0 +1,153 @@ +"""Dump the tracker's real candidate list (inlist/linkdecis) for one step. + +The autopsy replays the candidate stage with the REFERENCE implementations in +track.py. This probe instead reads what the COMPILED kernel actually produced, +so any divergence between the two shows up directly. + +For every frame-1 particle that the reference run linked forward but the run +under test did not, it prints how many candidates the kernel actually +registered (inlist) and whether the reference's chosen partner is among them. + + inlist == 0 the kernel found NO admissible candidate at all + partner in linkdecis the kernel saw it, so the loss is in resolution + partner NOT in list the kernel never registered it -> search/gate bug + (and the reference replay disagrees with the kernel) + +Run from repo root: + uv run python scripts/diag_inlist_probe.py --work scratch/_wp1_ab \ + --ref /res_ground_truth_backup --step 100001 +""" + +from __future__ import annotations + +import argparse +import os +from pathlib import Path + +import numpy as np + + +def read_ptv_is(path: Path): + lines = path.read_text().strip().splitlines() + n = int(lines[0]) + return np.array([int(line.split()[0]) for line in lines[1 : n + 1]]) + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument("--work", type=Path, required=True) + ap.add_argument("--ref", type=Path, required=True) + ap.add_argument("--step", type=int, default=100001) + ap.add_argument("--show", type=int, default=12) + ap.add_argument("--dacc", type=float, default=None, + help="override track_par.dacc (e.g. 1.9 to match 3dptv)") + ap.add_argument("--dv", type=float, default=None, + help="override all dv* bounds symmetrically") + args = ap.parse_args() + + work = args.work.resolve() + ref = args.ref.resolve() + + from openptv2.algorithms.track import trackcorr_c_loop + from openptv2.gui.parameter_manager import ParameterManager + from openptv2.gui.ptv import py_start_proc_c + from openptv2.tracker import default_naming + + old = os.getcwd() + os.chdir(work) + try: + pm = ParameterManager() + pm.from_yaml(work / "parameters_Run1.yaml") + cpar, spar, vpar, track_par, tpar, cals, epar = py_start_proc_c(pm) + for cam_id, short in enumerate(pm.get_target_filenames()): + spar.set_img_base_name(cam_id, str(Path(short).resolve()) + ".") + + if args.dacc is not None: + track_par.dacc = args.dacc + if args.dv is not None: + track_par.dvxmin = track_par.dvymin = track_par.dvzmin = -args.dv + track_par.dvxmax = track_par.dvymax = track_par.dvzmax = args.dv + + from openptv2.tracker import Tracker + + tracker = Tracker( + cpar, vpar, track_par, spar, cals, default_naming, loser_retry=0, + cold_start_neighbour=0, + ) + tracker.restart() # builds TrackingRun + track_forward_start + run = tracker._run + + # Advance the buffer to the requested step first: after restart() + # buf[1] holds seq_par.first, so stepping s = first..args.step - 1 + # brings buf[1] to args.step. Calling trackcorr_c_loop(run, s) with + # any other s processes the buffered frames but labels/writes them + # as s -- the old code did exactly that for --step != first and + # compared unrelated frames (plus mislabeled output files). + first = run.seq_par.first + if args.step < first: + raise SystemExit(f"--step {args.step} is before first={first}") + for s in range(first, args.step): + trackcorr_c_loop(run, s) + + # Snapshot the frame-1 arrays the loop is about to fill. + curr = run.fb.buf[1] + n1 = curr.num_parts + trackcorr_c_loop(run, args.step) + + # buf rotated; the processed frame is now buf[0] + done = run.fb.buf[0] + inlist = np.asarray(done.path_inlist[:n1]).copy() + linkdecis = np.asarray(done.path_linkdecis[:n1]).copy() + nxt = np.asarray(done.path_next[:n1]).copy() + finally: + os.chdir(old) + + # Reference forward links out of the same frame. + ref_next = {} + rp = read_ptv_is(ref / f"ptv_is.{args.step + 1}") + for j, pj in enumerate(rp): + if pj >= 0: + ref_next[pj] = j + + tot = miss = seen = empty = notseen = 0 + examples = [] + for h in range(n1): + if h not in ref_next: + continue + tot += 1 + if nxt[h] == ref_next[h]: + continue + miss += 1 + want = ref_next[h] + il = int(inlist[h]) + cands = [int(c) for c in linkdecis[h][:il]] if il > 0 else [] + if il == 0: + empty += 1 + tag = "inlist EMPTY" + elif want in cands: + seen += 1 + tag = f"partner PRESENT (rank {cands.index(want)}/{il})" + else: + notseen += 1 + tag = f"partner ABSENT (inlist={il})" + if len(examples) < args.show: + examples.append((h, want, int(nxt[h]), il, tag)) + + print(f"\nstep {args.step}: {n1} particles in frame, " + f"{tot} have a reference forward link") + print(f"openptv2 reproduced {tot - miss}, missed {miss}\n") + print(f" inlist EMPTY (no candidate registered at all) {empty:>6}" + f" {empty / miss * 100 if miss else 0:>5.1f}%") + print(f" partner PRESENT but not chosen {seen:>6}" + f" {seen / miss * 100 if miss else 0:>5.1f}%") + print(f" partner ABSENT from a non-empty inlist {notseen:>6}" + f" {notseen / miss * 100 if miss else 0:>5.1f}%") + + if examples: + print(f"\n{'particle':>9}{'want':>7}{'got':>7}{'inlist':>8} verdict") + for h, want, got, il, tag in examples: + print(f"{h:>9}{want:>7}{got:>7}{il:>8} {tag}") + + +if __name__ == "__main__": + main() diff --git a/scripts/diag_link_displacement.py b/scripts/diag_link_displacement.py new file mode 100644 index 00000000..10e4c536 --- /dev/null +++ b/scripts/diag_link_displacement.py @@ -0,0 +1,134 @@ +"""Overlay the per-link displacement distributions of two tracking runs. + +Both runs must have been made from the SAME rt_is.*, so a link-by-link +comparison is exact: position data is identical, only the linking differs. + +Read the shape, not just the counts: + + * a HARD CLIFF in one run at some |step| -> a search-volume clamp + (dvxmin/dvxmax mapping, or the searchquader pixel projection) + * a SOFT DEFICIT in the tail, no cliff -> candidates are found but + rejected downstream (dacc / dangle gating) + * a deficit at ALL speeds -> candidate search or particle + addition differs, not a kinematic bound + +Run from repo root: + uv run python scripts/diag_link_displacement.py \ + --a /res_orig --b /res_retry0 \ + --rt /res --first 100002 --last 100010 +""" + +from __future__ import annotations + +import argparse +from pathlib import Path + +import numpy as np + + +def read_links(res: Path, frame: int): + p = res / f"ptv_is.{frame}" + if not p.exists(): + return [] + lines = p.read_text().strip().splitlines() + if not lines: + return [] + n = int(lines[0]) + return [int(line.split()[0]) for line in lines[1 : n + 1]] + + +def read_xyz(res: Path, frame: int): + p = res / f"rt_is.{frame}" + lines = p.read_text().strip().splitlines() + n = int(lines[0]) + return np.array( + [[float(v) for v in line.split()[1:4]] for line in lines[1 : n + 1]] + ) + + +def steps(links_dir: Path, rt_dir: Path, first: int, last: int): + out = [] + for f in range(first, last + 1): + prev = read_links(links_dir, f) + cur = read_xyz(rt_dir, f) + old = read_xyz(rt_dir, f - 1) + for i, p in enumerate(prev): + if 0 <= p < len(old) and i < len(cur): + out.append(float(np.linalg.norm(cur[i] - old[p]))) + return np.array(out) + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument("--a", type=Path, required=True, help="reference run links") + ap.add_argument("--b", type=Path, required=True, help="run under test links") + ap.add_argument("--rt", type=Path, help="rt_is source (default: --a)") + ap.add_argument("--first", type=int, default=100002) + ap.add_argument("--last", type=int, default=100010) + ap.add_argument("--label-a", default="reference") + ap.add_argument("--label-b", default="openptv2") + ap.add_argument("--dvmax", type=float, default=None) + ap.add_argument("--out", type=Path, default=Path("scratch/link_displacement.png")) + args = ap.parse_args() + + rt = args.rt or args.a + sa = steps(args.a, rt, args.first, args.last) + sb = steps(args.b, rt, args.first, args.last) + + print(f"{args.label_a}: {len(sa)} links {args.label_b}: {len(sb)} links") + print(f"\n{'quantile':>10}{args.label_a:>14}{args.label_b:>14}") + for q in (50, 75, 90, 95, 99, 100): + print(f"{q:>9}%{np.percentile(sa, q):>14.4f}{np.percentile(sb, q):>14.4f}") + print(f"\n{'mean':>10}{sa.mean():>14.4f}{sb.mean():>14.4f}") + + # Where do the missing links live? Bin both and difference the counts. + hi = max(np.percentile(sa, 99.9), np.percentile(sb, 99.9)) + edges = np.linspace(0, hi, 26) + ca, _ = np.histogram(sa, bins=edges) + cb, _ = np.histogram(sb, bins=edges) + print(f"\n{'step range (mm)':>20}{args.label_a:>12}{args.label_b:>12}" + f"{'missing':>10}{'%lost':>8}") + for k in range(len(edges) - 1): + if ca[k] == 0 and cb[k] == 0: + continue + miss = int(ca[k] - cb[k]) + pct = miss / ca[k] * 100 if ca[k] else 0.0 + print(f"{edges[k]:>9.3f}-{edges[k + 1]:<10.3f}{ca[k]:>12}{cb[k]:>12}" + f"{miss:>10}{pct:>7.1f}%") + + import matplotlib + + matplotlib.use("Agg") + import matplotlib.pyplot as plt + + fig, (ax, ax2) = plt.subplots(2, 1, figsize=(8, 7), sharex=True) + ax.hist(sa, bins=edges, alpha=0.55, label=f"{args.label_a} ({len(sa)})", + color="#2a6ebb") + ax.hist(sb, bins=edges, alpha=0.55, label=f"{args.label_b} ({len(sb)})", + color="#c0392b") + if args.dvmax: + ax.axvline(args.dvmax, color="k", ls="--", label=f"dvxmax = {args.dvmax}") + ax.set_ylabel("links") + ax.set_title("Per-link displacement: where the missing links live") + ax.legend() + ax.grid(alpha=0.3) + + mid = 0.5 * (edges[:-1] + edges[1:]) + with np.errstate(divide="ignore", invalid="ignore"): + frac = np.where(ca > 0, (ca - cb) / ca, np.nan) + ax2.bar(mid, frac * 100, width=(edges[1] - edges[0]) * 0.9, color="#7f8c8d") + ax2.axhline(0, color="k", lw=0.8) + if args.dvmax: + ax2.axvline(args.dvmax, color="k", ls="--") + ax2.set_xlabel("link displacement (mm)") + ax2.set_ylabel(f"% of {args.label_a} links\nmissing in {args.label_b}") + ax2.grid(alpha=0.3) + + fig.tight_layout() + args.out.parent.mkdir(parents=True, exist_ok=True) + fig.savefig(args.out, dpi=130) + print(f"\nwrote {args.out}") + + +if __name__ == "__main__": + main() diff --git a/scripts/diag_position_noise.py b/scripts/diag_position_noise.py new file mode 100644 index 00000000..759f7046 --- /dev/null +++ b/scripts/diag_position_noise.py @@ -0,0 +1,159 @@ +"""Estimate 3D position noise from tracked trajectories, and compare it to the +noise the per-frame acceleration gate can actually tolerate. + +The tracker's `acc` is the 3-point stencil |x[n+1] - 2x[n] + x[n-1]|. With +independent position noise sigma on each point, each component of that +combination has variance (1+4+1)*sigma^2 = 6*sigma^2, so the 3D magnitude of +the PURE NOISE acceleration is about sqrt(18)*sigma = 4.24*sigma -- before the +particle has physically accelerated at all. + +So the gate only measures physics while + + sigma < dacc / sqrt(18) + +Above that it is rejecting noise, and real links die. This script measures the +actual sigma in a res/ folder and prints where it sits against that line. + +sigma is estimated from the same stencil: over short windows a smooth particle +path is nearly straight, so the second difference is noise-dominated, and + sigma ~= std(second difference per component) / sqrt(6) +This OVERSTATES sigma when real acceleration is present, so the estimate is a +conservative upper bound -- if even this says you are under the line, you are. + +Run from repo root: + uv run python scripts/diag_position_noise.py --res /res_orig +""" + +from __future__ import annotations + +import argparse +from pathlib import Path + +import numpy as np + + +def read_ptv_is(path: Path): + """Return (prev, next, xyz) arrays for one frame.""" + if not path.exists(): + return None + lines = path.read_text().strip().splitlines() + if not lines: + return None + n = int(lines[0]) + prev = np.empty(n, dtype=int) + nxt = np.empty(n, dtype=int) + xyz = np.empty((n, 3)) + for i, line in enumerate(lines[1 : n + 1]): + p = line.split() + prev[i] = int(p[0]) + nxt[i] = int(p[1]) + xyz[i] = [float(p[2]), float(p[3]), float(p[4])] + return prev, nxt, xyz + + +def collect(res: Path, first: int, last: int): + frames = {} + for f in range(first, last + 1): + got = read_ptv_is(res / f"ptv_is.{f}") + if got is not None: + frames[f] = got + return frames + + +def trajectories(frames, min_len=4): + """Walk `next` links forward into position sequences.""" + fs = sorted(frames) + out = [] + started = {f: set() for f in fs} + for fi, f in enumerate(fs): + prev, nxt, xyz = frames[f] + for i in range(len(xyz)): + if i in started[f] or prev[i] >= 0: + continue # not a track start + seq = [xyz[i]] + cf, ci = f, i + while True: + pr, nx, xy = frames[cf] + if ci >= len(nx) or nx[ci] < 0: + break + nf = fs[fs.index(cf) + 1] if fs.index(cf) + 1 < len(fs) else None + if nf is None or nf not in frames: + break + ni = nx[ci] + if ni >= len(frames[nf][2]): + break + seq.append(frames[nf][2][ni]) + started[nf].add(ni) + cf, ci = nf, ni + if len(seq) >= min_len: + out.append(np.array(seq)) + return out + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument("--res", type=Path, required=True) + ap.add_argument("--first", type=int, default=100001) + ap.add_argument("--last", type=int, default=100010) + ap.add_argument("--dacc", type=float, default=None, + help="defaults to /../parameters/track.par line 8") + args = ap.parse_args() + + dacc = args.dacc + if dacc is None: + tp = args.res.parent / "parameters" / "track.par" + dacc = float(tp.read_text().split()[7]) + + frames = collect(args.res, args.first, args.last) + if not frames: + raise SystemExit(f"no ptv_is.* in {args.res}") + trajs = trajectories(frames) + print(f"{len(frames)} frames, {len(trajs)} trajectories of length >= 4") + if not trajs: + raise SystemExit("no trajectories long enough to estimate noise") + + lens = np.array([len(t) for t in trajs]) + print(f"trajectory length: mean {lens.mean():.1f} max {lens.max()}") + + # Second differences, per component, over every interior triple. + d2 = np.concatenate( + [t[2:] - 2 * t[1:-1] + t[:-2] for t in trajs if len(t) >= 3] + ) + steps = np.concatenate([np.linalg.norm(np.diff(t, axis=0), axis=1) for t in trajs]) + + # Robust scale: MAD is far less sensitive to the genuinely accelerating + # tail than std, so it tracks the noise floor rather than the physics. + mad = np.median(np.abs(d2 - np.median(d2, axis=0)), axis=0) + sigma_mad = float(np.mean(mad * 1.4826) / np.sqrt(6.0)) + sigma_std = float(np.mean(np.std(d2, axis=0)) / np.sqrt(6.0)) + + acc_mag = np.linalg.norm(d2, axis=1) + ceiling = dacc / np.sqrt(18.0) + + print(f"\nmean step {steps.mean():8.4f} mm") + print(f"median |acc| (3-pt) {np.median(acc_mag):8.4f} mm") + print(f"95th pct |acc| {np.percentile(acc_mag, 95):8.4f} mm") + print(f"\nposition noise sigma {sigma_mad:8.4f} mm (robust/MAD)") + print(f" {sigma_std:8.4f} mm (std, includes real accel)") + print(f"\ndacc {dacc:8.4f} mm") + print(f"noise ceiling dacc/sqrt(18) {ceiling:8.4f} mm") + print(f"noise-only |acc| = 4.24*sigma {4.2426 * sigma_mad:8.4f} mm") + + frac_gated = float(np.mean(acc_mag > dacc)) + print(f"\nfraction of observed 3-pt |acc| above dacc: {frac_gated:.1%}") + if sigma_mad > ceiling: + print( + f"\n>>> sigma {sigma_mad:.3f} EXCEEDS the ceiling {ceiling:.3f} -- the " + f"acceleration gate is\n rejecting noise, not physics. Widening the " + f"acc/angle stencil is the lever." + ) + else: + print( + f"\n>>> sigma {sigma_mad:.3f} is under the ceiling {ceiling:.3f} -- the " + f"gate is measuring physics.\n Noise is NOT what is limiting this " + f"dataset." + ) + + +if __name__ == "__main__": + main() diff --git a/scripts/diag_speed_ceiling.py b/scripts/diag_speed_ceiling.py new file mode 100644 index 00000000..2a9fdfd0 --- /dev/null +++ b/scripts/diag_speed_ceiling.py @@ -0,0 +1,275 @@ +"""Measure the tracker's actual speed ceiling against its configured one. + +Synthetic particles move in straight lines at a sweep of known speeds. Every +particle is isolated (no two are close enough to contest a candidate), so the +ONLY thing that can break a link is the search/gate chain -- not conflict +resolution. We then ask, per speed: did the tracker link this particle all the +way through? + +The configured ceiling is track.par's dvxmax (mm/frame). If measured links die +well below it, the search chain is losing fast particles and the gap is a bug, +not a parameter choice. + +Run from repo root: + uv run python scripts/diag_speed_ceiling.py +""" + +from __future__ import annotations + +import argparse +import shutil +from pathlib import Path + +import numpy as np + +from openptv2.algorithms.calibration import Calibration +from openptv2.algorithms.imgcoord import img_coord_batch +from openptv2.algorithms.parameters import ControlPar + +REPO = Path(__file__).resolve().parent.parent +SRC = REPO / "test_data" / "track" +NCAMS = 2 +FIRST = 10001 + + +def write_targets(path: Path, rows) -> None: + lines = [str(len(rows))] + for t in rows: + lines.append( + f"{t[0]:4d} {t[1]:10.4f} {t[2]:10.4f} " + f"{t[3]:6d} {t[4]:4d} {t[5]:4d} {t[6]:6d} {t[7]:4d}" + ) + path.write_text("\n".join(lines) + "\n") + + +def write_rt_is(path: Path, rows) -> None: + lines = [str(len(rows))] + for r in rows: + lines.append( + f"{r[0]:4d} {r[1]:12.6f} {r[2]:12.6f} {r[3]:12.6f} " + f"{r[4]:5d} {r[5]:5d} {r[6]:5d} {r[7]:5d}" + ) + path.write_text("\n".join(lines) + "\n") + + +def build_scene(work: Path, speeds, n_frames, jitter=0.0, seed=42): + """Write targets + rt_is for straight-line particles at the given speeds. + + Returns {particle_index: {frame: (x, y, z)}} for the particles that stayed + visible in every frame (the only ones we score). + """ + rng = np.random.default_rng(seed) + cpar = ControlPar.from_file(str(SRC / "parameters" / "ptv.par")) + cals = [ + Calibration.from_file( + str(SRC / "cal" / f"cam{c}.tif.ori"), + str(SRC / "cal" / f"cam{c}.tif.addpar"), + ) + for c in range(1, NCAMS + 1) + ] + + img = work / "img" + res = work / "res" + img.mkdir(exist_ok=True) + res.mkdir(exist_ok=True) + + # Spread particles along y so they never compete for the same candidate. + # Start x low enough that even the fastest stays in the measured volume. + truth = { + i: { + FIRST + f: (10.0 + v * f, -18.0 + 3.0 * i, 40.0) + for f in range(n_frames) + } + for i, v in enumerate(speeds) + } + + visible_everywhere = set(truth) + projected = {} + + for f in range(n_frames): + frame = FIRST + f + per_cam = {c: [] for c in range(NCAMS)} + corres = [] + for pid in sorted(truth): + x, y, z = truth[pid][frame] + pnrs = [] + for ci in range(NCAMS): + xy = img_coord_batch( + np.array([[x, y, z]], dtype=np.float64), cals[ci], cpar.mm + )[0] + px = xy[0] / cpar.pix_x + cpar.imx / 2 + py = cpar.imy / 2 - xy[1] / cpar.pix_y + if jitter: + px += rng.normal(0.0, jitter) + py += rng.normal(0.0, jitter) + if not (0 <= px < cpar.imx and 0 <= py < cpar.imy): + pnrs.append(-1) + continue + # tnr must equal the particle index for candidate sorting. + per_cam[ci].append((pid, px, py, 50, 5, 5, 1000, pid)) + pnrs.append(pid) + while len(pnrs) < 4: + pnrs.append(-1) + if sum(1 for p in pnrs[:NCAMS] if p >= 0) < 2: + visible_everywhere.discard(pid) + continue + corres.append(pnrs) + projected[frame] = corres + + # rt_is rows must be renumbered per frame AND the per-camera target index + # must point at the row's position in that camera's target list -- rebuild + # both now that we know which particles survived. + for f in range(n_frames): + frame = FIRST + f + per_cam = {c: [] for c in range(NCAMS)} + rows = [] + for pid in sorted(visible_everywhere): + x, y, z = truth[pid][frame] + pnrs = [] + for ci in range(NCAMS): + xy = img_coord_batch( + np.array([[x, y, z]], dtype=np.float64), cals[ci], cpar.mm + )[0] + px = xy[0] / cpar.pix_x + cpar.imx / 2 + py = cpar.imy / 2 - xy[1] / cpar.pix_y + if jitter: + px += rng.normal(0.0, jitter) + py += rng.normal(0.0, jitter) + idx = len(per_cam[ci]) + per_cam[ci].append((idx, px, py, 50, 5, 5, 1000, len(rows))) + pnrs.append(idx) + while len(pnrs) < 4: + pnrs.append(-1) + rows.append((len(rows) + 1, x, y, z, *pnrs)) + + write_rt_is(res / f"rt_is.{frame}", rows) + for ci in range(NCAMS): + write_targets(img / f"cam{ci + 1}.{frame}_targets", per_cam[ci]) + + return {p: truth[p] for p in sorted(visible_everywhere)} + + +def read_ptv_is(path: Path): + """Return the `prev` link column: prev[i] is row i's parent in frame-1.""" + if not path.exists(): + return [] + lines = path.read_text().strip().splitlines() + if not lines: + return [] + n = int(lines[0]) + return [int(line.split()[0]) for line in lines[1 : n + 1]] + + +def run_tracker(work: Path): + import os + + from openptv2.gui.parameter_manager import ParameterManager + from openptv2.gui.ptv import py_start_proc_c + from openptv2.tracker import Tracker, default_naming + + old = os.getcwd() + os.chdir(work) + try: + pm = ParameterManager() + pm.from_yaml(work / "parameters_Run1.yaml") + cpar, spar, vpar, track_par, tpar, cals, epar = py_start_proc_c(pm) + for cam_id, short in enumerate(pm.get_target_filenames()): + spar.set_img_base_name(cam_id, str(Path(short).resolve()) + ".") + tracker = Tracker(cpar, vpar, track_par, spar, cals, default_naming) + tracker.full_forward() + return tracker.npart, tracker.nlinks + finally: + os.chdir(old) + + +def score(work: Path, truth, n_frames): + """Per particle: how many of the n_frames-1 possible links it actually got. + + Rows in our synthetic rt_is are in particle order and stable across frames, + so row i in every frame is the same particle -- a correct link at frame f is + prev[i] == i. + """ + pids = sorted(truth) + got = dict.fromkeys(pids, 0) + for f in range(1, n_frames): + prev = read_ptv_is(work / "res" / f"ptv_is.{FIRST + f}") + for i, p in enumerate(pids): + if i < len(prev) and prev[i] == i: + got[p] += 1 + return got + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument("--frames", type=int, default=8) + ap.add_argument("--vmax", type=float, default=20.0) + ap.add_argument("--n", type=int, default=13) + ap.add_argument("--jitter", type=float, default=0.0, help="pixel noise sigma") + ap.add_argument("--out", type=Path, default=REPO / "scratch" / "speed_ceiling.png") + args = ap.parse_args() + + work = REPO / "scratch" / "_speed_ceiling_run" + if work.exists(): + shutil.rmtree(work) + work.mkdir(parents=True) + shutil.copytree(SRC / "cal", work / "cal") + shutil.copytree(SRC / "parameters", work / "parameters") + shutil.copy(SRC / "parameters_Run1.yaml", work / "parameters_Run1.yaml") + + speeds = np.linspace(0.5, args.vmax, args.n) + truth = build_scene(work, speeds, args.frames, jitter=args.jitter) + print(f"{len(truth)}/{len(speeds)} particles visible in all frames") + + # Point the sequence at our synthetic frames. + import yaml + + ycfg = yaml.safe_load((work / "parameters_Run1.yaml").read_text()) + for key in ("sequence", "Sequence", "sequence_par"): + if key in ycfg: + ycfg[key]["first"] = FIRST + ycfg[key]["last"] = FIRST + args.frames - 1 + (work / "parameters_Run1.yaml").write_text(yaml.safe_dump(ycfg)) + + npart, nlinks = run_tracker(work) + print(f"tracker: npart={npart:.1f} nlinks={nlinks:.1f}") + + got = score(work, truth, args.frames) + possible = args.frames - 1 + dvxmax = float((SRC / "parameters" / "track.par").read_text().split()[1]) + + print(f"\n speed(mm/frame) links {possible} possible configured max={dvxmax}") + xs, ys = [], [] + for pid in sorted(truth): + v = speeds[pid] + frac = got[pid] / possible + xs.append(v) + ys.append(frac) + bar = "#" * int(round(frac * 30)) + print(f" {v:8.2f} {got[pid]:2d}/{possible} {bar:<30} {frac:5.0%}") + + plot(xs, ys, dvxmax, args.out) + print(f"\nwrote {args.out}") + + +def plot(xs, ys, dvxmax, out: Path): + import matplotlib + + matplotlib.use("Agg") + import matplotlib.pyplot as plt + + fig, ax = plt.subplots(figsize=(7, 4)) + ax.plot(xs, ys, "o-", color="#2a6ebb", label="measured link fraction") + ax.axvline(dvxmax, color="#c0392b", ls="--", label=f"configured dvxmax = {dvxmax}") + ax.set_xlabel("particle speed (mm/frame)") + ax.set_ylabel("fraction of links made") + ax.set_ylim(-0.05, 1.05) + ax.set_title("Tracker speed ceiling: measured vs configured") + ax.grid(alpha=0.3) + ax.legend() + fig.tight_layout() + out.parent.mkdir(parents=True, exist_ok=True) + fig.savefig(out, dpi=130) + + +if __name__ == "__main__": + main() diff --git a/scripts/diagnose_ppar_wp1.py b/scripts/diagnose_ppar_wp1.py new file mode 100644 index 00000000..6f97acbb --- /dev/null +++ b/scripts/diagnose_ppar_wp1.py @@ -0,0 +1,118 @@ +"""Diagnose the wp1 ppar regression: which fixed-good links does ppar break? + +Saves fixed-forward outputs to res_fixedfwd/, runs ppar-forward, then +profiles ppar-misses-that-fixed-hits (step/acc/crowd/frame) to name the +mechanism instead of guessing it. +""" + +import shutil +import sys +from pathlib import Path + +sys.path.insert(0, str(Path(__file__).resolve().parent)) + +import numpy as np # noqa: E402 +from verify_same_trajectories import DS, REF # noqa: E402 + +WORK = (Path(__file__).resolve().parent.parent / "scratch" / "_wp1_ab" + ).resolve() +FIRST, LAST = 100001, 100010 + + +def read_pn(d: Path, f: int): + lines = (d / f"ptv_is.{f}").read_text().splitlines() + n = int(lines[0]) + return [int(l.split()[0]) for l in lines[1: n + 1]] + + +def read_xyz(f: int): + lines = (REF / f"rt_is.{f}").read_text().splitlines() + n = int(lines[0]) + return np.array([[float(v) for v in l.split()[1:4]] for l in lines[1: n + 1]]) + + +def main(): + import os + + from phase_scheduler import run_adaptive_pp + + from openptv2.gui.parameter_manager import ParameterManager + from openptv2.gui.ptv import py_start_proc_c + from openptv2.tracker import Tracker, default_naming + + for f in range(FIRST, LAST + 1): + shutil.copyfile(REF / f"rt_is.{f}", WORK / "res" / f"rt_is.{f}") + for cam in range(1, 5): + shutil.copyfile( + DS / "img_3dptv" / f"Cam{cam}.{f}_targets", + WORK / "img_3dptv" / f"Cam{cam}.{f}_targets") + old = os.getcwd() + os.chdir(WORK) + try: + pm = ParameterManager() + pm.from_yaml(WORK / "parameters_Run_dacc19.yaml") + cpar, spar, vpar, track_par, tpar, cals, epar = py_start_proc_c(pm) + for cam_id, short in enumerate(pm.get_target_filenames()): + spar.set_img_base_name(cam_id, str(Path(short).resolve()) + ".") + tr = Tracker(cpar, vpar, track_par, spar, cals, default_naming, + loser_retry=0, cold_start_neighbour=0) + tr.full_forward() + finally: + os.chdir(old) + snap = WORK / "res_fixedfwd" + if snap.exists(): + shutil.rmtree(snap) + shutil.copytree(WORK / "res", snap, + ignore=shutil.ignore_patterns("rt_is.*")) + print("fixed-forward saved to res_fixedfwd", flush=True) + + run_adaptive_pp(WORK, 0, 0, hi=1.5, mode="vel", first=FIRST, + last=LAST, yaml="parameters_Run_dacc19.yaml") + + # per-step link counts + print(f"{'frame':>8}{'ref':>7}{'fixed':>7}{'ppar':>7}", flush=True) + for f in range(100002, 100011): + r = sum(1 for p in read_pn(REF, f) if p >= 0) + a = sum(1 for p in read_pn(snap, f) if p >= 0) + b = sum(1 for p in read_pn(WORK / "res", f) if p >= 0) + print(f"{f:>8}{r:>7}{a:>7}{b:>7}", flush=True) + + # regression features: ref links fixed-hits but ppar-misses + from scipy.spatial import cKDTree + XYZ = {f: read_xyz(f) for f in range(FIRST, LAST + 1)} + trees = {f: cKDTree(XYZ[f]) for f in XYZ} + reg, rec = [], [] + for f in range(100003, 100011): + r = read_pn(REF, f) + a = read_pn(snap, f) + b = read_pn(WORK / "res", f) + rp = read_pn(REF, f - 1) + for i in range(min(len(r), len(a), len(b))): + if r[i] < 0: + continue + fa = a[i] == r[i] + fb = b[i] == r[i] + if fa and not fb: + p = r[i] + step = float(np.linalg.norm(XYZ[f][i] - XYZ[f - 1][p])) + crowd = len(trees[f].query_ball_point(XYZ[f][i], r=1.9)) - 1 + reg.append((step, crowd, f)) + elif fa and fb: + p = r[i] + step = float(np.linalg.norm(XYZ[f][i] - XYZ[f - 1][p])) + rec.append(step) + reg = np.array(reg, dtype=object) + print(f"\nregressions (fixed-hits, ppar-misses): {len(reg)}", flush=True) + if len(reg): + st = np.array([x[0] for x in reg], dtype=float) + cr = np.array([x[1] for x in reg], dtype=float) + print(f"step mean={st.mean():.3f} p90={np.percentile(st, 90):.3f} " + f"max={st.max():.3f} crowd_mean={cr.mean():.2f}", flush=True) + import collections + print("per-frame:", dict(sorted(collections.Counter( + x[2] for x in reg).items())), flush=True) + print(f"control (both hit) step mean={np.mean(rec):.3f}", flush=True) + + +if __name__ == "__main__": + main() diff --git a/scripts/drill_s4_global.py b/scripts/drill_s4_global.py new file mode 100644 index 00000000..babe39e7 --- /dev/null +++ b/scripts/drill_s4_global.py @@ -0,0 +1,65 @@ +"""Drill: is the S4-global failure a decision artifact or history corruption? + +Finds an S4 seed where global fails the cross, runs parity on the same +seed, and diffs histories (frames 1..3 must be identical for a clean +resolution-level verdict at frame 4). +""" + +import sys +from pathlib import Path + +sys.path.insert(0, str(Path(__file__).resolve().parent)) + +from proto_global_resolve import run_global +from synth_crossing import ( + FIRST, + NF, + REPO, + load_optics, + make_truth, + run_tracker, + setup_work, + write_scene, +) + + +def prev_of(work): + out = {} + for f in range(NF): + lines = (work / "res" / f"ptv_is.{FIRST + f}").read_text().splitlines() + out[f] = [int(l.split()[0]) for l in lines[1: int(lines[0]) + 1]] + return out + + +def main(): + cpar, cals = load_optics() + for rep in range(20): + work = REPO / "scratch" / "_synth_drill" + setup_work(work) + truth = make_truth(v=0.5, noise=0.07, seed=1000 + rep) + write_scene(work, truth, cpar, cals) + run_global(work, 0) + lines = (work / "res" / f"ptv_is.{FIRST + 4}").read_text().splitlines() + prev = [int(l.split()[0]) for l in lines[1: int(lines[0]) + 1]] + if prev != [0, 1]: + print(f"global fails cross at seed {1000 + rep}: " + f"frame4 prev={prev}", flush=True) + g = prev_of(work) + setup_work(work) + write_scene(work, truth, cpar, cals) + run_tracker(work, 0, 0) + p = prev_of(work) + for f in (1, 2, 3): + mark = "SAME " if g[f] == p[f] else "DIFF " + print(f" {mark} frame{f}: global={g[f]} parity={p[f]}", + flush=True) + lines = (work / "res" / f"ptv_is.{FIRST + 4}" + ).read_text().splitlines() + pp = [int(l.split()[0]) for l in lines[1: int(lines[0]) + 1]] + print(f" parity frame4 prev={pp}", flush=True) + return + print("no failing seed found", flush=True) + + +if __name__ == "__main__": + main() diff --git a/scripts/inspect_refusals.py b/scripts/inspect_refusals.py new file mode 100644 index 00000000..f4729a71 --- /dev/null +++ b/scripts/inspect_refusals.py @@ -0,0 +1,65 @@ +"""Inspect Class-A refusals: is the candidate even visible to the search?""" + +from pathlib import Path + +import numpy as np + +DS = Path(r"C:\Users\alex\Downloads\HiDImaging\wp1_10_images") +REF = DS / "res_ground_truth_backup" + + +def rp_of(d, f): + l = (d / f"ptv_is.{f}").read_text().splitlines() + n = int(l[0]) + return [int(x.split()[0]) for x in l[1: n + 1]] + + +def rx(f): + l = (REF / f"rt_is.{f}").read_text().splitlines() + n = int(l[0]) + pos = [[float(v) for v in x.split()[1:4]] for x in l[1: n + 1]] + pcols = [x.split()[4:8] for x in l[1: n + 1]] + return pos, pcols + + +def main(): + import math + for (f, i) in [(100006, 535), (100007, 421), (100006, 334), + (100006, 875)]: + p = rp_of(REF, f)[i] + X1, P1 = rx(f - 1) + X3, P3 = rx(f) + print(f"--- frame {f} row {i} ref_prev={p} ---") + print(f" rt frame{f - 1}[{p}] pcols={P1[p]}") + print(f" rt frame{f}[{i}] pcols={P3[i]}") + d = np.array(X3[i]) - np.array(X1[p]) + print(f" per-axis step={np.round(d, 3)} max={np.abs(d).max():.3f} " + f"3D={np.linalg.norm(d):.3f}") + pp = rp_of(REF, f - 1)[p] + if pp >= 0: + X0, _ = rx(f - 2) + A, B, G = np.array(X1[p]), np.array(X3[i]), np.array(X0[pp]) + X2 = 2 * A - G + v0, v1 = X2 - A, B - A + n0, n1 = np.linalg.norm(v0), np.linalg.norm(v1) + if n0 == 0 or n1 == 0: + ang = 0.0 + else: + ang = math.acos(np.clip(np.dot(v0, v1) / (n0 * n1), -1, 1)) + ang *= 200 / math.pi + print(f" fallback: acc={np.linalg.norm(B - X2):.3f} " + f"angle={ang:.1f}gon grandparent={pp}") + for cam in range(1, 5): + j = P3[i][cam - 1] + msg = f" cam{cam}: rt->target {j}" + if j != "-1": + tl = (DS / "img_3dptv" / f"Cam{cam}.{f}_targets" + ).read_text().splitlines() + rec = [li for li in tl[1:] if li.split()[0] == j] + msg += (" file_tnr=" + rec[0].split()[7]) if rec else \ + " TARGET-ROW-MISSING" + print(msg) + + +if __name__ == "__main__": + main() diff --git a/scripts/judge_hard_links.py b/scripts/judge_hard_links.py new file mode 100644 index 00000000..029780a7 --- /dev/null +++ b/scripts/judge_hard_links.py @@ -0,0 +1,158 @@ +"""Manual-vote evidence pack for the 34 union-miss links. + +For each: the reference option (p->i) vs the engines' top option (q->i, +same history) with step/acc/appearance on both sides, onward consistency +(ref-next of p and q, test-next of p and q), and a verdict hint: + REF-PLAUSIBLE ref option smooth/uncrowded, engines stole from a + live track (swap) + ENG-PLAUSIBLE engines' option smoother, ref jumps far/fast + REF-FAST ref continues established fast motion (low acc, big step) + AMBIGUOUS both defensible + TAIL last-frame boundary (no lookahead for anyone) + +Run from repo root: + uv run python -u scripts/judge_hard_links.py +""" + +from __future__ import annotations + +from pathlib import Path + +import numpy as np + +REPO = Path(__file__).resolve().parent.parent +DS = Path(r"C:\Users\alex\Downloads\HiDImaging\wp1_10_images") +REF = DS / "res_ground_truth_backup" +ENGS = { + "parity": REPO / "scratch" / "_wp1_ab" / "res", + "default": REPO / "scratch" / "_ens_lr1" / "res", + "4be": REPO / "scratch" / "_ens_4be" / "res", + "2p": REPO / "scratch" / "_ens_2p" / "res_2p", +} + + +def read_pn(d: Path, f: int): + lines = (d / f"ptv_is.{f}").read_text().splitlines() + n = int(lines[0]) + p, x = [], [] + for line in lines[1: n + 1]: + s = line.split() + p.append(int(s[0])) + x.append(int(s[1])) + return p, x + + +def read_xyz(f: int): + lines = (REF / f"rt_is.{f}").read_text().splitlines() + n = int(lines[0]) + return np.array([[float(v) for v in l.split()[1:4]] for l in lines[1: n + 1]]) + + +def load_sumg(): + out = {} + for f in range(100001, 100011): + for cam in range(1, 5): + tl = (REPO / "scratch" / "_wp1_ab" / "img_3dptv" + / f"Cam{cam}.{f}_targets").read_text().splitlines() + for line in tl[1:]: + s = line.split() + out[(f, cam, int(s[0]))] = float(s[6]) + return out + + +RT_COLS = {} + + +def rt_pcols(f: int): + if f not in RT_COLS: + lines = (REF / f"rt_is.{f}").read_text().splitlines() + RT_COLS[f] = [l.split()[4:8] for l in lines[1:]] + return RT_COLS[f] + + +def app_of(f, row, SUMG): + vals = [] + pc = rt_pcols(f) + if row >= len(pc): + return float("nan") + for cam in range(1, 5): + j = pc[row][cam - 1] + if j != "-1" and (f, cam, int(j)) in SUMG: + vals.append(SUMG[(f, cam, int(j))]) + return np.mean(vals) if vals else float("nan") + + +def main(): + from collections import Counter + names = list(ENGS) + P = {n: {f: read_pn(d, f)[0] for f in range(100002, 100011)} + for n, d in ENGS.items()} + N = {n: {f: read_pn(d, f)[1] for f in range(100002, 100011)} + for n, d in ENGS.items()} + RP = {f: read_pn(REF, f)[0] for f in range(100001, 100011)} + RN = {f: read_pn(REF, f)[1] for f in range(100001, 100011)} + XYZ = {f: read_xyz(f) for f in range(100001, 100011)} + SUMG = load_sumg() + + print(f"{'frame':>8}{'row':>6}{'ref':>6}{'eng':>6}" + f"{'stepR':>7}{'accR':>7}{'stepE':>7}{'accE':>7}" + f"{'appR':>7}{'appE':>7}{'qRefNxt':>8}{'pTstNxt':>8} verdict", + flush=True) + verdicts: Counter = Counter() + for f in range(100003, 100011): + r = RP[f] + n = min(len(r), *(len(P[x][f]) for x in names)) + for i in range(n): + if r[i] < 0: + continue + votes = [P[x][f][i] for x in names] + if any(v == r[i] for v in votes): + continue + p = r[i] + cnt = Counter(votes) + top, c = cnt.most_common(1)[0] + q = top if top is not None and top >= 0 else -1 + X1, X3 = XYZ[f - 1], XYZ[f] + # Options share the SAME target i, differ in source: + # R (ref): p -> i E (engines' top): q -> i + stepR = float(np.linalg.norm(X3[i] - X1[p])) \ + if 0 <= p < len(X1) else -1 + pp = RP[f - 1][p] if 0 <= p < len(RP[f - 1]) else -1 + if 0 <= pp and pp < len(XYZ[f - 2]): + accR = float(np.linalg.norm(X3[i] - 2 * X1[p] + XYZ[f - 2][pp])) + else: + accR = float("nan") + stepE = float(np.linalg.norm(X3[i] - X1[q])) \ + if 0 <= q < len(X1) else -1 + gq = RP[f - 1][q] if 0 <= q < len(RP[f - 1]) else -1 + if 0 <= gq and gq < len(XYZ[f - 2]): + accE = float(np.linalg.norm(X3[i] - 2 * X1[q] + XYZ[f - 2][gq])) + else: + accE = float("nan") + sa = app_of(f - 1, p, SUMG) + appR = abs(app_of(f, i, SUMG) - sa) + appE = abs(app_of(f, i, SUMG) - app_of(f - 1, q, SUMG)) \ + if q >= 0 else float("nan") + qrefnxt = RN[f - 1][q] if 0 <= q < len(RN[f - 1]) else -9 + ptestnxt = N["default"][f - 1][p] \ + if 0 <= p < len(N["default"][f - 1]) else -9 + if f == 100010: + v = "TAIL" + elif not np.isnan(accR) and accR < 0.6 and stepR > 1.5: + v = "REF-FAST" + elif not np.isnan(accE) and not np.isnan(accR) and accE + 0.3 < accR: + v = "ENG-PLAUSIBLE" + elif not np.isnan(accE) and not np.isnan(accR) and accR + 0.3 < accE: + v = "REF-PLAUSIBLE" + else: + v = "AMBIGUOUS" + verdicts[v] += 1 + print(f"{f:>8}{i:>6}{p:>6}{q:>6}" + f"{stepR:>7.2f}{accR:>7.2f}{stepE:>7.2f}{accE:>7.2f}" + f"{appR:>7.0f}{appE:>7.0f}{qrefnxt:>8}{ptestnxt:>8} {v}", + flush=True) + print(f"\nverdicts: {dict(verdicts)}", flush=True) + + +if __name__ == "__main__": + main() diff --git a/scripts/miss_profiler.py b/scripts/miss_profiler.py new file mode 100644 index 00000000..8f9a8c09 --- /dev/null +++ b/scripts/miss_profiler.py @@ -0,0 +1,161 @@ +"""Miss profiler + appearance probe on wp1 (offline, existing outputs). + +1. Miss profiler: for every reference link (matched vs missed by our + fwd+bwd+post run), tabulate step, 3-pt acc, local crowding (neighbors + within 1.9 mm of the target), history length, frame. Shows WHERE the + headroom is. +2. Appearance probe: at contested divergences (ref a->b missed, test + a->c taken), compare brightness/size continuity |sumg(b)-sumg(a)| vs + |sumg(c)-sumg(a)| across cameras. Systematic truth-ward signal => + appearance belongs in the candidate cost. + +Run from repo root (needs scratch/_wp1_ab/res + dataset targets): + uv run python -u scripts/miss_profiler.py +""" + +from __future__ import annotations + +from pathlib import Path + +import numpy as np + +REPO = Path(__file__).resolve().parent.parent +DS = Path(r"C:\Users\alex\Downloads\HiDImaging\wp1_10_images") +REF = DS / "res_ground_truth_backup" +TST = REPO / "scratch" / "_wp1_ab" / "res" +FIRST, LAST = 100001, 100010 + + +def read_pn(d: Path, f: int): + l = (d / f"ptv_is.{f}").read_text().splitlines() + n = int(l[0]) + p, x = [], [] + for line in l[1: n + 1]: + s = line.split() + p.append(int(s[0])) + x.append(int(s[1])) + return p, x + + +def read_xyz(f: int): + l = (REF / f"rt_is.{f}").read_text().splitlines() + n = int(l[0]) + pos = np.array([[float(v) for v in s.split()[1:4]] for s in l[1: n + 1]]) + return pos, n + + +def load_sumg(): + """(frame, cam, target_row) -> (sumg, n_px). Keeps x,y too.""" + out = {} + for f in range(FIRST, LAST + 1): + for cam in range(1, 5): + tl = (REPO / "scratch" / "_wp1_ab" / "img_3dptv" + / f"Cam{cam}.{f}_targets").read_text().splitlines() + for line in tl[1:]: + s = line.split() + out[(f, cam, int(s[0]))] = (float(s[6]), int(s[3])) + return out + + +def main(): + XYZ = {f: read_xyz(f) for f in range(FIRST, LAST + 1)} + RP, RN = {}, {} + for f in range(FIRST, LAST + 1): + RP[f], RN[f] = read_pn(REF, f) + TP, TN = {}, {} + for f in range(FIRST, LAST + 1): + TP[f], TN[f] = read_pn(TST, f) + + # ref chains for history length + node2chain = {} + for f in range(FIRST, LAST + 1): + for i in range(len(RP[f])): + if RP[f][i] < 0: + ch, (cf, ci) = [(f, i)], (f, i) + while cf <= LAST and ci < len(RN[cf]) and RN[cf][ci] >= 0: + ci = RN[cf][ci] + cf += 1 + ch.append((cf, ci)) + for k, node in enumerate(ch): + node2chain[node] = (len(ch), k) + + from scipy.spatial import cKDTree + trees = {f: cKDTree(XYZ[f][0]) for f in XYZ} + + feats = {"hit": [], "miss": []} + contests = [] # (acc_ref, acc_test, app_ref, app_test) + SUMG = load_sumg() + + def particle_app(f, row): + """Mean sumg over correspondent cameras of rt row.""" + line = (REF / f"rt_is.{f}").read_text().splitlines()[row + 1].split() + vals = [] + for cam in range(1, 5): + j = int(line[3 + cam]) + if j >= 0 and (f, cam, j) in SUMG: + vals.append(SUMG[(f, cam, j)][0]) + return np.mean(vals) if vals else float("nan") + + for f in range(FIRST + 1, LAST + 1): + t = TP[f] + X1, _ = XYZ[f - 1] + X3, _ = XYZ[f] + for i, p in enumerate(RP[f]): + if p < 0 or p >= len(X1) or i >= len(X3): + continue + step = float(np.linalg.norm(X3[i] - X1[p])) + crowd = len(trees[f].query_ball_point(X3[i], r=1.9)) - 1 + L, k = node2chain.get((f - 1, p), (1, 0)) + pp = RP[f - 1][p] if p < len(RP[f - 1]) else -1 + if 0 <= pp: + X0, _ = XYZ[f - 2] + acc = float(np.linalg.norm(X3[i] - 2 * X1[p] + X0[pp])) \ + if pp < len(X0) else float("nan") + else: + acc = float("nan") + hit = i < len(t) and t[i] == p + feats["hit" if hit else "miss"].append((step, acc, crowd, L, f)) + if not hit: + # test's NEXT of p: where did test think p goes? + cn = TN[f - 1][p] if p < len(TN[f - 1]) else -2 + if cn >= 0 and cn < len(X3) and cn != i: + c = cn + a_ref, a_tst = acc, float("nan") + if 0 <= pp and pp < len(XYZ[f - 2][0]): + a_tst = float(np.linalg.norm( + X3[c] - 2 * X1[p] + XYZ[f - 2][0][pp])) + sa = particle_app(f - 1, p) + sb = particle_app(f, i) + sc = particle_app(f, c) + contests.append((a_ref, a_tst, abs(sb - sa), abs(sc - sa))) + + print("== miss profiler (matched vs missed ref links) ==") + for name in ("hit", "miss"): + A = np.array([r[0] for r in feats[name]]) + C = np.array([r[2] for r in feats[name]]) + L = np.array([r[3] for r in feats[name]]) + accs = np.array([r[1] for r in feats[name]]) + accs = accs[~np.isnan(accs)] + print(f"{name:5} n={len(A):5} step_mean={A.mean():.3f} " + f"step_p90={np.percentile(A, 90):.3f} " + f"crowd_mean={C.mean():.2f} crowd>=2:{(C >= 2).mean():.1%} " + f"histlen_mean={L.mean():.2f} starts:{(L == 1).mean():.1%} " + f"acc_mean={accs.mean():.3f} acc_p90={np.percentile(accs, 90):.3f}") + print("\nmiss frame histogram:", + {f: sum(1 for r in feats['miss'] if r[4] == f) + for f in range(FIRST + 1, LAST + 1)}) + + C = np.array(contests) + C = C[~np.isnan(C).any(axis=1)] + print(f"\n== contested divergences with full data: {len(C)} ==") + if len(C): + print(f"acc: ref-choice mean={C[:, 0].mean():.3f} vs " + f"test-choice mean={C[:, 1].mean():.3f}; " + f"test smoother {(C[:, 1] < C[:, 0]).mean():.1%}") + print(f"appearance |dsumg|: ref-choice mean={C[:, 2].mean():.1f} vs " + f"test-choice mean={C[:, 3].mean():.1f}; " + f"truth more similar {(C[:, 2] < C[:, 3]).mean():.1%}") + + +if __name__ == "__main__": + main() diff --git a/scripts/phase_scheduler.py b/scripts/phase_scheduler.py new file mode 100644 index 00000000..db472db9 --- /dev/null +++ b/scripts/phase_scheduler.py @@ -0,0 +1,323 @@ +"""Phase-aware scheduler for pulsatile flow (aorta case). + +Problem: global dv/dacc gates fit exactly one regime. In pulsatile flow, +systole needs wide gates (fast + accelerating), diastole needs tight ones +(slow + reversal-prone, swaps lurk). One fixed setting loses one phase. + +Scheduler (no kernel change -- pure Python around trackcorr_c_loop): +per step, measure the previous step's median link step M (robust) and its +spread. Scale = clamp(M / anchor, lo, hi) where anchor = running median +of past step-medians (self-calibrating, no nominal needed). dv bounds and +dacc scale together (ratio preserved). Reversal/strain detector: +IQR/M > rev_ratio (mixed fast/slow = turning field) boosts dacc only, +because reversal breaks the predictor while positions stay catchable. + +First two steps run at scale 1 (no statistics yet). + +Run from repo root: + uv run python -u scripts/phase_scheduler.py [--reps N] +""" + +from __future__ import annotations + +import argparse +import os +import sys +from pathlib import Path + +sys.path.insert(0, str(Path(__file__).resolve().parent)) + +import numpy as np # noqa: E402 +from synth_crossing import ( # noqa: E402 + FIRST, + NF, + REPO, + load_optics, + make_truth, + score, + setup_work, + write_scene, +) + + +def run_adaptive(work: Path, lr: int, cs: int, app: float = 0.0, + lo: float = 0.5, hi: float = 2.0, rev_boost: float = 1.5, + rev_ratio: float = 2.5, verbose: bool = False, + first: int | None = None, last: int | None = None, + scale_dv: bool = True, scale_dacc: bool = True, + yaml: str = "parameters_Run1.yaml"): + from openptv2.algorithms.track import trackcorr_c_finish, trackcorr_c_loop + from openptv2.gui.parameter_manager import ParameterManager + from openptv2.gui.ptv import py_start_proc_c + from openptv2.tracker import Tracker, default_naming + + old = os.getcwd() + os.chdir(work) + try: + pm = ParameterManager() + pm.from_yaml(work / yaml) + cpar, spar, vpar, track_par, tpar, cals, epar = py_start_proc_c(pm) + for cam_id, short in enumerate(pm.get_target_filenames()): + spar.set_img_base_name(cam_id, str(Path(short).resolve()) + ".") + tr = Tracker(cpar, vpar, track_par, spar, cals, default_naming, + loser_retry=lr, cold_start_neighbour=cs, app_weight=app) + tr.restart() + run = tr._run + base = run.tpar + if first is None: + first = FIRST + if last is None: + last = FIRST + NF - 1 + meds: list[float] = [] + scales = [] + last_iqr = (0.0, 0.0) + scale = 1.0 + prev_lost = 0.0 + losts: list[float] = [] + for step in range(first, last): + # dacc gets the reversal boost on top of the dv scale, both + # from PREVIOUS-step statistics (causal: nothing from the + # future is used). + dscale = scale + iqr, med = last_iqr + if med > 0 and iqr / med > rev_ratio: + dscale = min(scale * rev_boost, 3.0) + sv = scale if scale_dv else 1.0 + sa = dscale if scale_dacc else 1.0 + run.tpar = base._replace( + dvxmin=base.dvxmin * sv, dvxmax=base.dvxmax * sv, + dvymin=base.dvymin * sv, dvymax=base.dvymax * sv, + dvzmin=base.dvzmin * sv, dvzmax=base.dvzmax * sv, + dacc=base.dacc * sa) + scales.append(round(scale, 2)) + trackcorr_c_loop(run, step) + # post-rotation: buf[0] = processed frame, buf[1] = next + b0, b1 = run.fb.buf[0], run.fb.buf[1] + nx = np.asarray(b0.path_next[:b0.num_parts]) + pv = np.asarray(b0.path_prev[:b0.num_parts]) + # Starvation among particles WITH history only: cold-start + # losses (prev<0, early steps) are not systole and must not + # trigger widening (that swap-bombs dense data). + has_hist = pv >= 0 + if has_hist.sum() >= 1: + lost = 1.0 - (nx[has_hist] >= 0).sum() / has_hist.sum() + else: + lost = 0.0 + if len(losts) >= 2 and lost > float(np.median(losts)) + 0.10: + scale = min(hi, scale * 1.3) + else: + scale = max(1.0, scale * 0.9) + losts.append(lost) + prev_lost = lost + ok = nx >= 0 + if ok.sum() >= 3: + st = np.linalg.norm( + np.asarray(b1.path_x)[nx[ok]] - + np.asarray(b0.path_x)[:b0.num_parts][ok], axis=1) + meds.append(float(np.median(st))) + q75, q25 = np.percentile(st, [75, 25]) + last_iqr = (float(q75 - q25), float(np.median(st))) + if verbose: + print(f" scales: {scales}", flush=True) + trackcorr_c_finish(run, last) + finally: + os.chdir(old) + + +def run_adaptive_pp(work: Path, lr: int, cs: int, app: float = 0.0, + hi: float = 2.5, verbose: bool = False, + first: int | None = None, last: int | None = None, + mode: str = "vel", yaml: str = "parameters_Run1.yaml", + backward: bool = False): + """Per-particle gate freedom (safe for dense data). + + mode="vel": scale = clamp(|v|/v_typical, 1, hi). Too hot: steady-fast + particles get max scale and swap-bomb dense data (wp1 88%). + mode="acc" (default): scale = clamp(1 + |a|/dacc, 1, hi) where a is + the particle's last acceleration magnitude. Steady motion + (fast OR slow) keeps tight gates -- prediction is trustworthy; + only unpredictable (accelerating) particles get room. Noise + (~0.3) maps to a gentle ~1.16. + Density-gated crowd veto (automatic regime split): the swap-bomb + mechanism is density-dependent (lies need a crowd of smooth + rivals). In dense data (num_parts >= 100) freedom is vetoed + wherever another particle sits near the predicted position + (judged at PREDICTION, radius = own widened box + margin, self + counts: only the truly lonely are freed). In sparse data the veto + is skipped -- a lone bystander loses to truth on cost, and vetoing + it would kill genuine systole rescues (S14c). Dense+systole + combined remains an open case (needs a dense-systole truth scene). + Communicated via run.gate_scale (no tpar mutation). + """ + from openptv2.algorithms.track import trackcorr_c_finish, trackcorr_c_loop + from openptv2.gui.parameter_manager import ParameterManager + from openptv2.gui.ptv import py_start_proc_c + from openptv2.tracker import Tracker, default_naming + + old = os.getcwd() + os.chdir(work) + try: + pm = ParameterManager() + pm.from_yaml(work / yaml) + cpar, spar, vpar, track_par, tpar, cals, epar = py_start_proc_c(pm) + for cam_id, short in enumerate(pm.get_target_filenames()): + spar.set_img_base_name(cam_id, str(Path(short).resolve()) + ".") + tr = Tracker(cpar, vpar, track_par, spar, cals, default_naming, + loser_retry=lr, cold_start_neighbour=cs, app_weight=app) + tr.restart() + run = tr._run + if first is None: + first = FIRST + if last is None: + last = FIRST + NF - 1 + dacc0 = float(run.tpar.dacc) + # (frame, row) -> last velocity vector (for acceleration magnitude) + vel: dict = {} + run_adaptive_pp._speeds = [] + _sparse = int(run.fb.buf[1].num_parts) < 100 + if verbose: + print(f" [pp] density gate: sparse={_sparse} " + f"(n={int(run.fb.buf[1].num_parts)})", flush=True) + for step in range(first, last): + b1 = run.fb.buf[1] + maxp = b1.path_x.shape[0] + gs = np.ones(maxp, dtype=np.float64) + if mode == "acc": + pv = np.asarray(b1.path_prev[:b1.num_parts]) + px0 = np.asarray(run.fb.buf[0].path_x) + px1 = np.asarray(b1.path_x) + for j in range(b1.num_parts): + p = int(pv[j]) + if p < 0: + continue + v_new = px1[j] - px0[p] + # velocity that arrived at (frame step-1, row p), keyed + # by time index t = frame - first + v_old = vel.get((step - first, p)) + if v_old is not None: + a = float(np.linalg.norm(v_new - v_old)) + gs[j] = min(1.0 + a / dacc0, hi) + else: # mode == "vel" (legacy experiment, too hot) + speeds = getattr(run_adaptive_pp, "_speeds", []) + if speeds: + vtyp = float(np.median(speeds)) + if vtyp > 0 and step > first: + pv = np.asarray(b1.path_prev[:b1.num_parts]) + px0 = np.asarray(run.fb.buf[0].path_x) + px1 = np.asarray(b1.path_x) + for j in range(b1.num_parts): + p = int(pv[j]) + if 0 <= p: + v = float(np.linalg.norm(px1[j] - px0[p])) + gs[j] = min(max(v / vtyp, 1.0), hi) + # Crowd gate (dense data only; see docstring): veto freedom + # wherever another particle sits near the predicted position. + if not _sparse and float(gs.max()) > 1.0: + from scipy.spatial import cKDTree + _tp = run.tpar + _dv = max(abs(_tp.dvxmin), abs(_tp.dvxmax), + abs(_tp.dvymin), abs(_tp.dvymax), + abs(_tp.dvzmin), abs(_tp.dvzmax)) + _px1 = np.asarray(b1.path_x) + _tree = cKDTree(_px1[:b1.num_parts]) + _pv = np.asarray(b1.path_prev[:b1.num_parts]) + _px0 = np.asarray(run.fb.buf[0].path_x) + _npre, _nveto = int((gs > 1.0).sum()), 0 + for j in range(b1.num_parts): + if gs[j] <= 1.0: + continue + p = int(_pv[j]) + if p < 0: + continue + _vj = _px1[j] - _px0[p] + _pred = _px1[j] + _vj + _r = _dv * float(gs[j]) + 1.0 + if len(_tree.query_ball_point(_pred, r=_r)) > 0: + gs[j] = 1.0 + _nveto += 1 + if verbose and _npre: + print(f" [pp] step {step}: pre={_npre} vetoed={_nveto} " + f"free={_npre - _nveto}", flush=True) + run.gate_scale = gs + trackcorr_c_loop(run, step) + # harvest velocities keyed by (new frame, row) + c0, c1 = run.fb.buf[0], run.fb.buf[1] + nx = np.asarray(c0.path_next[:c0.num_parts]) + ok = nx >= 0 + if ok.sum(): + X0 = np.asarray(c0.path_x)[:c0.num_parts] + X1 = np.asarray(c1.path_x) + for h in range(c0.num_parts): + if nx[h] >= 0: + vel[(step + 1 - first, int(nx[h]))] = \ + X1[int(nx[h])] - X0[h] + if mode == "vel": + if ok.sum(): + st = np.linalg.norm( + np.asarray(c1.path_x)[nx[ok]] - + np.asarray(c0.path_x)[:c0.num_parts][ok], axis=1) + run_adaptive_pp._speeds = \ + getattr(run_adaptive_pp, "_speeds", []) + \ + [float(v) for v in st] + if verbose: + print(f" mode={mode}", flush=True) + trackcorr_c_finish(run, last) + if backward: + from openptv2.algorithms.track import trackback_c + trackback_c(run) + finally: + os.chdir(old) + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument("--reps", type=int, default=8) + args = ap.parse_args() + + cpar, cals = load_optics() + scenarios = [ + ("S1 head-on", {"v": 0.5}), + ("S14-hard p4", {"v": 0.5, "pulsatile": {"vmax": 1.5, "period": 4, + "modes": ("anti",)}}), + ("S14b-hard p4", {"v": 0.5, "pulsatile": {"vmax": 1.5, "period": 4, + "modes": ("plug",)}}), + ("S14c systole2.5", {"v": 0.5, "pulsatile": {"vmax": 2.5, "period": 10, + "modes": ("solo",)}}), + ("S11 kick+noise", {"v": 0.5, "maneuver": "kick", "noise": 0.07}), + ] + channels = [("both", True, True), ("dv-only", True, False), + ("dacc-only", False, True)] + print(f"{'scenario':16}{'engine':14}{'recall':>9}{'cross-ok':>10}", + flush=True) + for sname, kw in scenarios: + noisy = kw.get("noise", 0.0) > 0 + reps = args.reps if noisy else 3 + rows = [("fixed", None, None, None)] + if "S14c" in sname or "S11" in sname: + rows += [(f"adapt-{c}", dv, da, None) for c, dv, da in channels] + rows += [("ppar", True, True, 2.5), ("ppar15", True, True, 1.5)] + else: + rows += [("adaptive", True, True, None), ("ppar", True, True, 2.5)] + for ename, dv, da, hi in rows: + recs, crs = [], [] + for rep in range(reps): + work = REPO / "scratch" / "_synth_ph" + setup_work(work) + truth = make_truth(seed=1000 + rep, **kw) + rows_pf = write_scene(work, truth, cpar, cals) + if ename == "fixed": + from synth_crossing import run_tracker + run_tracker(work, 0, 0) + elif ename.startswith("ppar"): + run_adaptive_pp(work, 0, 0, hi=hi, mode="vel") + else: + run_adaptive(work, 0, 0, scale_dv=dv, scale_dacc=da) + r, c = score(work, rows_pf) + recs.append(r) + crs.append(c) + print(f"{sname:16}{ename:14}{np.mean(recs):>8.1%} " + f"{sum(crs)}/{len(crs)}", flush=True) + + +if __name__ == "__main__": + main() diff --git a/scripts/plot_hard_links.py b/scripts/plot_hard_links.py new file mode 100644 index 00000000..e1016e9a --- /dev/null +++ b/scripts/plot_hard_links.py @@ -0,0 +1,188 @@ +"""Plot the union-miss links for manual adjudication. + +For every reference link that NO engine reproduced, draws the local 3D +neighborhood (X-Y and X-Z): frame N-1 particles, frame N candidates, the +reference assignment (green) and each engine's vote (parity red, default +orange, 4BE purple, two-phase brown; abstentions labeled). Prints a +judgment table with step/acc/crowding/history per choice. + +Run from repo root: + uv run python -u scripts/plot_hard_links.py --out scratch/hard_links +""" + +from __future__ import annotations + +import argparse +from collections import Counter +from pathlib import Path + +import numpy as np + +REPO = Path(__file__).resolve().parent.parent +DS = Path(r"C:\Users\alex\Downloads\HiDImaging\wp1_10_images") +REF = DS / "res_ground_truth_backup" +ENGS = { + "parity": REPO / "scratch" / "_wp1_ab" / "res", + "default": REPO / "scratch" / "_ens_lr1" / "res", + "4be": REPO / "scratch" / "_ens_4be" / "res", + "2p": REPO / "scratch" / "_ens_2p" / "res_2p", +} +ECOL = {"parity": "#c0392b", "default": "#e67e22", "4be": "#8e44ad", + "2p": "#5d4037"} +FIRST, LAST = 100001, 100010 + + +def read_prev(d: Path, f: int): + lines = (d / f"ptv_is.{f}").read_text().splitlines() + n = int(lines[0]) + return [int(l.split()[0]) for l in lines[1: n + 1]] + + +def read_xyz(f: int): + lines = (REF / f"rt_is.{f}").read_text().splitlines() + n = int(lines[0]) + return np.array([[float(v) for v in l.split()[1:4]] for l in lines[1: n + 1]]) + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument("--out", type=Path, + default=REPO / "scratch" / "hard_links") + ap.add_argument("--first", type=int, default=100003) + ap.add_argument("--last", type=int, default=100010) + ap.add_argument("--per-fig", type=int, default=6) + args = ap.parse_args() + out = args.out + out.mkdir(parents=True, exist_ok=True) + + names = list(ENGS) + P = {n: {f: read_prev(d, f) for f in range(args.first, args.last + 1)} + for n, d in ENGS.items()} + R = {f: read_prev(REF, f) for f in range(args.first, args.last + 1)} + XYZ = {f: read_xyz(f) for f in range(FIRST - 1, LAST + 1) + if (REF / f"rt_is.{f}").exists()} + + # ref chains for history length + RP = {f: read_prev(REF, f) for f in range(FIRST, LAST + 1)} + RN = {} + for f in range(FIRST, LAST + 1): + lines = (REF / f"ptv_is.{f}").read_text().splitlines() + n = int(lines[0]) + RN[f] = [int(l.split()[1]) for l in lines[1: n + 1]] + histlen = {} + for f in range(FIRST, LAST + 1): + for i in range(len(RP[f])): + if RP[f][i] < 0: + ch = [(f, i)] + cf, ci = f, i + while cf <= LAST and ci < len(RN[cf]) and RN[cf][ci] >= 0: + ci = RN[cf][ci] + cf += 1 + ch.append((cf, ci)) + for k, node in enumerate(ch): + histlen[node] = (len(ch), k) + + hard = [] + for f in range(args.first, args.last + 1): + n = min(len(R[f]), *(len(P[x][f]) for x in names)) + for i in range(n): + if R[f][i] < 0: + continue + votes = [P[x][f][i] for x in names] + if not any(v == R[f][i] for v in votes): + hard.append((f, i, R[f][i], votes)) + print(f"union misses: {len(hard)}", flush=True) + + import matplotlib + matplotlib.use("Agg") + import matplotlib.pyplot as plt + from matplotlib.lines import Line2D + + rows = [] + for idx0 in range(0, len(hard), args.per_fig): + chunk = hard[idx0: idx0 + args.per_fig] + fig, axs = plt.subplots(len(chunk), 2, + figsize=(13, 2.6 * len(chunk))) + if len(chunk) == 1: + axs = np.array([axs]) + for ax_row, (f, i, r, votes) in zip(axs, chunk): + X1, X3 = XYZ[f - 1], XYZ[f] + tgt = X3[i] + box = (np.abs(X1 - tgt).max(axis=1) < 4.0) + box3 = (np.abs(X3 - tgt).max(axis=1) < 4.0) + src = X1[r] if 0 <= r < len(X1) else None + ax_xy, ax_xz = ax_row + for ax, dims, xl, yl in ((ax_xy, (0, 1), "x (mm)", "y (mm)"), + (ax_xz, (0, 2), "x (mm)", "z (mm)")): + ax.scatter(X1[box][:, dims[0]], X1[box][:, dims[1]], + s=8, color="#bbbbbb", label="frame N-1") + ax.scatter(X3[box3][:, dims[0]], X3[box3][:, dims[1]], + s=14, color="#7fb3d5", label="frame N cands") + ax.scatter([tgt[dims[0]]], [tgt[dims[1]]], s=90, + facecolor="none", edgecolor="black", lw=1.2) + if src is not None: + ax.annotate("", (tgt[dims[0]], tgt[dims[1]]), + xytext=(src[dims[0]], src[dims[1]]), + arrowprops={"arrowstyle": "->", "color": "green", + "lw": 2}) + ax.scatter([src[dims[0]]], [src[dims[1]]], s=60, + marker="*", color="green") + for ename, v in zip(names, votes): + if v is not None and v >= 0 and v < len(X1): + o = X1[v] + ax.annotate("", (tgt[dims[0]], tgt[dims[1]]), + xytext=(o[dims[0]], o[dims[1]]), + arrowprops={"arrowstyle": "->", + "color": ECOL[ename], + "lw": 1.2, "ls": "--"}) + ax.set_xlabel(xl) + ax.set_ylabel(yl) + ax.grid(alpha=0.3) + ax.set_aspect("equal", adjustable="datalim") + # feature row for the judgment table + step = float(np.linalg.norm(tgt - src)) if src is not None else -1 + crowd = int(box3.sum()) - 1 + pp = RP[f - 1][r] if 0 <= r < len(RP[f - 1]) else -1 + if 0 <= pp and (f - 2) in XYZ and pp < len(XYZ[f - 2]): + acc = float(np.linalg.norm(tgt - 2 * src + XYZ[f - 2][pp])) + else: + acc = float("nan") + L, _ = histlen.get((f - 1, r), (1, 0)) + cnt = Counter(votes) + agree = ",".join(f"{n}={v}" for n, v in zip(names, votes)) + rows.append((f, i, r, step, acc, crowd, L, agree)) + ax_xy.set_title(f"{f}:{i} ref {r}->{i} " + f"step={step:.2f} acc={acc:.2f} crowd={crowd} " + f"hist={L} | {agree}", fontsize=8, loc="left") + handles = [Line2D([0], [0], color="green", lw=2, label="3dptv ref"), + Line2D([0], [0], color="#bbbbbb", marker="o", ls="", + label="frame N-1"), + Line2D([0], [0], color="#7fb3d5", marker="o", ls="", + label="frame N"), + *[Line2D([0], [0], color=c, lw=1.2, ls="--", label=n) + for n, c in ECOL.items()]] + fig.legend(handles=handles, loc="upper center", ncol=7, fontsize=9) + fig.tight_layout(rect=(0, 0, 1, 0.96)) + path = out / f"hard_{idx0 // args.per_fig + 1}.png" + fig.savefig(path, dpi=120) + plt.close(fig) + print(f"wrote {path}", flush=True) + + print(f"\n{'frame':>8}{'row':>6}{'ref':>6}{'step':>7}{'acc':>7}" + f"{'crowd':>7}{'hist':>6} votes(parity,default,4be,2p)", + flush=True) + import csv + with open(out / "hard_links.csv", "w", newline="") as fh: + w = csv.writer(fh) + w.writerow(["frame", "row", "ref_prev", "step", "acc", "crowd", + "histlen", "parity", "default", "4be", "2p"]) + for (f, i, r, step, acc, crowd, L, agree) in rows: + v = agree.split(",") + w.writerow([f, i, r, f"{step:.3f}", f"{acc:.3f}", crowd, L, + *[x.split("=")[1] for x in v]]) + print(f"{f:>8}{i:>6}{r:>6}{step:>7.2f}{acc:>7.2f}" + f"{crowd:>7}{L:>6} {agree}", flush=True) + + +if __name__ == "__main__": + main() diff --git a/scripts/probe_search.py b/scripts/probe_search.py new file mode 100644 index 00000000..9c8832df --- /dev/null +++ b/scripts/probe_search.py @@ -0,0 +1,167 @@ +"""Narrow replay of the candidate SEARCH stage (searchquader + per-cam +4-nearest + freq merge), validated against the kernel's own inlist arrays +on control particles before trusting it on refusal cases. + +Question: do small-step refusals (e.g. 100005:1242 -> 100006:535, step +0.73, all gates pass, targets present with correct tnr) die because the +true target falls out of the per-cam top-4 in crowded regions +(prediction offset 1.3mm + 5 neighbours)? + +Run from repo root: + uv run python -u scripts/probe_search.py --work scratch/_wp1_ab +""" + +from __future__ import annotations + +import argparse +import os +import shutil +from pathlib import Path + +import numpy as np + + +def load_targets(work: Path, frame: int, ncams: int): + per_cam = [] + base = work / "img_3dptv" + for cam in range(1, ncams + 1): + # target base names come from the yaml; resolve the same way + cands = sorted(base.glob(f"Cam{cam}.{frame}_targets")) + if not cands: + cands = sorted(base.glob(f"cam{cam}.{frame}_targets")) + tl = cands[0].read_text().splitlines() + xs, ys, tnrs = [], [], [] + for line in tl[1:]: + s = line.split() + xs.append(float(s[1])) + ys.append(float(s[2])) + tnrs.append(int(s[7])) + per_cam.append((np.array(xs), np.array(ys), np.array(tnrs))) + return per_cam + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument("--work", type=Path, required=True) + ap.add_argument("--cases", type=str, default="100005:1242:535", + help="f1:h:want,row triples (frame,row-in-f1,true partner)") + ap.add_argument("--n-controls", type=int, default=60) + args = ap.parse_args() + work = args.work.resolve() + + from openptv2.algorithms.track import ( + point_to_pixel, + searchquader, + trackcorr_c_loop, + ) + from openptv2.gui.parameter_manager import ParameterManager + from openptv2.gui.ptv import py_start_proc_c + from openptv2.tracker import Tracker, default_naming + + old = os.getcwd() + os.chdir(work) + try: + pm = ParameterManager() + pm.from_yaml(work / "parameters_Run1.yaml") + cpar, spar, vpar, track_par, tpar, cals, epar = py_start_proc_c(pm) + for cam_id, short in enumerate(pm.get_target_filenames()): + spar.set_img_base_name(cam_id, str(Path(short).resolve()) + ".") + track_par.dacc = 1.9 + ncams = cpar.num_cams + + DS = Path(r"C:\Users\alex\Downloads\HiDImaging\wp1_10_images") + REF = DS / "res_ground_truth_backup" + # pristine inputs: the kernel rewrites rt/tnr in place + for f in range(100001, 100007): + shutil.copyfile(REF / f"rt_is.{f}", work / "res" / f"rt_is.{f}") + for cam in range(1, ncams + 1): + shutil.copyfile( + DS / "img_3dptv" / f"Cam{cam}.{f}_targets", + work / "img_3dptv" / f"Cam{cam}.{f}_targets") + + def rt(f): + lines = (REF / f"rt_is.{f}").read_text().splitlines() + n = int(lines[0]) + return np.array([[float(v) for v in l.split()[1:4]] + for l in lines[1: n + 1]]) + + def replay_freq(X2, f2, topk): + """freq dict {particle_row: cams} for frame-f2 search.""" + xr, xl, yd, yu = searchquader(X2, track_par, cpar, cals) + per_cam = load_targets(work, f2, ncams) + freq: dict = {} + for cam in range(ncams): + cx, cy = point_to_pixel(X2, cals[cam], cpar) + xmin, xmax = cx - xl[cam], cx + xr[cam] + ymin, ymax = cy - yu[cam], cy + yd[cam] + xs, ys, tnrs = per_cam[cam] + inside = [(np.hypot(xs[j] - cx, ys[j] - cy), int(tnrs[j])) + for j in range(len(xs)) + if xmin < xs[j] < xmax and ymin < ys[j] < ymax + and tnrs[j] != -1] + inside.sort() + for _, t in inside[:topk]: + freq[t] = freq.get(t, 0) + 1 + return freq + + # kernel truth for controls: run to step, read inlist/linkdecis + tr = Tracker(cpar, vpar, track_par, spar, cals, default_naming, + loser_retry=0, cold_start_neighbour=0) + tr.restart() + run = tr._run + for s in range(100001, 100005): + trackcorr_c_loop(run, s) + done = run.fb.buf[0] # processed frame 100004... we need 100005 + # one more step to fill inlist for frame 100005 + trackcorr_c_loop(run, 100005) + done = run.fb.buf[0] + n1 = done.num_parts + kinlist = np.asarray(done.path_inlist[:n1]).copy() + klink = np.asarray(done.path_linkdecis[:n1]).copy() + # TEST's own history (not the reference's -- cascade may differ) + tpl = (work / "res" / "ptv_is.100005").read_text().splitlines() + tprev5 = [int(l.split()[0]) for l in tpl[1: int(tpl[0]) + 1]] + + # controls: particles the kernel linked (inlist>0), sample randomly + rng = np.random.default_rng(0) + cands = [h for h in range(n1) if int(kinlist[h]) > 0] + controls = rng.choice(cands, size=min(args.n_controls, len(cands)), + replace=False) + X1 = rt(100005) + X0 = rt(100004) + agree = tot = 0 + for h in controls: + p = tprev5[h] if h < len(tprev5) else -1 + if p < 0 or p >= len(X0): + continue + X2 = 2 * X1[h] - X0[p] + fq = replay_freq(X2, 100006, 4) + tot += 1 + # kernel's best candidate must be among replay's freq set + best = int(klink[h][0]) + agree += 1 if best in fq else 0 + print(f"control validation: kernel-best candidate in replay freq " + f"set on {agree}/{tot}", flush=True) + + for spec in args.cases.split(","): + f1s, hs, ws = spec.split(":") + f1, h, want = int(f1s), int(hs), int(ws) + f2 = f1 + 1 + p = tprev5[h] + X2 = 2 * X1[h] - X0[p] + print(f"\ncase frame {f1} row {h} want {want}: |X3-X2|=" + f"{np.linalg.norm(rt(f2)[want] - X2):.3f}mm", flush=True) + for topk in (4, 8): + fq = replay_freq(X2, f2, topk) + got = fq.get(want, 0) + print(f" top-{topk}: true partner in {got} cams " + f"(need>=2); total cands={len(fq)}", flush=True) + print(f" kernel inlist[{h}]={int(kinlist[h])} " + f"linkdecis={list(map(int, klink[h][:int(kinlist[h])]))}", + flush=True) + finally: + os.chdir(old) + + +if __name__ == "__main__": + main() diff --git a/scripts/prof_trackcorr.py b/scripts/prof_trackcorr.py new file mode 100644 index 00000000..6ad0d3ec --- /dev/null +++ b/scripts/prof_trackcorr.py @@ -0,0 +1,62 @@ +"""Profile trackcorr forward: kernel vs Python orchestration vs IO split. + +Run from repo root: + uv run python -u scripts/prof_trackcorr.py +""" + +import cProfile +import pstats +import shutil +import sys +import time +from pathlib import Path + +sys.path.insert(0, str(Path(__file__).resolve().parent)) + +from verify_same_trajectories import DS, REF # noqa: E402 + +WORK = (Path(__file__).resolve().parent.parent / "scratch" / "_wp1_ab").resolve() + + +def main(): + import os + + for f in range(100001, 100011): + shutil.copyfile(REF / f"rt_is.{f}", WORK / "res" / f"rt_is.{f}") + for cam in range(1, 5): + shutil.copyfile( + DS / "img_3dptv" / f"Cam{cam}.{f}_targets", + WORK / "img_3dptv" / f"Cam{cam}.{f}_targets") + os.chdir(WORK) + from openptv2.gui.parameter_manager import ParameterManager + from openptv2.gui.ptv import py_start_proc_c + from openptv2.tracker import Tracker, default_naming + + pm = ParameterManager() + pm.from_yaml(WORK / "parameters_Run_dacc19.yaml") + cpar, spar, vpar, track_par, tpar, cals, epar = py_start_proc_c(pm) + tr = Tracker(cpar, vpar, track_par, spar, cals, default_naming, + loser_retry=0, cold_start_neighbour=0) + t0 = time.perf_counter() + pr = cProfile.Profile() + pr.enable() + tr.full_forward() + pr.disable() + wall = time.perf_counter() - t0 + print(f"wall forward: {wall:.1f}s", flush=True) + st = pstats.Stats(pr) + print(f"nfuncs={len(st.stats)}", flush=True) + rows = [] + for (fn, _ln, name), (_cc, nc, tt, ct, _callers) in st.stats.items(): + low = (fn + "#" + name).lower() + if "openptv2" in low and ( + "track" in low or "frame" in low or "sync" in low + or "path" in low or "read" in low or "write" in low): + rows.append((ct, tt, nc, name)) + print(f"matched={len(rows)}", flush=True) + for ct, tt, nc, name in sorted(rows, reverse=True)[:16]: + print(f"cum={ct:.3f} self={tt:.3f} nc={nc} {name}", flush=True) + + +if __name__ == "__main__": + main() diff --git a/scripts/proto_global_resolve.py b/scripts/proto_global_resolve.py new file mode 100644 index 00000000..ad3f6238 --- /dev/null +++ b/scripts/proto_global_resolve.py @@ -0,0 +1,145 @@ +"""Prototype: global (Hungarian) conflict resolution for trackcorr. + +Replaces the kernel's greedy Phase-2 (particle order, evict-on-cheaper) +with a minimum-total-cost assignment over each step's decis matrix -- +without touching the kernel: after each stock trackcorr_c_loop call, read +back inlist/decis/linkdecis, resolve globally, patch bufs + files, so the +cascade consumes the patched history. End-to-end honest test of the idea. + +Judged on S11 (kick+noise): if cross-ok beats parity's 2/8 with no +regressions elsewhere, the idea earns a kernel implementation (as a flag). + +Run from repo root: + uv run python -u scripts/proto_global_resolve.py [--reps N] +""" + +from __future__ import annotations + +import argparse +import os +import sys +from pathlib import Path + +sys.path.insert(0, str(Path(__file__).resolve().parent)) + +import numpy as np # noqa: E402 +from scipy.optimize import linear_sum_assignment # noqa: E402 +from synth_crossing import ( # noqa: E402 + FIRST, + NF, + REPO, + load_optics, + make_truth, + score, + setup_work, + write_scene, +) + + +def run_global(work: Path, cs: int): + """Forward tracking with Hungarian resolution per step.""" + from openptv2.algorithms.track import ( + _sync_soa_to_aos, + trackcorr_c_finish, + trackcorr_c_loop, + ) + from openptv2.gui.parameter_manager import ParameterManager + from openptv2.gui.ptv import py_start_proc_c + from openptv2.tracker import Tracker, default_naming + + old = os.getcwd() + os.chdir(work) + try: + pm = ParameterManager() + pm.from_yaml(work / "parameters_Run1.yaml") + cpar, spar, vpar, track_par, tpar, cals, epar = py_start_proc_c(pm) + for cam_id, short in enumerate(pm.get_target_filenames()): + spar.set_img_base_name(cam_id, str(Path(short).resolve()) + ".") + tr = Tracker(cpar, vpar, track_par, spar, cals, default_naming, + loser_retry=0, cold_start_neighbour=cs) + tr.restart() + run = tr._run + last = FIRST + NF - 1 + for step in range(FIRST, last): + # Stock kernel call advances bufs + files with greedy + # resolution; post-rotation the processed frame s is buf[0] + # and s+1 is buf[1]. Re-resolve globally, then patch bufs + # (next of s; prev of s+1, which this step alone writes) and + # rewrite the step's files, so the cascade consumes patched + # history. decis state is read, never modified. + trackcorr_c_loop(run, step) + done = run.fb.buf[0] + after = run.fb.buf[1] + n1 = done.num_parts + inlist = np.asarray(done.path_inlist[:n1]) + decis = np.asarray(done.path_decis[:n1]) + linkd = np.asarray(done.path_linkdecis[:n1]) + cand_set = sorted({int(c) for h in range(n1) + for c in linkd[h][:int(inlist[h])]}) + if cand_set: + cpos = {c: k for k, c in enumerate(cand_set)} + sub = np.full((n1, len(cand_set)), np.inf) + for h in range(n1): + for k in range(int(inlist[h])): + sub[h, cpos[int(linkd[h][k])]] = float(decis[h][k]) + ri, ci = linear_sum_assignment(sub) + new_next = np.full(n1, -2, dtype=np.int32) + for r, c in zip(ri, ci): + if np.isfinite(sub[r, c]): + new_next[r] = cand_set[c] + else: + new_next = np.full(n1, -2, dtype=np.int32) + np.asarray(done.path_next[:n1])[:] = new_next + ap = np.asarray(after.path_prev) + ap[:] = -1 + for h in range(n1): + if 0 <= new_next[h] < len(ap): + ap[new_next[h]] = h + _sync_soa_to_aos(done) + _sync_soa_to_aos(after) + run.fb.write_frame_from_start(step) + trackcorr_c_finish(run, last) + finally: + os.chdir(old) + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument("--reps", type=int, default=3) + args = ap.parse_args() + + cpar, cals = load_optics() + scenarios = [ + ("S1 head-on", {"v": 0.5}), + ("S8 kick", {"v": 0.5, "maneuver": "kick"}), + ("S11 kick+noise", {"v": 0.5, "maneuver": "kick", "noise": 0.07}), + ("S4 head-on+noise", {"v": 0.5, "noise": 0.07}), + ("S12 convoy+noise", {"v": 0.5, "convoy": 0.6, "noise": 0.07}), + ] + print(f"{'scenario':16}{'engine':14}{'recall':>9}{'cross-ok':>10}", + flush=True) + for sname, kw in scenarios: + noisy = kw.get("noise", 0.0) > 0 + reps = args.reps if noisy else 3 + for ename, fn in (("parity", None), ("default", None), + ("global", run_global)): + recs, crs = [], [] + for rep in range(reps): + work = REPO / "scratch" / "_synth_g" + setup_work(work) + truth = make_truth(seed=1000 + rep, **kw) + rows_pf = write_scene(work, truth, cpar, cals) + if fn is None: + from synth_crossing import run_tracker + run_tracker(work, 1 if ename == "default" else 0, 0) + else: + fn(work, 0) + r, c = score(work, rows_pf) + recs.append(r) + crs.append(c) + print(f"{sname:16}{ename:14}{np.mean(recs):>8.1%} " + f"{sum(crs)}/{len(crs)}", flush=True) + + +if __name__ == "__main__": + main() diff --git a/scripts/proto_long_validate.py b/scripts/proto_long_validate.py new file mode 100644 index 00000000..98547394 --- /dev/null +++ b/scripts/proto_long_validate.py @@ -0,0 +1,241 @@ +"""Long-window validation (50 real frames) for the three shared prototypes. + +Data: HiDImaging wp1 test, frames 100001-100050, ~1000 pts/frame. +Input: res_orig/rt_is.* 3D points. Reference: res_orig/ptv_is.* linkages +(0-based prev/next chains) = the res_orig tracking output itself. +Prototypes (worktree code): TwoPhase +/-share, Fast3D +/-share_tol, +greedy baseline vs +mark/assemble (trackcorr-linkage path). +Metrics: tracks, mean length, coverage, fragmentation of reference tracks, +impurity of prototype tracks (switch/merge proxy), shared events, runtime. +""" +import sys +import time + +import numpy as np + +TEST = r"C:\Users\alex\Downloads\HiDImaging\CompleteTest\wp1\test" +F0, NF = 100001, 50 +TOL = 1.0 # mm + + +def load_points(): + frames = [] + for f in range(F0, F0 + NF): + rows = [] + with open(f"{TEST}/res_orig/rt_is.{f}") as fh: + n = int(fh.readline().split()[0]) + for _ in range(n): + t = fh.readline().split() + rows.append([float(t[1]), float(t[2]), float(t[3])]) + frames.append(np.array(rows)) + return frames + + +def load_linkages(): + prev, nxt, pos = {}, {}, {} + for f in range(F0, F0 + NF): + with open(f"{TEST}/res_orig/ptv_is.{f}") as fh: + n = int(fh.readline().split()[0]) + pv = np.full(n, -1, np.int32) + nx = np.full(n, -2, np.int32) + xy = np.zeros((n, 3)) + for i in range(n): + t = fh.readline().split() + pv[i], nx[i] = int(t[0]), int(t[1]) + xy[i] = [float(t[2]), float(t[3]), float(t[4])] + prev[f], nxt[f], pos[f] = pv, nx, xy + return prev, nxt, pos + + +def assemble_ref(prev, nxt, pos): + chains = [] + visited = set() + for f in range(F0, F0 + NF): + for i in range(len(pos[f])): + if (f, i) in visited or int(prev[f][i]) >= 0: + continue + chain = [] + cf, ci = f, i + while True: + if (cf, ci) in visited: + break + visited.add((cf, ci)) + chain.append((cf, ci)) + if cf not in nxt or ci >= len(nxt[cf]): + break + ni = int(nxt[cf][ci]) + if ni < 0 or cf + 1 not in pos or ni >= len(pos[cf + 1]): + break + cf += 1 + ci = ni + if len(chain) >= 1: + chains.append(chain) + # orphans (all-prev/no-next singletons not yet visited) + for f in range(F0, F0 + NF): + for i in range(len(pos[f])): + if (f, i) not in visited: + chains.append([(f, i)]) + visited.add((f, i)) + return chains, pos + + +def calibrate(chains, pos): + steps = [] + for c in chains: + for a in range(1, len(c)): + f0, i0 = c[a - 1] + f1, i1 = c[a] + if f1 == f0 + 1: + steps.append(float(np.linalg.norm(pos[f1][i1] - pos[f0][i0]))) + steps = np.array(steps) + vm = float(np.percentile(steps, 99)) + print(f"calibrate: {len(chains)} ref chains, step p50=" + f"{np.median(steps):.3f} p99={vm:.3f}", flush=True) + return vm + + +def score(proto_tracks, ref_chains, pos): + from scipy.spatial import cKDTree + # frame -> (points, proto_tid) / (points, ref_chain_idx) indexes + ptrees, rtrees = {}, {} + pmap, rmap = {}, {} + for ti, t in enumerate(proto_tracks): + for f, p in zip(t["frames"], t["pos"]): + pmap.setdefault(int(f), []).append((np.asarray(p), ti)) + for ci, c in enumerate(ref_chains): + for (f, i) in c: + rmap.setdefault(f, []).append((pos[f][i], ci)) + for f in set(list(pmap) + list(rmap)): + if f in pmap: + pts = np.array([p for p, _ in pmap[f]]) + ptrees[f] = (cKDTree(pts), [ti for _, ti in pmap[f]]) + if f in rmap: + pts = np.array([p for p, _ in rmap[f]]) + rtrees[f] = (cKDTree(pts), [ci for _, ci in rmap[f]]) + # fragmentation: ref chains split across >1 proto tracks + frag, frag_den, cover_pts, cover_den = 0, 0, 0, 0 + for c in [c for c in ref_chains if len(c) >= 3]: + owners = set() + for (f, i) in c: + cover_den += 1 + if f not in ptrees: + continue + tree, tids = ptrees[f] + d, k = tree.query(pos[f][i], k=1, distance_upper_bound=TOL) + if d <= TOL: + owners.add(tids[int(k)]) + cover_pts += 1 + frag_den += 1 + if len(owners) > 1: + frag += 1 + # impurity: proto tracks spanning >1 ref chain + impure = 0 + for t in proto_tracks: + if len(t["frames"]) < 2: + continue + owners = set() + for f, p in zip(t["frames"], t["pos"]): + if int(f) not in rtrees: + continue + tree, cis = rtrees[int(f)] + d, k = tree.query(np.asarray(p), k=1, distance_upper_bound=TOL) + if d <= TOL: + owners.add(cis[int(k)]) + if len(owners) > 1: + impure += 1 + ntrk = sum(1 for t in proto_tracks if len(t["frames"]) >= 2) + ml = np.mean([len(t["frames"]) for t in proto_tracks + if len(t["frames"]) >= 2]) if ntrk else 0.0 + return {"ntrk": ntrk, "meanlen": round(float(ml), 2), + "cover": round(cover_pts / max(cover_den, 1), 3), + "frag%": round(100 * frag / max(frag_den, 1), 1), + "impure": impure} + + +def main(): + t0 = time.perf_counter() + frames = load_points() + prev, nxt, pos = load_linkages() + ref_chains, _ = assemble_ref(prev, nxt, pos) + v_max = calibrate(ref_chains, pos) + print(f"load done ({time.perf_counter() - t0:.1f}s)", flush=True) + ref_tracks = [{"frames": [f for f, _ in c], + "pos": np.array([pos[f][i] for f, i in c])} + for c in ref_chains] + print(f"res_orig: {score(ref_tracks, ref_chains, pos)}", flush=True) + + from openptv2.plugins.two_phase_tracking import ( + TwoPhaseTracker, + TwoPhaseTrackerConfig, + ) + + for share in [False, True]: + t1 = time.perf_counter() + cfg = TwoPhaseTrackerConfig(v_max=v_max, max_gap=2, dt=1.0, + leaf_weight=0.0, cost_mode="3d", + use_velocity=True, allow_shared=share) + _, chains = TwoPhaseTracker(cfg).track_frames( + [np.asarray(p) for p in frames], return_chains=True) + tracks = [{"frames": [F0 + f for f in c["frames"]], "pos": c["pos"]} + for c in chains if len(c["frames"]) >= 1] + ns = sum(sum(c["shared"]) for c in chains) + print(f"twophase share={share}: shared_pts={ns} " + f"{score(tracks, ref_chains, pos)} " + f"({time.perf_counter() - t1:.1f}s)", flush=True) + + sys.path.insert(0, "scripts") + from proto_shared_validate import fast3d_tracks, trackcorr_linkage_tracks + from scipy.spatial import cKDTree + + # Fast3D pure-Python kernel: 15 frames (50 would take too long in + # interpreted mode; the compiled path is unaffected). + NF_FAST = 15 + pos15 = {f: pos[f] for f in range(F0, F0 + NF_FAST)} + chains15 = [] + for c in ref_chains: + cc = [(f, i) for (f, i) in c if F0 <= f < F0 + NF_FAST] + if cc: + chains15.append(cc) + for tol in [0.0, 0.5]: + t1 = time.perf_counter() + tr, ns = fast3d_tracks([np.asarray(p) for p in frames[:NF_FAST]], + v_max=v_max, share_tol=tol) + for t in tr: + t["frames"] = [F0 + f for f in t["frames"]] + print(f"fast3d[{NF_FAST}f] tol={tol}: shared={ns} " + f"{score(tr, chains15, pos15)} " + f"({time.perf_counter() - t1:.1f}s)", flush=True) + + def greedy_links_kd(frame_particles, gate): + links = [] + for t in range(len(frame_particles) - 1): + p0 = np.asarray(frame_particles[t]) + p1 = np.asarray(frame_particles[t + 1]) + if len(p0) == 0 or len(p1) == 0: + continue + tree = cKDTree(p1) + dists, idxs = tree.query(p0, k=1, distance_upper_bound=gate) + used = set() + for i in range(len(p0)): + j = int(idxs[i]) + if j < len(p1) and j not in used and dists[i] <= gate: + used.add(j) + links.append((t, i, t + 1, j)) + return links + + t1 = time.perf_counter() + base = greedy_links_kd([np.asarray(p) for p in frames], gate=v_max) + plain, healed, nm = trackcorr_linkage_tracks( + [np.asarray(p) for p in frames], base, tol=1.0) + for name, tr in [("plain", plain), ("healed", healed)]: + for t in tr: + t["frames"] = [F0 + f for f in t["frames"]] + print(f"tclink {name}: marks={nm if name == 'healed' else 0} " + f"{score(tr, ref_chains, pos)} " + f"({time.perf_counter() - t1:.1f}s)", flush=True) + + print("done.") + + +if __name__ == "__main__": + main() diff --git a/scripts/proto_shared_validate.py b/scripts/proto_shared_validate.py new file mode 100644 index 00000000..87ac6cef --- /dev/null +++ b/scripts/proto_shared_validate.py @@ -0,0 +1,279 @@ +"""Shared-observation prototype validation: one 3D scene, three trackers. + +Scene (10 frames, mm units, constant velocity + jitter): + P0/P1: crossing pair, frames 4-5 occluded -> single midpoint point. + P2: dropout gap at frames 3-4 (no point at all). + P3: exits the volume after frame 6 (legal end). + P4/P5: clean crucirng traffic. +Ground truth = 6 trajectories. Metrics: tracks, switches, missing, purity. +""" +import sys + +import numpy as np + +N_FRAMES = 10 + + +def make_scene(seed=0, jitter=0.05): + rng = np.random.default_rng(seed) + vel = { + 0: np.array([2.0, 0.4, 0.0]), + 1: np.array([2.0, -0.4, 0.0]), + 2: np.array([1.0, 1.5, 0.2]), + 3: np.array([3.0, 0.0, 0.0]), + 4: np.array([-1.5, 1.0, 0.1]), + 5: np.array([0.5, -1.2, -0.1]), + } + p0 = { + 0: np.array([0.0, -2.0, 0.0]), + 1: np.array([0.0, 2.0, 0.0]), + 2: np.array([5.0, 5.0, 1.0]), + 3: np.array([0.0, 10.0, -1.0]), + 4: np.array([20.0, 0.0, 2.0]), + 5: np.array([8.0, 12.0, 0.5]), + } + # P0/P1 cross near x=10 around frame 5: shift so midpoint lands ~frame 4-5 + frames_pts, frames_ids = [], [] # per frame: (M,3) points, (M,) truth ids + for f in range(N_FRAMES): + pts, ids = [], [] + for i in range(6): + if i == 3 and f > 6: + continue # legal exit + if i == 2 and f in (3, 4): + continue # dropout gap + pos = p0[i] + vel[i] * f + rng.normal(0, jitter, 3) + pts.append(pos) + ids.append(i) + pts = np.array(pts) + ids = np.array(ids) + if f in (4, 5): + # occlusion: P0 and P1 merge into their midpoint (single point) + m0 = np.where(ids == 0)[0] + m1 = np.where(ids == 1)[0] + if len(m0) and len(m1): + mid = 0.5 * (pts[m0[0]] + pts[m1[0]]) + keep = [k for k in range(len(ids)) if k not in (m0[0], m1[0])] + pts = np.vstack([pts[keep], mid[None, :]]) + ids = np.append(ids[keep], -1) # -1 = merged, both truths + frames_pts.append(pts) + frames_ids.append(ids) + return frames_pts, frames_ids + + +def score_tracks(tracks, frames_ids, frames_pts, tol=1.0): + """tracks: list of dicts with 'frames' (time idx) and 'pos' (xyz). + + Coverage counts UNIQUE truth points (two tracks may honestly cover one + shared detection). Returns (n_tracks, switches, missing, pure).""" + truth_total = sum(int((ids >= 0).sum()) for ids in frames_ids) + # +2 merged points per occlusion frame counted separately + truth_total += sum(int((ids == -1).sum()) * 2 for ids in frames_ids) + switches, pure = 0, 0 + covered: dict[tuple[int, int], set[int]] = {} # (frame,row) -> track idx + for ti, t in enumerate(tracks): + seen: set[int] = set() + prev = None + for f, p in zip(t["frames"], t["pos"]): + ids = frames_ids[f] + pts = frames_pts[f] + if len(ids) == 0: + continue + d = np.linalg.norm(pts - p, axis=1) + b = int(np.argmin(d)) + if d[b] > tol: + continue + covered.setdefault((int(f), int(b)), set()).add(ti) + if ids[b] == -1: + seen |= {0, 1} + truth = {0, 1} + else: + seen.add(int(ids[b])) + truth = {int(ids[b])} + if prev is not None and not truth & prev: + switches += 1 + prev = truth if prev is None else (prev | truth) + if seen <= {0, 1} or len(seen) == 1: + pure += 1 + # unique truth points covered: a merged row counts double only when two + # tracks honestly hold it (shared observation), else single. + uniq = 0 + for (f, r), holders in covered.items(): + if r == -1 and len(holders) >= 2: + uniq += 2 + else: + uniq += 1 + return { + "n_tracks": len(tracks), + "switches": switches, + "missing": max(0, truth_total - uniq), + "pure": pure, + } + + +def to_links_format(tracks, key_frames="frames", key_pos="pos"): + return tracks + + +def fast3d_tracks(frame_particles, v_max=3.0, share_tol=0.0): + """Drive track3d_loop_fast directly (mirrors Cython3DTracker.track_frames) + with extended per-frame arrays. Shared pairs materialize as virtual + carrier particles: shared coordinates, own history (prev -> frame-t + detection). Returns (tracks, n_shared).""" + from openptv2.algorithms.constants import NEXT_NONE, PREV_NONE + from openptv2.algorithms.track_kernels_track3d import track3d_loop_fast + + nf = len(frame_particles) + ext_pos = [np.ascontiguousarray(p, dtype=np.float64) + for p in frame_particles] + ext_prev = [np.full(len(p), PREV_NONE, dtype=np.int32) + for p in frame_particles] + ext_next = [np.full(len(p), NEXT_NONE, dtype=np.int32) + for p in frame_particles] + n_shared_total = 0 + for t in range(nf - 1): + if t == 0: + n0 = 0 + pos_0 = np.empty((0, 3), dtype=np.float64) + prev_0 = np.empty(0, dtype=np.int32) + else: + n0 = len(ext_pos[t - 1]) + pos_0, prev_0 = ext_pos[t - 1], ext_prev[t - 1] + n1, n2 = len(ext_pos[t]), len(ext_pos[t + 1]) + if n1 == 0 or n2 == 0: + continue + pos_1, prev_1 = ext_pos[t], ext_prev[t] + nxt_1 = ext_next[t] + pos_2 = ext_pos[t + 1] + prev_2 = ext_prev[t + 1] + nxt_2 = ext_next[t + 1] + cap = max(n2, 1) + sc = np.zeros(1, dtype=np.int32) + si = np.full(cap, -1, dtype=np.int32) + sk = np.full(cap, -1, dtype=np.int32) + track3d_loop_fast( + n1, pos_0, prev_0, n0, pos_1, prev_1, nxt_1, n1, + pos_2, prev_2, nxt_2, n2, + v_max, v_max, v_max, 32, 0.0, + share_tol, sc, si, sk, + ) + ext_next[t] = nxt_1 + ext_prev[t + 1] = prev_2 + # materialize virtual carriers for the NEXT steps (curr/prev roles) + for e in range(int(sc[0])): + i, k = int(si[e]), int(sk[e]) + ext_pos[t + 1] = np.vstack( + [ext_pos[t + 1], pos_2[k][None, :]]) + ext_prev[t + 1] = np.append(ext_prev[t + 1], np.int32(i)) + ext_next[t + 1] = np.append(ext_next[t + 1], np.int32(NEXT_NONE)) + n_shared_total += 1 + # assemble (shared carriers emit their coords into their track) + counts = [len(p) for p in ext_pos] + visited = [np.zeros(n, dtype=bool) for n in counts] + tracks = [] + for t in range(nf): + for i in range(counts[t]): + if visited[t][i]: + continue + tr_pos, tr_time, ct, ci = [], [], t, i + while ct < nf and ci != NEXT_NONE and not visited[ct][ci]: + visited[ct][ci] = True + tr_pos.append(ext_pos[ct][ci]) + tr_time.append(ct) + nx = int(ext_next[ct][ci]) + if nx >= 0 and ct + 1 < nf: + ct, ci = ct + 1, nx + else: + break + if len(tr_pos) >= 2: + tracks.append({"frames": tr_time, + "pos": np.array(tr_pos)}) + return tracks, n_shared_total + + +def trackcorr_linkage_tracks(frame_particles, base_links, tol=1.0): + """Prototype trackcorr path: base_links (greedy forward output) -> plain + linkage arrays -> mark_shared_observations -> assemble_with_shared. + Returns (tracks_plain, tracks_shared, n_marks).""" + from openptv2.algorithms.constants import NEXT_NONE, PREV_NONE + from openptv2.tracking_shared import ( + assemble_with_shared, + mark_shared_observations, + ) + + nf = len(frame_particles) + frames = {} + for f in range(nf): + n = len(frame_particles[f]) + frames[f] = (np.full(n, PREV_NONE, dtype=np.int32), + np.full(n, NEXT_NONE, dtype=np.int32), + np.asarray(frame_particles[f], dtype=np.float64)) + # row bookkeeping: links reference (frame, row); rows == indices here + for t0, r0, t1, r1 in base_links: + _, nxt, _ = frames[t0] + prv, _, _ = frames[t1] + if r0 < len(nxt) and r1 < len(prv): + nxt[r0] = r1 + prv[r1] = r0 + plain = assemble_with_shared(frames, 0, nf - 1, {}) + shared = mark_shared_observations(frames, 0, nf - 1, tol=tol) + healed = assemble_with_shared(frames, 0, nf - 1, shared) + return plain, healed, len(shared) + + +def greedy_links(frame_particles, gate=4.0): + """Baseline forward pass: nearest-neighbour chains (what trackcorr's + kernel would produce before postprocess). Returns link tuples.""" + links = [] + for t in range(len(frame_particles) - 1): + p0 = np.asarray(frame_particles[t]) + p1 = np.asarray(frame_particles[t + 1]) + used = set() + for i in range(len(p0)): + best, bj = gate, -1 + for j in range(len(p1)): + if j in used: + continue + d = float(np.linalg.norm(p1[j] - p0[i])) + if d < best: + best, bj = d, j + if bj >= 0: + used.add(bj) + links.append((t, i, t + 1, bj)) + return links + + +if __name__ == "__main__": + from openptv2.plugins.two_phase_tracking import ( + TwoPhaseTracker, + TwoPhaseTrackerConfig, + ) + + fp, fi = make_scene() + print("dets/frame:", [len(p) for p in fp], "truth(server): 6 trajs") + for share in [False, True]: + cfg = TwoPhaseTrackerConfig(v_max=4.0, max_gap=2, dt=1.0, + leaf_weight=0.0, cost_mode="3d", + use_velocity=True, allow_shared=share, + max_shared=3) + tr = TwoPhaseTracker(cfg) + links, chains = tr.track_frames([np.asarray(p) for p in fp], + return_chains=True) + tracks = [{"frames": c["frames"], "pos": c["pos"]} for c in chains + if len(c["frames"]) >= 1] + n_shared = sum(sum(c["shared"]) for c in chains) + print(f"allow_shared={share}: links={len(links)} shared_pts={n_shared}", + score_tracks(tracks, fi, fp)) + + print("--- Fast3D (track3d_loop_fast, virtual carriers) ---") + for tol in [0.0, 0.3, 0.5, 1.0]: + tr, ns = fast3d_tracks([np.asarray(p) for p in fp], v_max=3.0, + share_tol=tol) + print(f"share_tol={tol}: shared={ns}", + score_tracks(tr, fi, fp)) + + print("--- trackcorr-linkage (greedy links -> mark+assemble) ---") + base = greedy_links([np.asarray(p) for p in fp]) + plain, healed, nm = trackcorr_linkage_tracks( + [np.asarray(p) for p in fp], base, tol=1.0) + print(f"plain: {score_tracks(plain, fi, fp)}") + print(f"shared marks={nm}: {score_tracks(healed, fi, fp)}") diff --git a/scripts/repair_compare_wp1.py b/scripts/repair_compare_wp1.py new file mode 100644 index 00000000..ad40412e --- /dev/null +++ b/scripts/repair_compare_wp1.py @@ -0,0 +1,242 @@ +"""Can flowtracks repair make two-phase beat trackcorr on wp1? + +Builds Trajectory objects from (a) trackcorr+postprocess linkage and +(b) two-phase links, runs flowtracks.repair_trajectories, rescores +link recall / claims / chains / smoothness vs the 3dptv reference. + +Run from repo root: + uv run python -u scripts/repair_compare_wp1.py +""" + +from __future__ import annotations + +import shutil +import sys +import time +from pathlib import Path + +sys.path.insert(0, str(Path(__file__).resolve().parent)) +sys.path.insert(0, r"C:\Users\alex\projects\postptv") + +import numpy as np # noqa: E402 +from verify_same_trajectories import DS, REF # noqa: E402 + +WORK = (Path(__file__).resolve().parent.parent / "scratch" / "_wp1_ab").resolve() +FIRST, LAST = 100001, 100010 +SFIRST, SLAST = 100003, 100010 +NCAMS = 4 + + +def restore(): + for f in range(FIRST, LAST + 1): + shutil.copyfile(REF / f"rt_is.{f}", WORK / "res" / f"rt_is.{f}") + for cam in range(1, NCAMS + 1): + shutil.copyfile( + DS / "img_3dptv" / f"Cam{cam}.{f}_targets", + WORK / "img_3dptv" / f"Cam{cam}.{f}_targets") + + +def to_traj_list(chains, positions, frames, tid0=0): + from flowtracks.trajectory import Trajectory + out = [] + for k, c in enumerate(chains): + fr = [frames[t] for (t, r) in c] + ps = np.array([positions[t][r] for (t, r) in c]) + v = np.zeros_like(ps) + if len(ps) > 2: + v[:-1] = ps[1:] - ps[:-1] + v[-1] = v[-2] + elif len(ps) == 2: + v[:] = ps[1] - ps[0] + out.append(Trajectory(ps, v, np.array(fr, dtype=int), tid0 + k)) + return out + + +def chains_from_linkage(resdir: Path): + P = {} + for f in range(FIRST, LAST + 1): + lines = (resdir / f"ptv_is.{f}").read_text().splitlines() + n = int(lines[0]) + P[f] = ([int(l.split()[0]) for l in lines[1: n + 1]], + [int(l.split()[1]) for l in lines[1: n + 1]]) + nxt = {} + for f in range(FIRST, LAST + 1): + for i, v in enumerate(P[f][1]): + if v >= 0: + # find target frame (gap-aware via reciprocation) + for g in range(f + 1, LAST + 1): + if v < len(P[g][0]) and P[g][0][v] == i: + nxt[(f - FIRST, i)] = (g - FIRST, v) + break + tgt = set(nxt.values()) + chains = [] + for s in sorted(set(nxt) - tgt): + c, k = [s], s + while k in nxt: + k = nxt[k] + c.append(k) + chains.append(c) + # singletons: unlinked particles + seen = set(n for c in chains for n in c) + for f in range(FIRST, LAST + 1): + n = len(P[f][0]) + for i in range(n): + if (f - FIRST, i) not in seen: + chains.append([(f - FIRST, i)]) + return chains + + +def score_trajs(trajs, positions, frames, ref_prev, ref_total, tag): + exact = claims = consec = 0 + mp = {} + for t in trajs: + fr = list(t.time()) + ps = np.asarray(t.pos()) + for a in range(len(fr) - 1): + f0, f1 = int(fr[a]), int(fr[a + 1]) + if not (SFIRST <= f1 <= SLAST): + continue + if f1 - f0 != 1: + continue + t0i, t1 = frames.index(f0), frames.index(f1) + # row lookup by position identity + d0 = np.linalg.norm(positions[t0i] - ps[a], axis=1) + d1 = np.linalg.norm(positions[t1] - ps[a + 1], axis=1) + r0, r1 = int(np.argmin(d0)), int(np.argmin(d1)) + if d0[r0] > 1e-6 or d1[r1] > 1e-6: + continue + claims += 1 + consec += 1 + mp[(t1, r1)] = (t0i, r0) + rp = ref_prev[t1] + if r1 < len(rp) and rp[r1] >= 0 and rp[r1] == r0: + exact += 1 + accs = [] + for (t1, r1), (t0i, r0) in mp.items(): + if (t0i, r0) in mp and t0i - mp[(t0i, r0)][0] == 1: + t_1, r_1 = mp[(t0i, r0)] + accs.append(float(np.linalg.norm( + (positions[t1][r1] - positions[t0i][r0]) - + (positions[t0i][r0] - positions[t_1][r_1])))) + accs = np.array(accs) + lens = np.array([len(t.time()) for t in trajs]) + print(f"{tag}: recall={exact / ref_total:.2%} claims~{claims} " + f"trajs={len(trajs)} lenmean={lens.mean():.2f} " + f"accmean={accs.mean():.3f} rough={(accs > 1.9).sum()}", + flush=True) + + +def main(): + import os + + from flowtracks.repair import repair_trajectories + + from openptv2.gui.parameter_manager import ParameterManager + from openptv2.gui.ptv import py_start_proc_c + from openptv2.tracker import Tracker, default_naming + + restore() + os.chdir(WORK) + try: + pm = ParameterManager() + pm.from_yaml(WORK / "parameters_Run_dacc19.yaml") + cpar, spar, vpar, track_par, tpar, cals, epar = py_start_proc_c(pm) + tr = Tracker(cpar, vpar, track_par, spar, cals, default_naming, + loser_retry=0, cold_start_neighbour=0) + tr.full_forward() + tr.full_backward() + tr.postprocess(cold_start=True, reciprocity=True, + gap_relinking=True, max_gap=2) + finally: + os.chdir(Path.cwd()) + + frames = list(range(FIRST, LAST + 1)) + positions = [] + for f in frames: + lines = (WORK / "res" / f"rt_is.{f}").read_text().splitlines() + n = int(lines[0]) + positions.append(np.array([[float(v) for v in l.split()[1:4]] + for l in lines[1: n + 1]])) + ref_prev = {} + for k, f in enumerate(frames): + lines = (REF / f"ptv_is.{f}").read_text().splitlines() + n = int(lines[0]) + ref_prev[k] = [int(l.split()[0]) for l in lines[1: n + 1]] + ref_total = sum(sum(1 for p in ref_prev[k] if p >= 0) + for k, f in enumerate(frames) if SFIRST <= f <= SLAST) + + tc_chains = chains_from_linkage(WORK / "res") + tc_trajs = to_traj_list(tc_chains, positions, frames) + score_trajs(tc_trajs, positions, frames, ref_prev, ref_total, + "trackcorr+post raw") + t0 = time.perf_counter() + tc_rep, tc_report = repair_trajectories(tc_trajs, long_track=5) + print(f"repair wall: {time.perf_counter() - t0:.1f}s " + f"({len(tc_trajs)} -> {len(tc_rep)} trajs) report={tc_report}", + flush=True) + score_trajs(tc_rep, positions, frames, ref_prev, ref_total, + "trackcorr+post repaired") + + # two-phase + from openptv2.algorithms.calibration import Calibration + from openptv2.algorithms.imgcoord import img_coord_batch + from openptv2.algorithms.parameters import ControlPar + from openptv2.plugins.two_phase_tracking import ( + TwoPhaseTracker, + TwoPhaseTrackerConfig, + ) + cpar = ControlPar.from_file(str(WORK / "parameters" / "ptv.par")) + cals = [Calibration.from_file( + str(WORK / "cal" / f"cam_{c}.tif.ori"), + str(WORK / "cal" / f"cam_{c}.tif.addpar")) for c in range(1, NCAMS + 1)] + leaves = [] + for P in positions: + xy = np.full((len(P), NCAMS * 2), np.nan) + for i, (x, y, z) in enumerate(P): + for ci in range(NCAMS): + m = img_coord_batch(np.array([[x, y, z]]), cals[ci], + cpar.mm)[0] + xy[i, 2 * ci] = m[0] / cpar.pix_x + cpar.imx / 2 + xy[i, 2 * ci + 1] = cpar.imy / 2 - m[1] / cpar.pix_y + leaves.append(np.nan_to_num(xy)) + + def project_fn(pred): + pred = np.asarray(pred, dtype=np.float64) + out = np.full((len(pred), NCAMS * 2), np.nan) + for i, (x, y, z) in enumerate(pred): + for ci in range(NCAMS): + m = img_coord_batch(np.array([[x, y, z]]), cals[ci], cpar.mm)[0] + out[i, 2 * ci] = m[0] / cpar.pix_x + cpar.imx / 2 + out[i, 2 * ci + 1] = cpar.imy / 2 - m[1] / cpar.pix_y + return np.nan_to_num(out) + + cfg = TwoPhaseTrackerConfig(v_max=2.0, leaf_weight=1.0, use_velocity=True, + cost_mode="projected", max_gap=1) + links = TwoPhaseTracker(cfg).track_frames(positions, leaves, + project_fn=project_fn) + nxt = {} + for (t0i, r0, t1, r1) in links: + if t1 - t0i == 1: + nxt[(t0i, r0)] = (t1, r1) + tgt = set(nxt.values()) + tp_chains = [] + for s in sorted(set(nxt) - tgt): + c, k = [s], s + while k in nxt: + k = nxt[k] + c.append(k) + tp_chains.append(c) + tp_trajs = to_traj_list(tp_chains, positions, frames, tid0=10_000_000) + score_trajs(tp_trajs, positions, frames, ref_prev, ref_total, + "two-phase raw") + t0 = time.perf_counter() + tp_rep, tp_report = repair_trajectories(tp_trajs, long_track=5) + print(f"repair wall: {time.perf_counter() - t0:.1f}s " + f"({len(tp_trajs)} -> {len(tp_rep)} trajs) report={tp_report}", + flush=True) + score_trajs(tp_rep, positions, frames, ref_prev, ref_total, + "two-phase repaired") + + +if __name__ == "__main__": + main() diff --git a/scripts/run_cascade.py b/scripts/run_cascade.py new file mode 100644 index 00000000..46b59043 --- /dev/null +++ b/scripts/run_cascade.py @@ -0,0 +1,275 @@ +"""Cascade driver: trackcorr base + additive two-phase merge, scored. + +Modes: + synth : S-scenes with known truth (trackcorr fwd+bwd, then merge). + wp1 : scratch work dir vs 3dptv reference (merged ptv to res_cascade/). + +Run from repo root: + uv run python -u scripts/run_cascade.py --mode synth [--reps N] + uv run python -u scripts/run_cascade.py --mode wp1 +""" + +from __future__ import annotations + +import argparse +import os +import shutil +import sys +from pathlib import Path + +sys.path.insert(0, str(Path(__file__).resolve().parent)) + +import numpy as np # noqa: E402 +from cascade_track import merge_links, write_ptv_is # noqa: E402 + +REPO = Path(__file__).resolve().parent.parent +DS = Path(r"C:\Users\alex\Downloads\HiDImaging\wp1_10_images") +REF = DS / "res_ground_truth_backup" +NCAMS = 4 + + +# ------------------------------------------------------------------ trackcorr + +def run_trackcorr(work: Path, params_file: str, backward: bool = True): + from openptv2.gui.parameter_manager import ParameterManager + from openptv2.gui.ptv import py_start_proc_c + from openptv2.tracker import Tracker, default_naming + + old = os.getcwd() + os.chdir(work) + try: + pm = ParameterManager() + pm.from_yaml(work / params_file) + cpar, spar, vpar, track_par, tpar, cals, epar = py_start_proc_c(pm) + for cam_id, short in enumerate(pm.get_target_filenames()): + spar.set_img_base_name(cam_id, str(Path(short).resolve()) + ".") + tr = Tracker(cpar, vpar, track_par, spar, cals, default_naming, + loser_retry=0, cold_start_neighbour=0) + tr.full_forward() + if backward: + tr.full_backward() + finally: + os.chdir(old) + + +def read_pn(res: Path, frame: int): + lines = (res / f"ptv_is.{frame}").read_text().splitlines() + n = int(lines[0]) + prev, nxt = [], [] + for line in lines[1: n + 1]: + p = line.split() + prev.append(int(p[0])) + nxt.append(int(p[1])) + return prev, nxt + + +# ------------------------------------------------------------------ two-phase + +def run_two_phase_arrays(positions, leaves, project_fn, v_max: float, + max_gap: int): + from openptv2.plugins.two_phase_tracking import ( + TwoPhaseTracker, + TwoPhaseTrackerConfig, + ) + + cfg = TwoPhaseTrackerConfig(v_max=v_max, leaf_weight=1.0, + use_velocity=True, cost_mode="projected", + max_gap=max_gap) + return TwoPhaseTracker(cfg).track_frames(positions, leaves, + project_fn=project_fn) + + +# ------------------------------------------------------------------ synth mode + +def mode_synth(reps: int): + from bench_two_phase import V_MAX, make_leaves, project_points, score_links + from synth_crossing import ( + FIRST, + NF, + load_optics, + make_truth, + setup_work, + write_scene, + ) + + cpar, cals = load_optics() + scenarios = [ + ("S1 head-on", {"v": 0.5}), + ("S8 kick", {"v": 0.5, "maneuver": "kick"}), + ("S11 kick+noise", {"v": 0.5, "maneuver": "kick", "noise": 0.07}), + ("S4 head-on+noise", {"v": 0.5, "noise": 0.07}), + ("S13 gap-occl", {"v": 0.5, "dropout": True}), + ] + print(f"{'scenario':16}{'base':>9}{'merged':>9}{'+exact':>8}", + flush=True) + for sname, kw in scenarios: + noisy = kw.get("noise", 0.0) > 0 + nreps = reps if noisy else 3 + base_r, merg_r, adds = [], [], [] + for rep in range(nreps): + work = REPO / "scratch" / "_synth_casc" + setup_work(work) + truth = make_truth(seed=1000 + rep, **kw) + rows_pf = write_scene(work, truth, cpar, cals) + run_trackcorr(work, "parameters_Run1.yaml", backward=True) + # base score from corr ptv + corr_prev, corr_next, nrows = [], [], [] + for f in range(NF): + p, x = read_pn(work / "res", FIRST + f) + corr_prev.append(p) + corr_next.append(x) + nrows.append(len(p)) + from bench_two_phase import _pid_frames + exp = set() + for pf in _pid_frames(rows_pf, NF, FIRST): + for a, b in zip(pf, pf[1:]): + exp.add((a[0], a[1], b[0], b[1])) + got_base = {(t0, r0, t0 + 1, r1) + for t0 in range(NF - 1) for r0, r1 in + enumerate(corr_next[t0]) if r1 >= 0} + base_r.append(len(exp & got_base) / len(exp)) + # two-phase on the same positions + positions, leaves = [], [] + for f in range(NF): + lines = (work / "res" / f"rt_is.{FIRST + f}" + ).read_text().splitlines() + n = int(lines[0]) + P = np.array([[float(v) for v in l.split()[1:4]] + for l in lines[1: n + 1]]) + positions.append(P) + leaves.append(project_points(P, cpar, cals)) + tp = run_two_phase_arrays( + positions, leaves, + lambda P: project_points(np.asarray(P), cpar, cals), + V_MAX, 2) + mp, mn, nadd, added = merge_links(corr_prev, corr_next, tp, + nrows) + got_m = {(t0, r0, t0 + 1, r1) + for t0 in range(NF - 1) for r0, r1 in enumerate(mn[t0]) + if r1 >= 0} | set(added) + merg_r.append(len(exp & got_m) / len(exp)) + adds.append(nadd) + print(f"{sname:16}{np.mean(base_r):>8.1%}{np.mean(merg_r):>8.1%} " + f"+{np.mean(adds):.1f} links", flush=True) + + +# ------------------------------------------------------------------ wp1 mode + +def mode_wp1(): + from openptv2.algorithms.calibration import Calibration + from openptv2.algorithms.imgcoord import img_coord_batch + from openptv2.algorithms.parameters import ControlPar + + work = (REPO / "scratch" / "_wp1_ab").resolve() + first, last = 100001, 100010 + frames = list(range(first, last + 1)) + out = work / "res_cascade" + + for f in frames: + shutil.copyfile(REF / f"rt_is.{f}", work / "res" / f"rt_is.{f}") + for cam in range(1, NCAMS + 1): + shutil.copyfile( + DS / "img_3dptv" / f"Cam{cam}.{f}_targets", + work / "img_3dptv" / f"Cam{cam}.{f}_targets") + + run_trackcorr(work, "parameters_Run_dacc19.yaml", backward=True) + + cpar = ControlPar.from_file(str(work / "parameters" / "ptv.par")) + cals = [Calibration.from_file( + str(work / "cal" / f"cam_{c}.tif.ori"), + str(work / "cal" / f"cam_{c}.tif.addpar")) + for c in range(1, NCAMS + 1)] + + positions, leaves = [], [] + for f in frames: + lines = (work / "res" / f"rt_is.{f}").read_text().splitlines() + n = int(lines[0]) + P = np.array([[float(v) for v in l.split()[1:4]] + for l in lines[1: n + 1]]) + positions.append(P) + xy = np.full((n, NCAMS * 2), np.nan) + for i, (x, y, z) in enumerate(P): + for ci in range(NCAMS): + m = img_coord_batch(np.array([[x, y, z]]), cals[ci], + cpar.mm)[0] + xy[i, 2 * ci] = m[0] / cpar.pix_x + cpar.imx / 2 + xy[i, 2 * ci + 1] = cpar.imy / 2 - m[1] / cpar.pix_y + leaves.append(np.nan_to_num(xy)) + + def project_fn(pred): + pred = np.asarray(pred, dtype=np.float64) + out = np.full((len(pred), NCAMS * 2), np.nan) + for i, (x, y, z) in enumerate(pred): + for ci in range(NCAMS): + m = img_coord_batch(np.array([[x, y, z]]), cals[ci], + cpar.mm)[0] + out[i, 2 * ci] = m[0] / cpar.pix_x + cpar.imx / 2 + out[i, 2 * ci + 1] = cpar.imy / 2 - m[1] / cpar.pix_y + return np.nan_to_num(out) + + tp = run_two_phase_arrays(positions, leaves, project_fn, v_max=2.0, + max_gap=1) + corr_prev, corr_next, nrows = [], [], [] + for f in frames: + p, x = read_pn(work / "res", f) + corr_prev.append(p) + corr_next.append(x) + nrows.append(len(p)) + mp, mn, nadd, added = merge_links(corr_prev, corr_next, tp, nrows) + write_ptv_is(out, frames, mp, mn, positions) + + ref_total = match_base = match_merg = 0 + # exact/extra split by span using merge-time records + ref_prev_all = {} + for k, f in enumerate(frames): + lines = (REF / f"ptv_is.{f}").read_text().splitlines() + n = int(lines[0]) + ref_prev_all[k] = [int(l.split()[0]) for l in lines[1: n + 1]] + span_stats = {"consec": [0, 0], "gap": [0, 0]} # [exact, extra] + for t0, r0, t1, r1 in added: + f1 = frames[t1] + if not (100003 <= f1 <= 100010): + continue + rp = ref_prev_all[t1] + key = "gap" if t1 - t0 > 1 else "consec" + if r1 < len(rp) and rp[r1] >= 0 and rp[r1] == r0: + span_stats[key][0] += 1 + else: + span_stats[key][1] += 1 + print(f"added split: consecutive exact={span_stats['consec'][0]} " + f"extra={span_stats['consec'][1]}; gap exact={span_stats['gap'][0]}" + f" extra={span_stats['gap'][1]}", flush=True) + + ref_total = match_base = match_merg = 0 + for k, f in enumerate(frames): + if not (100003 <= f <= 100010): + continue + lines = (REF / f"ptv_is.{f}").read_text().splitlines() + n = int(lines[0]) + rp = [int(l.split()[0]) for l in lines[1: n + 1]] + ref_total += sum(1 for p in rp if p >= 0) + for i in range(min(n, nrows[k])): + if rp[i] >= 0: + if corr_prev[k][i] == rp[i]: + match_base += 1 + if mp[k][i] == rp[i]: + match_merg += 1 + print(f"wp1 cascade: base={match_base}/{ref_total} " + f"({match_base / ref_total:.2%}) merged={match_merg}/{ref_total} " + f"({match_merg / ref_total:.2%}) +{nadd} links -> {out}", + flush=True) + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument("--mode", choices=["synth", "wp1"], required=True) + ap.add_argument("--reps", type=int, default=8) + args = ap.parse_args() + if args.mode == "synth": + mode_synth(args.reps) + else: + mode_wp1() + + +if __name__ == "__main__": + main() diff --git a/scripts/run_dacc19.py b/scripts/run_dacc19.py new file mode 100644 index 00000000..e56990ae --- /dev/null +++ b/scripts/run_dacc19.py @@ -0,0 +1,71 @@ +"""Decisive T0.1 follow-up: run openptv2 with dacc=1.9 (the 3dptv value). + +The work folder YAML sets dacc=0.8 while track.par / 3dptv use dacc=1.9. +17.9% of reference links have 3-point acc >= 0.8 vs a 16.3% exact-link gap. +If this run reproduces ~100% of reference links, the gap is config. +""" + +import os +import shutil +import sys +from pathlib import Path + +REPO = Path(__file__).resolve().parent.parent +DATASET = Path(r"C:\Users\alex\Downloads\HiDImaging\wp1_10_images") +REF = DATASET / "res_ground_truth_backup" +FIRST, LAST = 100001, 100010 +SCORE_FIRST, SCORE_LAST = 100003, 100010 + + +def main(): + work = Path(sys.argv[1]).resolve() if len(sys.argv) > 1 else ( + REPO / "scratch" / "_wp1_ab").resolve() + dacc = float(sys.argv[2]) if len(sys.argv) > 2 else 1.9 + + for f in range(FIRST, LAST + 1): + shutil.copyfile(REF / f"rt_is.{f}", work / "res" / f"rt_is.{f}") + for cam in range(1, 5): + shutil.copyfile( + DATASET / "img_3dptv" / f"Cam{cam}.{f}_targets", + work / "img_3dptv" / f"Cam{cam}.{f}_targets", + ) + print(f"inputs restored; running dacc={dacc}", flush=True) + + from openptv2.gui.parameter_manager import ParameterManager + from openptv2.gui.ptv import py_start_proc_c + from openptv2.tracker import Tracker, default_naming + + old = os.getcwd() + os.chdir(work) + try: + pm = ParameterManager() + pm.from_yaml(work / "parameters_Run1.yaml") + cpar, spar, vpar, track_par, tpar, cals, epar = py_start_proc_c(pm) + for cam_id, short in enumerate(pm.get_target_filenames()): + spar.set_img_base_name(cam_id, str(Path(short).resolve()) + ".") + print(f"effective dacc before override: {track_par.dacc}", flush=True) + track_par.dacc = dacc + Tracker(cpar, vpar, track_par, spar, cals, default_naming, + loser_retry=0, cold_start_neighbour=0).full_forward() + finally: + os.chdir(old) + + def read_links(res: Path, frame: int): + lines = (res / f"ptv_is.{frame}").read_text().strip().splitlines() + n = int(lines[0]) + return [int(line.split()[0]) for line in lines[1: n + 1]] + + ref_total = test_total = match = 0 + for f in range(SCORE_FIRST, SCORE_LAST + 1): + r = read_links(REF, f) + t = read_links(work / "res", f) + ref_total += sum(1 for p in r if p >= 0) + test_total += sum(1 for p in t if p >= 0) + match += sum(1 for i in range(min(len(r), len(t))) + if r[i] >= 0 and t[i] == r[i]) + print(f"dacc={dacc}: ref={ref_total} test={test_total} " + f"exact={match} recall={match / ref_total:.2%}", flush=True) + + +if __name__ == "__main__": + main() diff --git a/scripts/run_postprocess_wp1.py b/scripts/run_postprocess_wp1.py new file mode 100644 index 00000000..e3483d4e --- /dev/null +++ b/scripts/run_postprocess_wp1.py @@ -0,0 +1,121 @@ +"""Willneff-pipeline completion: forward + backward + postprocess on wp1. + +The standard openptv2 plugin flow runs tracker.postprocess() (cold-start +seeding, gap relinking, reciprocity enforcement) after tracking -- our +verification runs skipped it. This script measures exactly what it adds: +link efficiency, chain census, and exact reproduction vs the 3dptv reference. + +Run from repo root: + uv run python -u scripts/run_postprocess_wp1.py +""" + +from __future__ import annotations + +import os +import shutil +from pathlib import Path + +import numpy as np + +REPO = Path(__file__).resolve().parent.parent +DS = Path(r"C:\Users\alex\Downloads\HiDImaging\wp1_10_images") +REF = DS / "res_ground_truth_backup" +WORK = (REPO / "scratch" / "_wp1_ab").resolve() +FIRST, LAST = 100001, 100010 +NCAMS = 4 + + +def load_all(res: Path): + prev, nxt = {}, {} + for f in range(FIRST, LAST + 1): + lines = (res / f"ptv_is.{f}").read_text().splitlines() + n = int(lines[0]) + p, x = [], [] + for line in lines[1: n + 1]: + s = line.split() + p.append(int(s[0])) + x.append(int(s[1])) + prev[f], nxt[f] = p, x + return prev, nxt + + +def census(prev, nxt, tag: str): + eff = np.mean([sum(1 for v in nxt[f] if v >= 0) / max(len(nxt[f]), 1) + for f in range(FIRST, LAST)]) + links = np.mean([sum(1 for v in nxt[f] if v >= 0) + for f in range(FIRST, LAST)]) + chains = [] + for f in range(FIRST, LAST + 1): + for i in range(len(prev[f])): + if prev[f][i] < 0: + ch = [(f, i)] + cf, ci = f, i + while cf <= LAST and ci < len(nxt[cf]) and nxt[cf][ci] >= 0: + ci = nxt[cf][ci] + cf += 1 + ch.append((cf, ci)) + chains.append(ch) + L = np.array([len(c) for c in chains]) + import collections + h = collections.Counter(len(c) for c in chains) + print(f"{tag}: eff={eff:.1%} links/step={links:.0f} chains={len(chains)} " + f"meanlen={L.mean():.2f} fullspan={h[LAST - FIRST + 1]} " + f"len>=5={sum(v for k, v in h.items() if k >= 5)} " + f"len>=8={sum(v for k, v in h.items() if k >= 8)}", flush=True) + return chains + + +def score_links(res: Path, first: int, last: int): + tot = match = 0 + for f in range(first, last + 1): + lines = (res / f"ptv_is.{f}").read_text().splitlines() + n = int(lines[0]) + t = [int(l.split()[0]) for l in lines[1: n + 1]] + rlines = (REF / f"ptv_is.{f}").read_text().splitlines() + rn = int(rlines[0]) + r = [int(l.split()[0]) for l in rlines[1: rn + 1]] + tot += sum(1 for p in r if p >= 0) + match += sum(1 for i in range(min(n, rn)) if r[i] >= 0 and t[i] == r[i]) + return tot, match + + +def main(): + from openptv2.gui.parameter_manager import ParameterManager + from openptv2.gui.ptv import py_start_proc_c + from openptv2.tracker import Tracker, default_naming + + for f in range(FIRST, LAST + 1): + shutil.copyfile(REF / f"rt_is.{f}", WORK / "res" / f"rt_is.{f}") + for cam in range(1, NCAMS + 1): + shutil.copyfile( + DS / "img_3dptv" / f"Cam{cam}.{f}_targets", + WORK / "img_3dptv" / f"Cam{cam}.{f}_targets") + + old = os.getcwd() + os.chdir(WORK) + try: + pm = ParameterManager() + pm.from_yaml(WORK / "parameters_Run_dacc19.yaml") + cpar, spar, vpar, track_par, tpar, cals, epar = py_start_proc_c(pm) + for cam_id, short in enumerate(pm.get_target_filenames()): + spar.set_img_base_name(cam_id, str(Path(short).resolve()) + ".") + tr = Tracker(cpar, vpar, track_par, spar, cals, default_naming, + loser_retry=0, cold_start_neighbour=0) + tr.full_forward() + tr.full_backward() + p0, x0 = load_all(WORK / "res") + census(p0, x0, "fwd+bwd ") + t, m = score_links(WORK / "res", 100003, 100010) + print(f"fwd+bwd exact: {m}/{t} = {m / t:.2%}", flush=True) + stats = tr.postprocess() + print(f"postprocess stats: {stats}", flush=True) + p1, x1 = load_all(WORK / "res") + census(p1, x1, "fwd+bwd+post ") + t, m = score_links(WORK / "res", 100003, 100010) + print(f"+post exact: {m}/{t} = {m / t:.2%}", flush=True) + finally: + os.chdir(old) + + +if __name__ == "__main__": + main() diff --git a/scripts/sweep_app_weight.py b/scripts/sweep_app_weight.py new file mode 100644 index 00000000..0a91ce3c --- /dev/null +++ b/scripts/sweep_app_weight.py @@ -0,0 +1,72 @@ +"""Tune app_weight on S15 (appearance-decides kick) + no-regression spot. + +S15: bright A (2000) vs dim B (500), kick maneuver + noise + 10% sumg +jitter. Kinematics favor the swap; appearance favors truth. Expect a +fail -> pass transition as app_weight grows, with S1/S8/S11 unchanged. + +Run from repo root: + uv run python -u scripts/sweep_app_weight.py [--reps N] +""" + +import sys +from pathlib import Path + +sys.path.insert(0, str(Path(__file__).resolve().parent)) + +import numpy as np # noqa: E402 +from synth_crossing import ( # noqa: E402 + REPO, + load_optics, + make_truth, + run_tracker, + score, + setup_work, + write_scene, +) + + +def main(): + import argparse + ap = argparse.ArgumentParser() + ap.add_argument("--reps", type=int, default=8) + args = ap.parse_args() + + cpar, cals = load_optics() + s15 = {"v": 0.5, "maneuver": "kick", "noise": 0.07, + "_sumg": {0: 2000.0, 1: 500.0}, "_sj": 0.10} + sfn = lambda pid, frame: {0: 2000.0, 1: 500.0}.get(pid, 1000.0) # noqa: E731 + + print(f"{'w_app':>7}{'S15 recall':>12}{'cross-ok':>10} " + f"{'S1':>6}{'S8':>6}{'S11r':>7}", flush=True) + for w in (0.0, 0.05, 0.1, 0.2, 0.4, 0.8): + recs, crs = [], [] + for rep in range(args.reps): + work = REPO / "scratch" / "_synth_app" + setup_work(work) + truth = make_truth(seed=1000 + rep, **{k: v for k, v in + s15.items() + if not k.startswith("_")}) + rows_pf = write_scene(work, truth, cpar, cals, sumg_fn=sfn, + sumg_jitter=0.10, seed=2000 + rep) + run_tracker(work, 0, 0, app_weight=w) + r, c = score(work, rows_pf) + recs.append(r) + crs.append(c) + # no-regression spot checks (S1/S8 single, S11 mean recall) + spots = [] + for skw in ({"v": 0.5}, + {"v": 0.5, "maneuver": "kick"}, + {"v": 0.5, "maneuver": "kick", "noise": 0.07}): + work = REPO / "scratch" / "_synth_app" + setup_work(work) + truth = make_truth(seed=5, **skw) + rows_pf = write_scene(work, truth, cpar, cals) + run_tracker(work, 0, 0, app_weight=w) + r, _ = score(work, rows_pf) + spots.append(r) + print(f"{w:>7.2f}{np.mean(recs):>11.1%} {sum(crs)}/{len(crs)} " + f"{spots[0]:>5.0%}{spots[1]:>6.0%}{spots[2]:>7.1%}", flush=True) + + +if __name__ == "__main__": + main() diff --git a/scripts/sweep_track_params.py b/scripts/sweep_track_params.py new file mode 100644 index 00000000..1bd200e7 --- /dev/null +++ b/scripts/sweep_track_params.py @@ -0,0 +1,149 @@ +"""T2.1 — parameter sensitivity sweep: which gate binds the missing links? + +Runs full forward tracking on the wp1 working folder from PRISTINE inputs +(restored before every run: rt_is.* from the 3dptv reference backup, +Cam*_targets from the dataset) while widening one track.par parameter at a +time. Scores exact-link reproduction vs the 3dptv reference on clean frames. + +Methodology note: a tracking run REWRITES res/rt_is.* (added particles) and +updates target-file tnr columns in place, so re-running in the same folder +without restoring inputs silently changes the inputs. Every run here starts +from byte-identical inputs (verified by hash before the run). + +Run from repo root: + uv run python scripts/sweep_track_params.py + +Baseline reproduces the plan's 59.0% openptv2 link rate; each row shows which +parameter closes the gap toward 3dptv's fixed 69.5%. +""" + +from __future__ import annotations + +import argparse +import hashlib +import os +import shutil +from pathlib import Path + +REPO = Path(__file__).resolve().parent.parent +DATASET = Path(r"C:\Users\alex\Downloads\HiDImaging\wp1_10_images") +REF = DATASET / "res_ground_truth_backup" +FIRST, LAST = 100001, 100010 +SCORE_FIRST, SCORE_LAST = 100003, 100010 # clean frames (100001 is corrupt) + + +def restore_inputs(work: Path) -> None: + """Copy pristine rt_is + target files into the work folder.""" + res = work / "res" + img = work / "img_3dptv" + for f in range(FIRST, LAST + 1): + shutil.copyfile(REF / f"rt_is.{f}", res / f"rt_is.{f}") + for cam in range(1, 5): + shutil.copyfile( + DATASET / "img_3dptv" / f"Cam{cam}.{f}_targets", + img / f"Cam{cam}.{f}_targets", + ) + + +def hash_inputs(work: Path) -> str: + h = hashlib.sha256() + for f in range(FIRST, LAST + 1): + h.update((work / "res" / f"rt_is.{f}").read_bytes()) + for cam in range(1, 5): + h.update( + (work / "img_3dptv" / f"Cam{cam}.{f}_targets").read_bytes() + ) + return h.hexdigest()[:16] + + +def run_once(work: Path, overrides: dict, loser_retry: int, + cold_start_neighbour: int) -> None: + from openptv2.gui.parameter_manager import ParameterManager + from openptv2.gui.ptv import py_start_proc_c + from openptv2.tracker import Tracker, default_naming + + old = os.getcwd() + os.chdir(work) + try: + pm = ParameterManager() + pm.from_yaml(work / "parameters_Run1.yaml") + cpar, spar, vpar, track_par, tpar, cals, epar = py_start_proc_c(pm) + for cam_id, short in enumerate(pm.get_target_filenames()): + spar.set_img_base_name(cam_id, str(Path(short).resolve()) + ".") + for key, val in overrides.items(): + setattr(track_par, key, val) + t = Tracker( + cpar, vpar, track_par, spar, cals, default_naming, + loser_retry=loser_retry, + cold_start_neighbour=cold_start_neighbour, + ) + t.full_forward() + finally: + os.chdir(old) + + +def read_links(res: Path, frame: int): + p = res / f"ptv_is.{frame}" + lines = p.read_text().strip().splitlines() + n = int(lines[0]) + return [int(line.split()[0]) for line in lines[1: n + 1]] + + +def score(work: Path): + ref_total = test_total = match = 0 + for f in range(SCORE_FIRST, SCORE_LAST + 1): + r = read_links(REF, f) + t = read_links(work / "res", f) + ref_total += sum(1 for p in r if p >= 0) + test_total += sum(1 for p in t if p >= 0) + for i in range(min(len(r), len(t))): + if r[i] >= 0 and t[i] == r[i]: + match += 1 + return ref_total, test_total, match + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument("--work", type=Path, + default=REPO / "scratch" / "_wp1_ab") + args = ap.parse_args() + work = args.work.resolve() + + print(f"reference dir: {REF}") + print(f"scoring frames {SCORE_FIRST}..{SCORE_LAST} (clean frames)") + + grid = [ + ("baseline (track.c parity)", {}, 0, 0), + ("baseline (openptv2 defaults)", {}, 1, 1), + ("dv xmax 1.9 -> 2.5", + {"dvxmin": -2.5, "dvxmax": 2.5, "dvymin": -2.5, "dvymax": 2.5, + "dvzmin": -2.5, "dvzmax": 2.5}, 0, 0), + ("dv xmax 1.9 -> 4.0", + {"dvxmin": -4.0, "dvxmax": 4.0, "dvymin": -4.0, "dvymax": 4.0, + "dvzmin": -4.0, "dvzmax": 4.0}, 0, 0), + ("dacc 1.9 -> 3.0", {"dacc": 3.0}, 0, 0), + ("dacc 1.9 -> 6.0", {"dacc": 6.0}, 0, 0), + ("dangle 270 -> 400", {"dangle": 400.0}, 0, 0), + ("add 1 -> 0", {"add": 0}, 0, 0), + ] + + print(f"\n{'config':32}{'ref':>7}{'test':>7}{'exact':>7}" + f"{'recall':>9}{'rate-vs-ref':>12}") + results = [] + for label, overrides, lr, csn in grid: + restore_inputs(work) + h = hash_inputs(work) + run_once(work, overrides, lr, csn) + ref_total, test_total, match = score(work) + recall = match / ref_total if ref_total else 0.0 + results.append((label, ref_total, test_total, match, recall, h)) + print(f"{label:32}{ref_total:>7}{test_total:>7}{match:>7}" + f"{recall:>8.1%} ({match}/{ref_total}, inputs {h})") + + print("\nDecision: whichever parameter moves recall toward ~100% of the " + "reference link count names the binding constraint. If none moves " + "it, the loss is structural (see T1.1 census).") + + +if __name__ == "__main__": + main() diff --git a/scripts/sweep_twophase_wp1.py b/scripts/sweep_twophase_wp1.py new file mode 100644 index 00000000..dd5125c5 --- /dev/null +++ b/scripts/sweep_twophase_wp1.py @@ -0,0 +1,149 @@ +"""Sweep two-phase configs on wp1 to beat trackcorr (98.16% / +31 claims). + +Leaves built once, configs swept (each track run is milliseconds). +Scores: link recall, claim count, chain census, smoothness. + +Run from repo root: + uv run python -u scripts/sweep_twophase_wp1.py +""" + +from __future__ import annotations + +import shutil +import sys +import time +from pathlib import Path + +sys.path.insert(0, str(Path(__file__).resolve().parent)) + +import numpy as np # noqa: E402 +from verify_same_trajectories import DS, REF # noqa: E402 + +WORK = (Path(__file__).resolve().parent.parent / "scratch" / "_wp1_ab").resolve() +FIRST, LAST = 100001, 100010 +SFIRST, SLAST = 100003, 100010 +NCAMS = 4 + + +def main(): + from openptv2.algorithms.calibration import Calibration + from openptv2.algorithms.imgcoord import img_coord_batch + from openptv2.algorithms.parameters import ControlPar + from openptv2.plugins.two_phase_tracking import ( + TwoPhaseTracker, + TwoPhaseTrackerConfig, + ) + + for f in range(FIRST, LAST + 1): + shutil.copyfile(REF / f"rt_is.{f}", WORK / "res" / f"rt_is.{f}") + cpar = ControlPar.from_file(str(WORK / "parameters" / "ptv.par")) + cals = [Calibration.from_file( + str(WORK / "cal" / f"cam_{c}.tif.ori"), + str(WORK / "cal" / f"cam_{c}.tif.addpar")) for c in range(1, NCAMS + 1)] + frames = list(range(FIRST, LAST + 1)) + positions = [] + for f in frames: + lines = (WORK / "res" / f"rt_is.{f}").read_text().splitlines() + n = int(lines[0]) + positions.append(np.array([[float(v) for v in l.split()[1:4]] + for l in lines[1: n + 1]])) + t0 = time.perf_counter() + leaves = [] + for P in positions: + xy = np.full((len(P), NCAMS * 2), np.nan) + for i, (x, y, z) in enumerate(P): + for ci in range(NCAMS): + m = img_coord_batch(np.array([[x, y, z]]), cals[ci], + cpar.mm)[0] + xy[i, 2 * ci] = m[0] / cpar.pix_x + cpar.imx / 2 + xy[i, 2 * ci + 1] = cpar.imy / 2 - m[1] / cpar.pix_y + leaves.append(np.nan_to_num(xy)) + print(f"leaves build: {time.perf_counter() - t0:.1f}s", flush=True) + + def project_fn(pred): + pred = np.asarray(pred, dtype=np.float64) + out = np.full((len(pred), NCAMS * 2), np.nan) + for i, (x, y, z) in enumerate(pred): + for ci in range(NCAMS): + m = img_coord_batch(np.array([[x, y, z]]), cals[ci], cpar.mm)[0] + out[i, 2 * ci] = m[0] / cpar.pix_x + cpar.imx / 2 + out[i, 2 * ci + 1] = cpar.imy / 2 - m[1] / cpar.pix_y + return np.nan_to_num(out) + + ref_prev = {} + for k, f in enumerate(frames): + lines = (REF / f"ptv_is.{f}").read_text().splitlines() + n = int(lines[0]) + ref_prev[k] = [int(l.split()[0]) for l in lines[1: n + 1]] + ref_total = sum(sum(1 for p in ref_prev[k] if p >= 0) + for k, f in enumerate(frames) if SFIRST <= f <= SLAST) + + def score(links, tag): + exact = consec = gaps = 0 + mp = {} + for (t0i, r0, t1, r1) in links: + if not (SFIRST <= frames[t1] <= SLAST): + continue + if t1 - t0i > 1: + gaps += 1 + continue + consec += 1 + mp[(t1, r1)] = (t0i, r0) + rp = ref_prev[t1] + if r1 < len(rp) and rp[r1] >= 0 and rp[r1] == r0: + exact += 1 + # chains + smoothness + nxt = {(a, b): (c, d) for (a, b, c, d) in links if c - a == 1} + tgt = set(nxt.values()) + chains = [] + for s in sorted(set(nxt) - tgt): + c, k = [s], s + while k in nxt: + k = nxt[k] + c.append(k) + chains.append(c) + accs = [] + for (t1, r1), (t0i, r0) in mp.items(): + if (t0i, r0) in mp and t0i - mp[(t0i, r0)][0] == 1: + t_1, r_1 = mp[(t0i, r0)] + accs.append(float(np.linalg.norm( + (positions[t1][r1] - positions[t0i][r0]) - + (positions[t0i][r0] - positions[t_1][r_1])))) + accs = np.array(accs) + print(f"{tag}: recall={exact / ref_total:.2%} claims={len(links)} " + f"consec={consec} gaps={gaps} chains={len(chains)} " + f"accmean={accs.mean():.3f} rough={(accs > 1.9).sum()}", + flush=True) + + cfgs = [ + ("base v2.0 proj", dict(v_max=2.0)), + ("confirm0.5", dict(v_max=2.0, confirm_tol=0.5)), + ("confirm1.0", dict(v_max=2.0, confirm_tol=1.0)), + ("confirm1.5", dict(v_max=2.0, confirm_tol=1.5)), + ("confirm2.0", dict(v_max=2.0, confirm_tol=2.0)), + ("confirm3.0", dict(v_max=2.0, confirm_tol=3.0)), + ("confirm1.0+ends", dict(v_max=2.0, confirm_tol=1.0, + confirm_ends=True)), + ("confirm1.5+ends", dict(v_max=2.0, confirm_tol=1.5, + confirm_ends=True)), + ("confirm2.0+ends", dict(v_max=2.0, confirm_tol=2.0, + confirm_ends=True)), + ("v2.2 confirm1.5+ends", dict(v_max=2.2, confirm_tol=1.5, + confirm_ends=True)), + ("v2.5 confirm2.0+ends", dict(v_max=2.5, confirm_tol=2.0, + confirm_ends=True)), + ] + for tag, kw in cfgs: + t0 = time.perf_counter() + base = dict(leaf_weight=1.0, use_velocity=True, + cost_mode="projected", max_gap=1) + base.update(kw) + cfg = TwoPhaseTrackerConfig(**base) + links = TwoPhaseTracker(cfg).track_frames(positions, leaves, + project_fn=project_fn) + dt = time.perf_counter() - t0 + score(links, f"{tag} ({dt:.2f}s)") + + +if __name__ == "__main__": + main() diff --git a/scripts/synth_crossing.py b/scripts/synth_crossing.py new file mode 100644 index 00000000..e396d8be --- /dev/null +++ b/scripts/synth_crossing.py @@ -0,0 +1,366 @@ +"""Synthetic crossing laboratory: which decision is correct in hard cases? + +Two (or three) particles cross between frames with KNOWN ground truth, so +every contested decision is scored instead of debated. Answers the case-2 +question empirically: violent-vs-smooth -- who is right, and under which +conditions does openptv2 choose wrong? + +Scenes use the real wp1 calibration (4 cams) + real position noise +(sigma=0.07 mm). Work dir is a scratch copy of _wp1_ab with the dacc=1.9 +parity yaml, so optics/params match the verification runs. + +Scenarios (A/B cross between frames 3->4 of 0..6, speed v mm/frame): + S1 head-on : miss distance eps=0 + S2 near-miss : eps=0.15 (below noise floor) + S3 near-miss : eps=0.40 + S4 head-on+noise + S5 fast : v=1.2, eps=0 + S6 three-way : C crosses through the intersection at the cross frame + S7 asymmetric : vA=0.7, vB=0.35 + +Run from repo root: + uv run python -u scripts/synth_crossing.py [--reps N] +""" + +from __future__ import annotations + +import argparse +import os +import shutil +from pathlib import Path + +import numpy as np +import yaml + +from openptv2.algorithms.calibration import Calibration +from openptv2.algorithms.imgcoord import img_coord_batch +from openptv2.algorithms.parameters import ControlPar + +REPO = Path(__file__).resolve().parent.parent +SRC = REPO / "scratch" / "_wp1_ab" +NCAMS = 4 +FIRST = 200001 +NF = 10 +X0, Y0, Z0 = 39.0, 43.0, -40.0 # real-data-like, verified visible below + + +def setup_work(work: Path) -> None: + if work.exists(): + shutil.rmtree(work) + (work / "img_3dptv").mkdir(parents=True) + (work / "res").mkdir(parents=True) + shutil.copytree(SRC / "cal", work / "cal") + shutil.copytree(SRC / "parameters", work / "parameters") + shutil.copy(SRC / "parameters_Run_dacc19.yaml", + work / "parameters_Run1.yaml") + y = yaml.safe_load((work / "parameters_Run1.yaml").read_text()) + for key in ("sequence", "Sequence", "sequence_par"): + if key in y: + y[key]["first"] = FIRST + y[key]["last"] = FIRST + NF - 1 + (work / "parameters_Run1.yaml").write_text(yaml.safe_dump(y)) + + +def load_optics(): + cpar = ControlPar.from_file(str(SRC / "parameters" / "ptv.par")) + cals = [ + Calibration.from_file( + str(SRC / "cal" / f"cam_{c}.tif.ori"), + str(SRC / "cal" / f"cam_{c}.tif.addpar"), + ) + for c in range(1, NCAMS + 1) + ] + return cpar, cals + + +def project(cpar, cals, x, y, z): + out = [] + for ci in range(NCAMS): + xy = img_coord_batch(np.array([[x, y, z]], dtype=np.float64), + cals[ci], cpar.mm)[0] + px = xy[0] / cpar.pix_x + cpar.imx / 2 + py = cpar.imy / 2 - xy[1] / cpar.pix_y + out.append((px, py)) + return out + + +def write_scene(work: Path, truth, cpar, cals, sumg_fn=None, + sumg_jitter: float = 0.0, seed: int = 0): + """truth: {pid: {frame: (x,y,z)}} (a pid may skip frames = occlusion). + + Rows in pid order per frame; targets y-sorted. Returns per-frame + row maps {frame: {pid: row}} (rows shift when a pid is absent, so + scoring must use these, not a static map). + + sumg_fn(pid, frame) -> base grey sum for that particle (default 1000); + per-target detection jitter ~ N(0, sumg_jitter * base) models + illumination/view variation. + """ + import math as _math + + rng = np.random.default_rng(seed) + if sumg_fn is None: + sumg_fn = lambda pid, frame: 1000.0 # noqa: E731 + pids = sorted(truth) + rows_pf = {} + for f in range(NF): + frame = FIRST + f + present = [pid for pid in pids if frame in truth[pid]] + rows_pf[frame] = {pid: k for k, pid in enumerate(present)} + det = {c: [] for c in range(NCAMS)} # (px, py, pid) + for pid in present: + x, y, z = truth[pid][frame] + for ci, (px, py) in enumerate(project(cpar, cals, x, y, z)): + assert 0 <= px < cpar.imx and 0 <= py < cpar.imy, \ + f"pid {pid} frame {frame} cam {ci} out of view " \ + f"({px:.1f},{py:.1f})" + det[ci].append((px, py, pid)) + per_cam, pnrs_of = {}, {} + for ci in range(NCAMS): + det[ci].sort(key=lambda t: t[1]) # y-sort for candsearch + per_cam[ci] = [] + for idx, (px, py, pid) in enumerate(det[ci]): + base = float(sumg_fn(pid, frame)) + sg = base + (rng.normal(0, sumg_jitter * base) + if sumg_jitter else 0.0) + per_cam[ci].append((idx, px, py, 50, 5, 5, max(sg, 1.0), + rows_pf[frame][pid])) + pnrs_of[(pid, ci)] = idx + rt_rows = [] + for pid in present: + x, y, z = truth[pid][frame] + pnrs = [pnrs_of[(pid, ci)] for ci in range(NCAMS)] + rt_rows.append((len(rt_rows) + 1, x, y, z, *pnrs)) + with open(work / "res" / f"rt_is.{frame}", "w") as fh: + fh.write(f"{len(rt_rows)}\n") + for r in rt_rows: + fh.write(f"{r[0]:4d} {r[1]:12.6f} {r[2]:12.6f} " + f"{r[3]:12.6f} {r[4]:5d} {r[5]:5d} " + f"{r[6]:5d} {r[7]:5d}\n") + for ci in range(NCAMS): + with open(work / "img_3dptv" / f"Cam{ci + 1}.{frame}_targets", + "w") as fh: + fh.write(f"{len(per_cam[ci])}\n") + for t in per_cam[ci]: + fh.write(f"{t[0]:4d} {t[1]:10.4f} {t[2]:10.4f} " + f"{t[3]:6d} {t[4]:4d} {t[5]:4d} " + f"{int(round(t[6])):6d} {t[7]:4d}\n") + return rows_pf + + +def run_tracker(work: Path, loser_retry: int, cold_start: int, + app_weight: float = 0.0): + from openptv2.gui.parameter_manager import ParameterManager + from openptv2.gui.ptv import py_start_proc_c + from openptv2.tracker import Tracker, default_naming + + old = os.getcwd() + os.chdir(work) + try: + pm = ParameterManager() + pm.from_yaml(work / "parameters_Run1.yaml") + cpar, spar, vpar, track_par, tpar, cals, epar = py_start_proc_c(pm) + for cam_id, short in enumerate(pm.get_target_filenames()): + spar.set_img_base_name(cam_id, str(Path(short).resolve()) + ".") + Tracker(cpar, vpar, track_par, spar, cals, default_naming, + loser_retry=loser_retry, cold_start_neighbour=cold_start, + app_weight=app_weight).full_forward() + finally: + os.chdir(old) + + +def score(work: Path, rows_pf): + """Fraction of true links recovered + the crossing decision (frame 4). + + rows_pf: {frame: {pid: row}}. A pid absent in frame f-1 but present in + f with a link to its last-seen row counts as recovered (gap link). + """ + n_ok = n_tot = 0 + cross_ok = None + last_row = dict(rows_pf[FIRST]) + for f in range(1, NF): + frame = FIRST + f + lines = (work / "res" / f"ptv_is.{frame}").read_text().splitlines() + n = int(lines[0]) + prev = [int(l.split()[0]) for l in lines[1: n + 1]] + for pid, row_now in rows_pf[frame].items(): + if pid not in last_row: + continue # track head (first appearance), no link expected + n_tot += 1 + ok = row_now < len(prev) and prev[row_now] == last_row[pid] + n_ok += ok + if f == 4: + cross_ok = (cross_ok is not False) and bool(ok) + if not ok: + cross_ok = False + for pid, row_now in rows_pf[frame].items(): + last_row[pid] = row_now + return n_ok / n_tot if n_tot else 1.0, \ + (cross_ok if cross_ok is not None else True) + + +def make_truth(v, eps=0.0, noise=0.0, seed=0, three_way=False, asym=None, + maneuver=None, convoy=None, dropout=False, pulsatile=None): + rng = np.random.default_rng(seed) + if asym is not None: + va, vb = asym + else: + va = vb = v + + def jit(x, y, z): + if noise: + return (x + rng.normal(0, noise), y + rng.normal(0, noise), + z + rng.normal(0, noise)) + return (x, y, z) + + truth = {0: {}, 1: {}} + for f in range(NF): + if maneuver == "kick": + # A crawls in at 0.15, then jumps 1.0 through the crossing + # (the case-2 shape: sudden acceleration AT the crossing). + xa = [X0 - 0.45, X0 - 0.30, X0 - 0.15, X0, + X0 + 1.0, X0 + 2.0, X0 + 3.0, X0 + 4.0, X0 + 5.0, + X0 + 6.0][f] + xb = X0 - 0.5 * (f - 3.5) + elif maneuver == "brake": + # A rushes in at 1.0, then nearly stops after crossing. + xa = [X0 - 3.0, X0 - 2.0, X0 - 1.0, X0, + X0 + 0.15, X0 + 0.30, X0 + 0.45, X0 + 0.60, X0 + 0.75, + X0 + 0.90][f] + xb = X0 - 0.5 * (f - 3.5) + else: + xa = X0 + va * (f - 3.5) + xb = X0 - vb * (f - 3.5) + truth[0][FIRST + f] = jit(xa, Y0, Z0) + truth[1][FIRST + f] = jit(xb, Y0, Z0 + eps) + if three_way: + # C crosses along z through the intersection at the cross frame + truth[2] = {} + for f in range(NF): + truth[2][FIRST + f] = jit(X0, Y0, Z0 - 1.0 * (f - 3.5)) + if convoy: + # second head-on pair, offset in y by dy, same timing + dy = convoy + truth[2], truth[3] = {}, {} + for f in range(NF): + truth[2][FIRST + f] = jit(X0 + 0.5 * (f - 3.5), Y0 + dy, Z0) + truth[3][FIRST + f] = jit(X0 - 0.5 * (f - 3.5), Y0 + dy, Z0) + if dropout: + # third particle, occluded exactly at frame FIRST+3 (gap of 1). + # Kept as the LAST pid so surviving rows never shift. + truth[2] = {} + for f in range(NF): + if f == 3: + continue + truth[2][FIRST + f] = jit(X0 + 3.0 + 0.1 * f, Y0 + 2.0, Z0) + if pulsatile: + # aorta-like plug: x(t) = X0 + A*sin(2*pi*(f-3.5)/T + phase). + # Anti-phase pair crosses WHILE reversing (worst case); in-phase + # pair reverses without crossing (isolates reversal cost). + import math as _math + vmax = pulsatile.get("vmax", 1.5) + T = float(pulsatile.get("period", NF)) + A = vmax * T / (2 * _math.pi) + modes = pulsatile.get("modes", ("anti",)) + truth = {} + pid = 0 + if "anti" in modes: + for ph in (0.0, _math.pi): + truth[pid] = {} + for f in range(NF): + x = X0 + A * _math.sin(2 * _math.pi * (f - 3.5) / T + ph) + truth[pid][FIRST + f] = jit(x, Y0, Z0) + pid += 1 + if "plug" in modes: + for dy in (-1.5, 1.5): + truth[pid] = {} + for f in range(NF): + x = X0 + A * _math.sin(2 * _math.pi * (f - 3.5) / T) + truth[pid][FIRST + f] = jit(x, Y0 + dy, Z0) + pid += 1 + if "solo" in modes: + # one oscillator (vmax may exceed the dv box) + a steady + # neighbour 1.0 mm away in y: diastole swap-bait that tight + # gates must reject and systole must not lose. + truth[pid] = {} + for f in range(NF): + x = X0 + A * _math.sin(2 * _math.pi * (f - 3.5) / T) + truth[pid][FIRST + f] = jit(x, Y0, Z0) + pid += 1 + truth[pid] = {} + for f in range(NF): + truth[pid][FIRST + f] = jit(X0 + 1.0, Y0 + 1.0, Z0) + pid += 1 + return truth + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument("--reps", type=int, default=5) + ap.add_argument("--app", type=float, default=0.0, + help="appearance weight for every run in the sweep") + args = ap.parse_args() + + cpar, cals = load_optics() + # visibility sanity at the crossing point, all cams + for ci, (px, py) in enumerate(project(cpar, cals, X0, Y0, Z0)): + assert 0 <= px < cpar.imx and 0 <= py < cpar.imy, (ci, px, py) + print(f"imx={cpar.imx} imy={cpar.imy} pix={cpar.pix_x},{cpar.pix_y} " + f"crossing point visible in all {NCAMS} cams") + + scenarios = [ + ("S1 head-on v=0.5", {"v": 0.5}), + ("S2 near-miss e=0.15", {"v": 0.5, "eps": 0.15}), + ("S3 near-miss e=0.40", {"v": 0.5, "eps": 0.40}), + ("S4 head-on+noise", {"v": 0.5, "noise": 0.07}), + ("S5 fast v=1.2", {"v": 1.2}), + ("S6 three-way", {"v": 0.5, "three_way": True}), + ("S7 asymmetric", {"v": 0.5, "asym": (0.7, 0.35)}), + ("S8 kick-at-cross", {"v": 0.5, "maneuver": "kick"}), + ("S9 brake-at-cross", {"v": 0.5, "maneuver": "brake"}), + ("S10 convoy dy=0.6", {"v": 0.5, "convoy": 0.6}), + ("S11 kick+noise", {"v": 0.5, "maneuver": "kick", "noise": 0.07}), + ("S12 convoy+noise", {"v": 0.5, "convoy": 0.6, "noise": 0.07}), + ("S14 anti-phase", {"v": 0.5, + "pulsatile": {"vmax": 1.5, "modes": ("anti",)}}), + ("S14b plug-reverse", {"v": 0.5, + "pulsatile": {"vmax": 1.5, + "modes": ("plug",)}}), + ("S15 bright-kick", {"v": 0.5, "maneuver": "kick", "noise": 0.07, + "_sumg": {0: 2000.0, 1: 500.0}, + "_sj": 0.10}), + ] + variants = [("parity(lr0)", 0, 0), ("default(lr1)", 1, 1)] + + print(f"\n{'scenario':22}{'variant':14}{'recall':>9}{'cross-ok':>10}") + for sname, kw in scenarios: + kw = dict(kw) + sumg_map = kw.pop("_sumg", None) + sumg_jitter = kw.pop("_sj", 0.0) + noisy = kw.get("noise", 0.0) > 0 or sumg_jitter > 0 + reps = args.reps if (noisy or kw.get("three_way") + or kw.get("convoy") or kw.get("pulsatile") + or sumg_map) else 1 + for vname, lr, cs in variants: + recs, crs = [], [] + for rep in range(reps): + work = REPO / "scratch" / "_synth_x" + setup_work(work) + truth = make_truth(seed=1000 + rep, **kw) + if sumg_map: + sfn = lambda pid, frame, _m=sumg_map: _m.get(pid, 1000.0) # noqa: E731 + else: + sfn = None + rows_pf = write_scene(work, truth, cpar, cals, + sumg_fn=sfn, sumg_jitter=sumg_jitter, + seed=2000 + rep) + run_tracker(work, lr, cs, app_weight=args.app) + r, c = score(work, rows_pf) + recs.append(r) + crs.append(c) + print(f"{sname:22}{vname:14}{np.mean(recs):>8.1%} " + f"{sum(crs)}/{len(crs)}") + + +if __name__ == "__main__": + main() diff --git a/scripts/synth_dacc_rescue.py b/scripts/synth_dacc_rescue.py new file mode 100644 index 00000000..7bc7275a --- /dev/null +++ b/scripts/synth_dacc_rescue.py @@ -0,0 +1,69 @@ +"""Does widening dacc rescue the noisy-maneuver crossing (S11)? + +Distinguishes gate-binding (fixable by parameter) from contest loss +(structural). Reuses synth_crossing builders. +""" + +import os +import sys +from pathlib import Path + +sys.path.insert(0, str(Path(__file__).resolve().parent)) + +import synth_crossing as SC # noqa: E402 +from synth_crossing import ( # noqa: E402 + REPO, + load_optics, + make_truth, + score, + setup_work, + write_scene, +) +from synth_crossing import ( + run_tracker as _run, +) + + +def run_tracker_dacc(work: Path, lr: int, cs: int, dacc: float): + from openptv2.gui.parameter_manager import ParameterManager + from openptv2.gui.ptv import py_start_proc_c + from openptv2.tracker import Tracker, default_naming + + old = os.getcwd() + os.chdir(work) + try: + pm = ParameterManager() + pm.from_yaml(work / "parameters_Run1.yaml") + cpar, spar, vpar, track_par, tpar, cals, epar = py_start_proc_c(pm) + for cam_id, short in enumerate(pm.get_target_filenames()): + spar.set_img_base_name(cam_id, str(Path(short).resolve()) + ".") + track_par.dacc = dacc + Tracker(cpar, vpar, track_par, spar, cals, default_naming, + loser_retry=lr, cold_start_neighbour=cs).full_forward() + finally: + os.chdir(old) + + +def main(): + cpar, cals = load_optics() + print(f"{'dacc':>6}{'lr':>4}{'recall':>9}{'cross-ok':>10}", flush=True) + for dacc in (1.9, 2.5, 3.5): + for lr, cs in ((0, 0), (1, 1)): + recs, crs = [], [] + for rep in range(8): + work = REPO / "scratch" / "_synth_d" + setup_work(work) + truth = make_truth(v=0.5, maneuver="kick", noise=0.07, + seed=1000 + rep) + rows_pf = write_scene(work, truth, cpar, cals) + run_tracker_dacc(work, lr, cs, dacc) + r, c = score(work, rows_pf) + recs.append(r) + crs.append(c) + import numpy as np + print(f"{dacc:>6}{lr:>4}{np.mean(recs):>8.1%} " + f"{sum(crs)}/{len(crs)}", flush=True) + + +if __name__ == "__main__": + main() diff --git a/scripts/trace_divergence.py b/scripts/trace_divergence.py new file mode 100644 index 00000000..d67837b7 --- /dev/null +++ b/scripts/trace_divergence.py @@ -0,0 +1,77 @@ +"""Find where test history first diverges from ref along hard-link sources. + +Many union-miss links are downstream victims: at decision time the test +prediction was already 20-36 mm off because an UPSTREAM link diverged. +This script walks both chains back from each hard link's source and names +the first divergence frame + the competing choice -- the actual decision +to adjudicate by manual vote. +""" + +from collections import Counter +from pathlib import Path + +DS = Path(r"C:\Users\alex\Downloads\HiDImaging\wp1_10_images") +REF = DS / "res_ground_truth_backup" +TST = Path("scratch/_ens_lr1/res") +FIRST, LAST = 100001, 100010 + + +def load(d): + prev, nxt = {}, {} + for f in range(FIRST, LAST + 1): + l = (d / f"ptv_is.{f}").read_text().splitlines() + n = int(l[0]) + p, x = [], [] + for line in l[1: n + 1]: + s = line.split() + p.append(int(s[0])) + x.append(int(s[1])) + prev[f], nxt[f] = p, x + return prev, nxt + + +def chain_back(prev, f, i): + out = [(f, i)] + while out[-1][0] > FIRST: + ff, ii = out[-1] + p = prev[ff][ii] if ii < len(prev[ff]) else -1 + if p < 0: + break + out.append((ff - 1, p)) + return out + + +def main(): + rp, rn = load(REF) + tp, tn = load(TST) + import csv + hard = list(csv.DictReader(open("scratch/hard_links/hard_links.csv"))) + div_at = Counter() + print(f"{'frame':>8}{'row':>6} divergence (frame, ref-node -> test-node)") + shown = 0 + for r in hard: + f, i, p = int(r["frame"]), int(r["row"]), int(r["ref_prev"]) + rc = chain_back(rp, f - 1, p) # rc[0] == tc[0] == (f-1, p) + tc = chain_back(tp, f - 1, p) + div = None + for k in range(1, max(len(rc), len(tc))): + rk = rc[k] if k < len(rc) else None + tk = tc[k] if k < len(tc) else None + if rk != tk: + div = (rc[k - 1], rk, tk) + break + if div is None: + print(f"{f:>8}{i:>6} chains identical " + f"(len {len(rc)}); miss is LOCAL to this link") + else: + prev_node, rnode, tnode = div + print(f"{f:>8}{i:>6} after {prev_node}: ref->{rnode} " + f"test->{tnode}") + div_at[(prev_node, rnode, tnode)] += 1 + print(f"\n{len(div_at)} unique upstream divergences:") + for k, v in div_at.most_common(): + print(f" {v}x after {k[0]}: ref->{k[1]} test->{k[2]}") + + +if __name__ == "__main__": + main() diff --git a/scripts/track_triage_typesafe.py b/scripts/track_triage_typesafe.py new file mode 100644 index 00000000..76fd7f20 --- /dev/null +++ b/scripts/track_triage_typesafe.py @@ -0,0 +1,96 @@ +"""Prototype: post-tracking trajectory triage with TypeSafe (Jev) Noul judgments. + +Reads trajectories from a run.zarr, summarises each one in kinematic terms, and +asks Jev "does this look like a spurious link?". Tracking itself is untouched. + + uv run --with typesafe-sdk python scripts/track_triage_typesafe.py RUN.zarr --dry-run + TYPESAFE_API_KEY=... uv run --with typesafe-sdk python scripts/track_triage_typesafe.py RUN.zarr + +--dry-run prints the state/question for a few trajectories, no API call. +Output: per-trajectory p_spurious plus a plain-code baseline for comparison. +""" + +import argparse +import json +from concurrent.futures import ThreadPoolExecutor + +import numpy as np + +from openptv2.storage.zarr_store import read_zarr_trajectories + +QUESTION = ( + "Is this trajectory likely a spurious link (an ID swap between two particles, " + "or a ghost) rather than one real particle moving through a smooth flow?" +) +CRITERIA = ( + "Real tracks change speed and direction gradually between frames. Suspect: a " + "step much larger than its neighbours, a sharp reversal or turn, or large " + "acceleration relative to speed. Very short tracks give weak evidence." +) + + +def summarise(traj) -> dict: + pos = np.asarray(traj.pos()) * 1000.0 # m -> mm + step = np.diff(pos, axis=0) + speed = np.linalg.norm(step, axis=1) + turn = [] + for a, b in zip(step[:-1], step[1:]): + d = np.linalg.norm(a) * np.linalg.norm(b) + turn.append(float(np.degrees(np.arccos(np.clip(a @ b / d, -1, 1)))) if d else 0.0) + acc = np.linalg.norm(np.diff(step, axis=0), axis=1) + return { + "n_points": len(pos), + "step_mm_per_frame": [round(float(s), 3) for s in speed], + "turn_angle_deg": [round(t, 1) for t in turn], + "accel_mm_per_frame2": [round(float(a), 3) for a in acc], + } + + +def baseline(s: dict) -> bool: + """Plain-code rule Jev has to beat: sharp turn or a step-size jump >2x.""" + st = s["step_mm_per_frame"] + jump = max((max(a, b) / max(min(a, b), 1e-9) for a, b in zip(st, st[1:])), default=1) + return max(s["turn_angle_deg"], default=0) > 90 or jump > 2 + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument("zarr") + ap.add_argument("--limit", type=int, default=200) + ap.add_argument("--dry-run", action="store_true") + ap.add_argument("--out", default="triage.jsonl") + args = ap.parse_args() + + trajs = [t for t in read_zarr_trajectories(args.zarr) if len(t.time()) >= 3] + trajs = trajs[: args.limit] + states = [summarise(t) for t in trajs] + print(f"{len(trajs)} trajectories with >=3 points; baseline flags " + f"{sum(baseline(s) for s in states)}") + + if args.dry_run: + for s in states[:3]: + print(json.dumps({"state": s, "question": QUESTION}, indent=1)) + return + + from typesafe_sdk import Noul, TypeSafeClient + + q = {"spurious": Noul(instructions=QUESTION, criteria=CRITERIA)} + with TypeSafeClient() as client: + def ask(s): + return client.system_one(state={"trajectory": s}, questions=q) + with ThreadPoolExecutor(8) as ex: + resp = list(ex.map(ask, states)) + + with open(args.out, "w") as f: + for t, s, r in zip(trajs, states, resp): + p = float(r.nouls["spurious"].noul) + f.write(json.dumps({"trajid": int(t.trajid()), "p_spurious": p, + "baseline": baseline(s), **s}) + "\n") + p = np.array([json.loads(l)["p_spurious"] for l in open(args.out)]) + b = np.array([baseline(s) for s in states]) + print(f"p>0.5: {(p > .5).sum()} agree with baseline: {((p > .5) == b).mean():.0%}") + print(f"wrote {args.out}; inspect disagreements by eye") + + +if __name__ == "__main__": + main() diff --git a/scripts/verify_adaptive_wp1.py b/scripts/verify_adaptive_wp1.py new file mode 100644 index 00000000..badff94c --- /dev/null +++ b/scripts/verify_adaptive_wp1.py @@ -0,0 +1,44 @@ +"""wp1 validation for the adaptive scheduler (parity base, app=0). + +Restores pristine inputs, runs adaptive forward + stock backward, scores +vs the 3dptv reference. Must not regress vs fixed-gate 98.16%. + +Run from repo root (detached): + uv run python -u scripts/verify_adaptive_wp1.py +""" + +import shutil +import sys +from pathlib import Path + +sys.path.insert(0, str(Path(__file__).resolve().parent)) + +from verify_same_trajectories import ( # noqa: E402 + DS, + REF, + compare_links, + compare_tracks, +) + +WORK = (Path(__file__).resolve().parent.parent / "scratch" / "_wp1_ab" + ).resolve() +FIRST, LAST = 100001, 100010 + + +def main(): + from phase_scheduler import run_adaptive + + for f in range(FIRST, LAST + 1): + shutil.copyfile(REF / f"rt_is.{f}", WORK / "res" / f"rt_is.{f}") + for cam in range(1, 5): + shutil.copyfile( + DS / "img_3dptv" / f"Cam{cam}.{f}_targets", + WORK / "img_3dptv" / f"Cam{cam}.{f}_targets") + print("inputs restored; running adaptive forward", flush=True) + run_adaptive(WORK, 0, 0, first=FIRST, last=LAST, verbose=True) + compare_links(WORK, 100003, 100010, "adaptive-fwd") + compare_tracks(WORK) + + +if __name__ == "__main__": + main() diff --git a/scripts/verify_ppar_wp1.py b/scripts/verify_ppar_wp1.py new file mode 100644 index 00000000..f8866a5a --- /dev/null +++ b/scripts/verify_ppar_wp1.py @@ -0,0 +1,56 @@ +"""wp1 validation for the adaptive scheduler (parity base, app=0). + +Restores pristine inputs, runs adaptive forward + stock backward, scores +vs the 3dptv reference. Must not regress vs fixed-gate 98.16%. + +Run from repo root (detached): + uv run python -u scripts/verify_adaptive_wp1.py +""" + +import shutil +import sys +from pathlib import Path + +sys.path.insert(0, str(Path(__file__).resolve().parent)) + +from verify_same_trajectories import ( # noqa: E402 + DS, + REF, + compare_links, + compare_tracks, +) + +WORK = (Path(__file__).resolve().parent.parent / "scratch" / "_wp1_ab" + ).resolve() +FIRST, LAST = 100001, 100010 + + +def main(): + import argparse + ap = argparse.ArgumentParser() + ap.add_argument("--lr", type=int, default=0) + ap.add_argument("--hi", type=float, default=1.5) + ap.add_argument("--app", type=float, default=0.0) + ap.add_argument("--mode", type=str, default="vel") + ap.add_argument("--yaml", type=str, default="parameters_Run_dacc19.yaml") + args = ap.parse_args() + from phase_scheduler import run_adaptive_pp + + for f in range(FIRST, LAST + 1): + shutil.copyfile(REF / f"rt_is.{f}", WORK / "res" / f"rt_is.{f}") + for cam in range(1, 5): + shutil.copyfile( + DS / "img_3dptv" / f"Cam{cam}.{f}_targets", + WORK / "img_3dptv" / f"Cam{cam}.{f}_targets") + print(f"inputs restored; running per-particle adaptive forward " + f"(lr={args.lr} hi={args.hi} app={args.app} mode={args.mode} " + f"yaml={args.yaml})", flush=True) + run_adaptive_pp(WORK, args.lr, 0, hi=args.hi, mode=args.mode, + app=args.app, first=FIRST, last=LAST, verbose=True, + yaml=args.yaml) + compare_links(WORK, 100003, 100010, "adaptive-fwd") + compare_tracks(WORK) + + +if __name__ == "__main__": + main() diff --git a/scripts/verify_same_trajectories.py b/scripts/verify_same_trajectories.py new file mode 100644 index 00000000..cdcf1004 --- /dev/null +++ b/scripts/verify_same_trajectories.py @@ -0,0 +1,214 @@ +"""Full verification on the wp1 dataset folder: same parameters => same trajectories? + +Runs openptv2 tracking IN the dataset work folder +(C:\\Users\\alex\\Downloads\\HiDImaging\\wp1_10_images) with dacc=1.9 to match +the 3dptv reference, then compares against res_ground_truth_backup at two +levels: per-link exact reproduction and full trajectory-chain reproduction. + +Safety: inputs (rt_is.*, Cam*_targets) are restored from pristine sources +before the run. Outputs (res/ptv_is.*, res/added.*, res/rt_is.*) are the +comparison subject. Target-file tnr columns dirtied by the run are restored +afterwards by scripts/restore_dataset.py. + +Run from repo root: + uv run python -u scripts/verify_same_trajectories.py [--backward] +""" + +from __future__ import annotations + +import argparse +import os +import shutil +import sys +from pathlib import Path + +REPO = Path(__file__).resolve().parent.parent +DS = Path(r"C:\Users\alex\Downloads\HiDImaging\wp1_10_images") +REF = DS / "res_ground_truth_backup" +FIRST, LAST = 100001, 100010 +SCORE_FIRST, SCORE_LAST = 100003, 100010 + + +def restore_inputs(work: Path) -> None: + res = work / "res" + res.mkdir(exist_ok=True) + for f in range(FIRST, LAST + 1): + shutil.copyfile(REF / f"rt_is.{f}", res / f"rt_is.{f}") + for cam in range(1, 5): + src = DS / "img_3dptv" / f"Cam{cam}.{f}_targets" + dst = work / "img_3dptv" / f"Cam{cam}.{f}_targets" + if src.resolve() != dst.resolve(): + shutil.copyfile(src, dst) + + +def check_consistency(work: Path) -> int: + """rt_is p-columns must be the inverse of target-file tnr columns. + + Returns mismatches on CLEAN frames only; frame 100001 is a known + corrupt frame (rt p-columns disagree with targets) and is reported + separately without aborting. + """ + bad_clean = bad_100001 = 0 + for f in range(FIRST, LAST + 1): + rt = (REF / f"rt_is.{f}").read_text().strip().splitlines() + n = int(rt[0]) + tnr = {} + for cam in range(1, 5): + tl = (work / "img_3dptv" / f"Cam{cam}.{f}_targets" + ).read_text().strip().splitlines() + tnr[cam] = {int(x.split()[0]): int(x.split()[7]) for x in tl[1:]} + bad = 0 + for line in rt[1: n + 1]: + p = line.split() + row = int(p[0]) + for cam in range(1, 5): + j = int(p[3 + cam]) + t = tnr[cam].get(j, -1) + if j >= 0 and t not in (row - 1, row): + bad += 1 + if f == 100001: + bad_100001 = bad + else: + bad_clean += bad + print(f"consistency: clean-frame mismatches={bad_clean}, " + f"frame-100001 mismatches={bad_100001} (known corrupt)", flush=True) + return bad_clean + + +def run_tracking(work: Path, do_backward: bool, params_file: str, + dacc: float | None, app: float = 0.0) -> None: + from openptv2.gui.parameter_manager import ParameterManager + from openptv2.gui.ptv import py_start_proc_c + from openptv2.tracker import Tracker, default_naming + + old = os.getcwd() + os.chdir(work) + try: + pm = ParameterManager() + pm.from_yaml(work / params_file) + cpar, spar, vpar, track_par, tpar, cals, epar = py_start_proc_c(pm) + for cam_id, short in enumerate(pm.get_target_filenames()): + spar.set_img_base_name(cam_id, str(Path(short).resolve()) + ".") + if dacc is not None: + track_par.dacc = dacc + print(f"effective params from {params_file}" + f"{' + dacc override' if dacc is not None else ''}: " + f"dv={track_par.dvxmax} dacc={track_par.dacc} " + f"dangle={track_par.dangle} add={track_par.add}", flush=True) + tr = Tracker(cpar, vpar, track_par, spar, cals, default_naming, + loser_retry=0, cold_start_neighbour=0, app_weight=app) + tr.full_forward() + if do_backward: + tr.full_backward() + finally: + os.chdir(old) + + +def read_pn(res: Path, frame: int): + """prev/next columns of ptv_is..""" + lines = (res / f"ptv_is.{frame}").read_text().strip().splitlines() + n = int(lines[0]) + prev, nxt = [], [] + for line in lines[1: n + 1]: + p = line.split() + prev.append(int(p[0])) + nxt.append(int(p[1])) + return prev, nxt + + +def compare_links(work: Path, first: int, last: int, tag: str) -> tuple: + ref_total = test_total = match = mis = 0 + for f in range(first, last + 1): + r, _ = read_pn(REF, f) + t, _ = read_pn(work / "res", f) + ref_total += sum(1 for p in r if p >= 0) + test_total += sum(1 for p in t if p >= 0) + for i in range(min(len(r), len(t))): + if r[i] >= 0: + if t[i] == r[i]: + match += 1 + else: + mis += 1 + print(f"[{tag}] links frames {first}..{last}: ref={ref_total} " + f"test={test_total} exact={match} miss={mis} " + f"recall={match / ref_total:.2%}", flush=True) + return ref_total, test_total, match + + +def compare_tracks(work: Path) -> None: + """Chain-level comparison over rows shared with the reference. + + Added particles append rows, so reference rows keep their indices in the + test output; chains are comparable as (frame, row) node sequences. + """ + # next pointers per frame for both runs + ref_nxt, tst_nxt, ref_prev, tst_prev, ref_n = {}, {}, {}, {}, {} + for f in range(FIRST, LAST + 1): + rp, rn = read_pn(REF, f) + tp, tn = read_pn(work / "res", f) + ref_prev[f], ref_nxt[f] = rp, rn + tst_prev[f], tst_nxt[f] = tp, tn + ref_n[f] = len(rp) + + def chains(prev, nxt, nrows, first, last): + out = [] + for f in range(first, last + 1): + for i in range(min(nrows[f], len(prev[f]))): + if prev[f][i] < 0: + ch = [(f, i)] + cf, ci = f, i + while cf <= last and ci < len(nxt[cf]) and nxt[cf][ci] >= 0: + ci = nxt[cf][ci] + cf += 1 + ch.append((cf, ci)) + out.append(tuple(ch)) + return out + + ref_chains = chains(ref_prev, ref_nxt, ref_n, FIRST, LAST) + tst_chains = chains(tst_prev, tst_nxt, ref_n, FIRST, LAST) + tst_set = set(tst_chains) + exact = sum(1 for c in ref_chains if c in tst_set) + print(f"[tracks] reference chains: {len(ref_chains)}, test chains " + f"(on shared rows): {len(tst_chains)}, exactly reproduced " + f"end-to-end: {exact} ({exact / len(ref_chains):.2%})", flush=True) + + import numpy as np + rl = np.array([len(c) for c in ref_chains]) + tl = np.array([len(c) for c in tst_chains]) + print(f"[tracks] ref length: mean {rl.mean():.2f} median " + f"{np.median(rl):.0f} max {rl.max()}; test length: mean " + f"{tl.mean():.2f} median {np.median(tl):.0f} max {tl.max()}", + flush=True) + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument("--work", type=Path, default=DS) + ap.add_argument("--backward", action="store_true") + ap.add_argument("--params", default="parameters_Run1.yaml", + help="yaml file in work dir (use a parity yaml to test " + "config-level equality with track.par)") + ap.add_argument("--dacc", type=float, default=None, + help="in-memory dacc override; omit to use the yaml value") + ap.add_argument("--app", type=float, default=0.0, + help="appearance weight (requires rebuilt kernels)") + ap.add_argument("--skip-run", action="store_true", + help="only compare existing outputs") + args = ap.parse_args() + work = args.work.resolve() + + if not args.skip_run: + restore_inputs(work) + print("inputs restored from pristine sources", flush=True) + bad = check_consistency(work) + if bad: + sys.exit(f"ABORT: {bad} inconsistent correspondences") + run_tracking(work, args.backward, args.params, args.dacc, args.app) + + compare_links(work, SCORE_FIRST, SCORE_LAST, "clean") + compare_links(work, FIRST + 1, LAST, "all") + compare_tracks(work) + + +if __name__ == "__main__": + main() diff --git a/scripts/verify_two_phase_wp1.py b/scripts/verify_two_phase_wp1.py new file mode 100644 index 00000000..ff866514 --- /dev/null +++ b/scripts/verify_two_phase_wp1.py @@ -0,0 +1,130 @@ +"""wp1 re-verification for the productized two-phase tracker. + +Loads pristine rt_is.* (restored first), builds 2D leaves by projection +with the work calibrations, runs TwoPhaseTracker (+velocity, re-projected +costs, gaps) as a pure function (no file writes), and scores per-link exact +reproduction + chain census vs the 3dptv reference. + +Run from repo root: + uv run python -u scripts/verify_two_phase_wp1.py +""" + +from __future__ import annotations + +import shutil +from pathlib import Path + +import numpy as np + +REPO = Path(__file__).resolve().parent.parent +DS = Path(r"C:\Users\alex\Downloads\HiDImaging\wp1_10_images") +REF = DS / "res_ground_truth_backup" +WORK = (REPO / "scratch" / "_wp1_ab").resolve() +FIRST, LAST = 100001, 100010 +SCORE_FIRST, SCORE_LAST = 100003, 100010 + +NCAMS = 4 + + +def restore_inputs() -> None: + for f in range(FIRST, LAST + 1): + shutil.copyfile(REF / f"rt_is.{f}", WORK / "res" / f"rt_is.{f}") + for cam in range(1, NCAMS + 1): + shutil.copyfile( + DS / "img_3dptv" / f"Cam{cam}.{f}_targets", + WORK / "img_3dptv" / f"Cam{cam}.{f}_targets", + ) + + +def main(): + from openptv2.algorithms.calibration import Calibration + from openptv2.algorithms.imgcoord import img_coord_batch + from openptv2.algorithms.parameters import ControlPar + from openptv2.plugins.two_phase_tracking import ( + TwoPhaseTracker, + TwoPhaseTrackerConfig, + ) + + restore_inputs() + + cpar = ControlPar.from_file(str(WORK / "parameters" / "ptv.par")) + cals = [Calibration.from_file( + str(WORK / "cal" / f"cam_{c}.tif.ori"), + str(WORK / "cal" / f"cam_{c}.tif.addpar")) + for c in range(1, NCAMS + 1)] + + frames = list(range(FIRST, LAST + 1)) + positions, leaves = [], [] + for f in frames: + lines = (WORK / "res" / f"rt_is.{f}").read_text().splitlines() + n = int(lines[0]) + P = np.array([[float(v) for v in l.split()[1:4]] + for l in lines[1: n + 1]]) + positions.append(P) + xy = np.full((n, NCAMS * 2), np.nan) + for i, (x, y, z) in enumerate(P): + for ci in range(NCAMS): + m = img_coord_batch(np.array([[x, y, z]]), cals[ci], + cpar.mm)[0] + xy[i, 2 * ci] = m[0] / cpar.pix_x + cpar.imx / 2 + xy[i, 2 * ci + 1] = cpar.imy / 2 - m[1] / cpar.pix_y + leaves.append(np.nan_to_num(xy)) + + def project_fn(pred): + pred = np.asarray(pred, dtype=np.float64) + out = np.full((len(pred), NCAMS * 2), np.nan) + for i, (x, y, z) in enumerate(pred): + for ci in range(NCAMS): + m = img_coord_batch(np.array([[x, y, z]]), cals[ci], + cpar.mm)[0] + out[i, 2 * ci] = m[0] / cpar.pix_x + cpar.imx / 2 + out[i, 2 * ci + 1] = cpar.imy / 2 - m[1] / cpar.pix_y + return np.nan_to_num(out) + + for v_max in (2.0, 2.5, 3.3): + for max_gap in (1, 2): + cfg = TwoPhaseTrackerConfig( + v_max=v_max, leaf_weight=1.0, use_velocity=True, + cost_mode="projected", max_gap=max_gap) + links = TwoPhaseTracker(cfg).track_frames( + positions, leaves, project_fn=project_fn) + # Link-based stats with correct frames (gap links span >1). + # ref linkage: consecutive frames only. + ref_prev = {} + for k, f in enumerate(frames): + lines = (REF / f"ptv_is.{f}").read_text().splitlines() + n = int(lines[0]) + ref_prev[k] = [int(l.split()[0]) for l in lines[1: n + 1]] + ref_total = sum( + sum(1 for p in ref_prev[k] if p >= 0) + for k, f in enumerate(frames) + if SCORE_FIRST <= f <= SCORE_LAST) + exact = consec_only = gap_links = 0 + only_steps = [] + for t0, r0, t1, r1 in links: + f1 = frames[t1] + if not (SCORE_FIRST <= f1 <= SCORE_LAST): + continue + if t1 - t0 > 1: + gap_links += 1 + continue + consec_only += 1 + rp = ref_prev[t1] + if r1 < len(rp) and rp[r1] >= 0 and rp[r1] == r0: + exact += 1 + else: + only_steps.append(float(np.linalg.norm( + positions[t1][r1] - positions[t0][r0]))) + only = np.array(only_steps) + extra = (f" only-step p50={np.median(only):.3f} " + f"p90={np.percentile(only, 90):.3f} " + f"max={only.max():.3f}" if len(only) else "") + print(f"two-phase+vel v_max={v_max} gap={max_gap}: " + f"ref={ref_total} exact={exact} " + f"recall={exact / ref_total:.2%} links={len(links)} " + f"consec={consec_only} gaplinks={gap_links}{extra}", + flush=True) + + +if __name__ == "__main__": + main() diff --git a/scripts/vote_links.py b/scripts/vote_links.py new file mode 100644 index 00000000..abb7f8ea --- /dev/null +++ b/scripts/vote_links.py @@ -0,0 +1,90 @@ +"""Per-link ensemble voting with agreement flags (prototype). + +Takes several engine outputs (ptv_is.*) on identical inputs plus the +reference, and writes agreement sidecars agree. next to them: +each row -> flag, top choice, vote count, per-engine prev values. + +Flags: 4 = unanimous, 3 = majority, 2 = split (2-2 or 2-1-1), +1 = all disagree, 0 = fewer than 2 engines have this row. + +Also reports the no-engine-right set (union misses) with agreement +patterns for manual adjudication. + +Run from repo root: + uv run python -u scripts/vote_links.py --ref --eng ... + --out --first F --last L +""" + +from __future__ import annotations + +import argparse +from collections import Counter +from pathlib import Path + + +def read_prev(d: Path, f: int): + lines = (d / f"ptv_is.{f}").read_text().splitlines() + n = int(lines[0]) + return [int(l.split()[0]) for l in lines[1: n + 1]] + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument("--ref", type=Path, required=True) + ap.add_argument("--eng", action="append", required=True, + help="NAME=DIR, repeatable") + ap.add_argument("--out", type=Path, required=True) + ap.add_argument("--first", type=int, required=True) + ap.add_argument("--last", type=int, required=True) + args = ap.parse_args() + + engines = [] + for spec in args.eng: + name, d = spec.split("=", 1) + engines.append((name, Path(d))) + names = [n for n, _ in engines] + out = args.out + out.mkdir(parents=True, exist_ok=True) + + P = {name: {f: read_prev(d, f) for f in range(args.first, args.last + 1)} + for name, d in engines} + R = {f: read_prev(args.ref, f) for f in range(args.first, args.last + 1)} + + patterns: Counter = Counter() + hard = [] # (frame, row, ref_prev, votes) + for f in range(args.first, args.last + 1): + lines = [f"{len(R[f])}"] + n = min(len(R[f]), *(len(P[x][f]) for x in names)) + for i in range(n): + votes = [P[x][f][i] for x in names] + cnt = Counter(votes) + top, c = cnt.most_common(1)[0] + if len(cnt) == 1: + flag = 4 + elif c >= 3: + flag = 3 + elif c == 2: + flag = 2 + else: + flag = 1 + lines.append(f"{i:5d} {flag} {top:5d} {c} " + + " ".join(f"{v:5d}" for v in votes)) + if R[f][i] >= 0: + if top == R[f][i] and flag >= 3: + patterns["majority-right"] += 1 + if not any(v == R[f][i] for v in votes): + hard.append((f, i, R[f][i], votes)) + patterns[f"hard:flag{flag}"] += 1 + (out / f"agree.{f}").write_text("\n".join(lines) + "\n") + + print(f"engines: {names}", flush=True) + print(f"union misses (no engine right): {len(hard)}", flush=True) + print(f"patterns: {dict(patterns)}", flush=True) + for f, i, r, v in hard: + print(f" frame {f} row {i:4d} ref_prev={r:4d} " + + " ".join(f"{n}={x:4d}" for n, x in zip(names, v)), + flush=True) + + +if __name__ == "__main__": + main() diff --git a/setup.py b/setup.py index d2e460e4..729f0f7a 100644 --- a/setup.py +++ b/setup.py @@ -58,7 +58,6 @@ "track_kernels_position", # 3D ray tracing + position reconstruction "track_kernels_track3d", # stereo-3D tracking loop "track_kernels_corr", # forward/backward tracking loops + CAS - "track_kernels_tracking", # compatibility shim (re-exports above) "track_kernels_batch", # batch processing + detection ] diff --git a/src/openptv2/algorithms/segmentation.py b/src/openptv2/algorithms/segmentation.py index d5bcdded..96975551 100644 --- a/src/openptv2/algorithms/segmentation.py +++ b/src/openptv2/algorithms/segmentation.py @@ -561,6 +561,15 @@ def _detect_single_worker(task: tuple) -> dict: max_targets, ) + if n_found > 1: + order = np.argsort(oy[:n_found], kind="stable") + ox[:n_found] = ox[:n_found][order] + oy[:n_found] = oy[:n_found][order] + on[:n_found] = on[:n_found][order] + onx[:n_found] = onx[:n_found][order] + ony[:n_found] = ony[:n_found][order] + osumg[:n_found] = osumg[:n_found][order] + if write_path is not None: p = Path(write_path) p.parent.mkdir(parents=True, exist_ok=True) diff --git a/src/openptv2/algorithms/track.py b/src/openptv2/algorithms/track.py index fb9ea673..1d6cc68e 100644 --- a/src/openptv2/algorithms/track.py +++ b/src/openptv2/algorithms/track.py @@ -1076,6 +1076,34 @@ def trackcorr_c_loop(run_info, step, num_threads=None): nc = fb.num_cams orig_parts = fb.buf[1].num_parts + # Per-step candidate appearance: mean grey sum over each frame-2 + # particle's claimed targets (target->particle inversion). -1 where + # a particle claims no targets. Feeds the app_weight cost term. + maxparts2 = fb.buf[2].path_x.shape[0] + cand_app_2 = np.full(maxparts2, -1.0, dtype=np.float64) + _sums = np.zeros(maxparts2, dtype=np.float64) + _cnts = np.zeros(maxparts2, dtype=np.int32) + for _cam in range(nc): + _nt = int(fb.buf[2].num_targets[_cam]) + _tnr = np.asarray(fb.buf[2].targ_tnr[_cam, :_nt]) + _sg = np.asarray(fb.buf[2].targ_sumg[_cam, :_nt]) + _ok = (_tnr >= 0) & (_tnr < maxparts2) + np.add.at(_sums, _tnr[_ok], _sg[_ok]) + np.add.at(_cnts, _tnr[_ok], 1) + _has = _cnts > 0 + cand_app_2[_has] = _sums[_has] / _cnts[_has] + + # Per-particle gate freedom set by an external scheduler (phase-aware + # tracking); None/absent -> all ones = configured behaviour exactly. + _gs = getattr(run_info, "gate_scale", None) + _maxp1 = fb.buf[1].path_x.shape[0] + if _gs is None: + gate_scale = np.ones(_maxp1, dtype=np.float64) + else: + gate_scale = np.asarray(_gs, dtype=np.float64) + if gate_scale.shape[0] < _maxp1: + gate_scale = np.ones(_maxp1, dtype=np.float64) + # Synchronize path data from AoS to SoA fb.buf[0]._sync_path_to_soa() fb.buf[1]._sync_path_to_soa() @@ -1089,6 +1117,22 @@ def trackcorr_c_loop(run_info, step, num_threads=None): nt2 = np.array(fb.buf[2].num_targets[:nc], dtype=np.int32) nt3 = np.array(fb.buf[3].num_targets[:nc], dtype=np.int32) + # Check if candidate search grid is requested or required. If targets in + # frame 2 or 3 are not monotonically non-decreasing in y, auto-promote + # to grid search to prevent the legacy binary-search from skipping targets. + use_grid = int(getattr(run_info, "use_grid", 0)) + if not use_grid: + for buf_idx in (2, 3): + for c in range(nc): + _nt = int(fb.buf[buf_idx].num_targets[c]) + if _nt > 1: + _ty = np.asarray(fb.buf[buf_idx].targ_y[c, :_nt]) + if not (np.diff(_ty) >= -1e-7).all(): + use_grid = 1 + break + if use_grid: + break + count1, num_added = _trackcorr_loop_fast( orig_parts, fb.buf[0].path_x, @@ -1103,6 +1147,7 @@ def trackcorr_c_loop(run_info, step, num_threads=None): fb.buf[1].targ_x, fb.buf[1].targ_y, fb.buf[1].targ_tnr, + fb.buf[1].targ_sumg, fb.buf[2].path_x, fb.buf[2].path_prev, fb.buf[2].path_next, @@ -1116,6 +1161,8 @@ def trackcorr_c_loop(run_info, step, num_threads=None): fb.buf[2].targ_x, fb.buf[2].targ_y, fb.buf[2].targ_tnr, + fb.buf[2].targ_sumg, + cand_app_2, nt2, np2, fb.buf[3].path_x, @@ -1166,6 +1213,12 @@ def trackcorr_c_loop(run_info, step, num_threads=None): cpar.pix_x, cpar.pix_y, run_info.flatten_tol, + 1, # num_threads + int(getattr(run_info, "loser_retry", 1)), + int(getattr(run_info, "cold_start_neighbour", 1)), + float(getattr(run_info, "app_weight", 0.0)), + gate_scale, + use_grid, ) fb.buf[2].num_parts = int(np2[0]) diff --git a/src/openptv2/algorithms/track_kernels.py b/src/openptv2/algorithms/track_kernels.py index 7862e433..ee29c7ec 100644 --- a/src/openptv2/algorithms/track_kernels.py +++ b/src/openptv2/algorithms/track_kernels.py @@ -105,6 +105,10 @@ def pack_mmlut(cal): init_mmlut_data_nlay_fast, targ_rec_fast, ) +from .track_kernels_corr import ( # noqa: E402, F401 + trackback_loop_fast, + trackcorr_loop_fast, +) from .track_kernels_geom import ( # noqa: E402, F401 point_to_pixel_fast, searchquader_fast, @@ -112,15 +116,9 @@ def pack_mmlut(cal): from .track_kernels_search import ( # noqa: E402, F401 candsearch_in_pix_fast, candsearch_in_pix_rest_fast, - sort_candidates_by_freq_fast, sorted_candidates_fast, ) -from .track_kernels_tracking import ( # noqa: E402, F401 +from .track_kernels_track3d import ( # noqa: E402, F401 track3d_loop_fast, track4be_loop_fast, - trackback_loop_fast, - trackcorr_loop_fast, -) -from .track_kernels_transform import ( # noqa: E402, F401 - point_position_fast, ) diff --git a/src/openptv2/algorithms/track_kernels_batch.py b/src/openptv2/algorithms/track_kernels_batch.py index 899910f8..7ebec371 100644 --- a/src/openptv2/algorithms/track_kernels_batch.py +++ b/src/openptv2/algorithms/track_kernels_batch.py @@ -37,145 +37,14 @@ _M_PI: cython.double = 3.141592653589793 -from .track_kernels_geom import ( - _multimed_r_nlay_1layer, - _ray_tracing_out, -) -from .track_kernels_transform import ( - _metric_to_pixel_out, - _pixel_to_metric_out, - point_position_fast, -) - - -def ray_tracing_batch_fast(xy: cython.double[:, :], cal: cython.double[:]): - """Trace N rays through multi-media interface. - - Args: - xy: (N, 2) float64 — metric image coordinates. - cal: (31,) float64 — packed calibration. - - Returns: - (positions, directions) each (N, 3) float64. - """ - n: cython.Py_ssize_t - i: cython.Py_ssize_t - _ray_out = np.empty(6, dtype=np.float64) - _ray_out_mv: cython.double[:] = _ray_out - n = xy.shape[0] - positions = np.empty((n, 3), dtype=np.float64) - directions = np.empty((n, 3), dtype=np.float64) - for i in range(n): - _ray_tracing_out(xy[i, 0], xy[i, 1], cal, _ray_out_mv) - positions[i, 0] = _ray_out_mv[0] - positions[i, 1] = _ray_out_mv[1] - positions[i, 2] = _ray_out_mv[2] - directions[i, 0] = _ray_out_mv[3] - directions[i, 1] = _ray_out_mv[4] - directions[i, 2] = _ray_out_mv[5] - return positions, directions - - -@cython.ccall -@cython.boundscheck(False) -@cython.wraparound(False) -def point_position_batch_fast( - all_targets: cython.double[:, :, :], - num_pts: cython.int, - num_cams: cython.int, - cal_arrays, -): - """Triangulate M targets from N cameras. - - Args: - all_targets: (M, num_cams, 2) float64. - num_pts: M. - num_cams: N. - cal_arrays: tuple of (31,) float64 arrays. - - Returns: - (positions, distances) — (M, 3) and (M,) float64. - """ - i: cython.Py_ssize_t - dist: cython.double - positions = np.empty((num_pts, 3), dtype=np.float64) - distances = np.empty(num_pts, dtype=np.float64) - for i in range(num_pts): - _cal_arr = np.asarray(list(cal_arrays), dtype=np.float64) - pos, dist = point_position_fast(all_targets[i], num_cams, _cal_arr) - positions[i, 0] = pos[0] - positions[i, 1] = pos[1] - positions[i, 2] = pos[2] - distances[i] = dist - return positions, distances - - -@cython.ccall -@cython.boundscheck(False) -@cython.wraparound(False) -def pixel_to_metric_batch_fast( - xy: cython.double[:, :], - imx: cython.int, - imy: cython.int, - pix_x: cython.double, - pix_y: cython.double, - chfield: cython.int, -): - """Convert N pixel coordinates to metric.""" - n: cython.Py_ssize_t - i: cython.Py_ssize_t - _pp = np.empty(2, dtype=np.float64) - _pp_mv: cython.double[:] = _pp - n = xy.shape[0] - result = np.empty((n, 2), dtype=np.float64) - for i in range(n): - _pixel_to_metric_out( - xy[i, 0], - xy[i, 1], - imx, - imy, - pix_x, - pix_y, - chfield, - _pp_mv, - ) - result[i, 0] = _pp_mv[0] - result[i, 1] = _pp_mv[1] - return result - - -@cython.ccall -@cython.boundscheck(False) -@cython.wraparound(False) -def metric_to_pixel_batch_fast( - xy: cython.double[:, :], - imx: cython.int, - imy: cython.int, - pix_x: cython.double, - pix_y: cython.double, - chfield: cython.int, -): - """Convert N metric coordinates to pixel.""" - n: cython.Py_ssize_t - i: cython.Py_ssize_t - _pp = np.empty(2, dtype=np.float64) - _pp_mv: cython.double[:] = _pp - n = xy.shape[0] - result = np.empty((n, 2), dtype=np.float64) - for i in range(n): - _metric_to_pixel_out( - xy[i, 0], - xy[i, 1], - imx, - imy, - pix_x, - pix_y, - chfield, - _pp_mv, - ) - result[i, 0] = _pp_mv[0] - result[i, 1] = _pp_mv[1] - return result +if cython.compiled: + from cython.cimports.openptv2.algorithms.track_kernels_pixel import ( + _multimed_r_nlay_1layer, + ) +else: + from .track_kernels_pixel import ( + _multimed_r_nlay_1layer, + ) @cython.ccall diff --git a/src/openptv2/algorithms/track_kernels_corr.py b/src/openptv2/algorithms/track_kernels_corr.py index 94ea23d7..f1d7f840 100644 --- a/src/openptv2/algorithms/track_kernels_corr.py +++ b/src/openptv2/algorithms/track_kernels_corr.py @@ -4,35 +4,33 @@ import numpy as np if cython.compiled: - from cython.cimports.libc.math import ( - acos as c_acos, - ) from cython.cimports.libc.math import ( sqrt as c_sqrt, ) else: - from math import ( - acos as c_acos, - ) from math import ( sqrt as c_sqrt, ) if cython.compiled: from cython.cimports.openptv2.algorithms.track_kernels_pixel import ( + _grid_build_nogil, _point_to_pixel_out, _sorted_candidates_fast_out_nogil, ) from cython.cimports.openptv2.algorithms.track_kernels_position import ( + _angle_acc_out, _point_position_out, assess_new_position_fast_nogil, ) else: from .track_kernels_pixel import ( + _grid_build_nogil, _point_to_pixel_out, _sorted_candidates_fast_out_nogil, ) from .track_kernels_position import ( + _angle_acc_out, _point_position_out, assess_new_position_fast_nogil, ) @@ -61,73 +59,6 @@ ADD_PART_K = 3.0 -@cython.ccall -@cython.inline -@cython.boundscheck(False) -@cython.wraparound(False) -@cython.cdivision(True) -@cython.profile(False) -@cython.nogil -def _angle_acc_out( - start_x: cython.double, - start_y: cython.double, - start_z: cython.double, - pred_x: cython.double, - pred_y: cython.double, - pred_z: cython.double, - cand_x: cython.double, - cand_y: cython.double, - cand_z: cython.double, - out: cython.double[:], -) -> cython.int: - """Write angle and acc to out[0], out[1] — no tuple creation.""" - v0x: cython.double - v0y: cython.double - v0z: cython.double - v1x: cython.double - v1y: cython.double - v1z: cython.double - angle: cython.double - norm0: cython.double - norm1: cython.double - dot: cython.double - dx: cython.double - dy: cython.double - dz: cython.double - acc: cython.double - v0x = pred_x - start_x - v0y = pred_y - start_y - v0z = pred_z - start_z - v1x = cand_x - start_x - v1y = cand_y - start_y - v1z = cand_z - start_z - - if v0x == -v1x and v0y == -v1y and v0z == -v1z: - angle = 200.0 - elif v0x == v1x and v0y == v1y and v0z == v1z: - angle = 0.0 - else: - norm0 = c_sqrt(v0x * v0x + v0y * v0y + v0z * v0z) - norm1 = c_sqrt(v1x * v1x + v1y * v1y + v1z * v1z) - if norm0 == 0.0 or norm1 == 0.0: - angle = 0.0 - else: - dot = (v0x * v1x + v0y * v1y + v0z * v1z) / (norm0 * norm1) - if dot > 1.0: - dot = 1.0 - elif dot < -1.0: - dot = -1.0 - angle = c_acos(dot) * 200.0 / 3.141592653589793 - - dx = v1x - v0x - dy = v1y - v0y - dz = v1z - v0z - acc = c_sqrt(dx * dx + dy * dy + dz * dz) - out[0] = angle - out[1] = acc - return 0 - - @cython.cfunc @cython.inline @cython.boundscheck(False) @@ -194,9 +125,12 @@ def _trackcorr_particle_fast( corres_p_1: cython.int[:, ::1], targ_x_1: cython.double[:, ::1], targ_y_1: cython.double[:, ::1], + targ_sumg_1: cython.double[:, ::1], targ_x_2: cython.double[:, ::1], targ_y_2: cython.double[:, ::1], targ_tnr_2: cython.int[:, ::1], + targ_sumg_2: cython.double[:, ::1], + cand_app_2: cython.double[:], num_targets_2: cython.int[:], dvxmin: cython.double, dvxmax: cython.double, @@ -227,6 +161,17 @@ def _trackcorr_particle_fast( Zmin_lay_0: cython.double, Zmax_lay_1: cython.double, add_flag: cython.int, + cold_start_neighbour: cython.int, + app_weight: cython.double, + gate_scale: cython.double[:], + use_grid: cython.int, + grid_head_2: cython.int[:, :], + grid_next_2: cython.int[:, :], + grid_head_3: cython.int[:, :], + grid_next_3: cython.int[:, :], + grid_nx: cython.int, + grid_ny: cython.int, + grid_cell: cython.double, ) -> cython.int: prev_h: cython.int j: cython.int @@ -267,6 +212,12 @@ def _trackcorr_particle_fast( nb_vz: cython.double nb_n: cython.int nb_inv: cython.double + app_h: cython.double + app_c: cython.double + app_n: cython.int + app_term: cython.double + ci2: cython.int + tidx: cython.int path_inlist_1[h] = 0 @@ -274,8 +225,49 @@ def _trackcorr_particle_fast( X[1, 1] = path_x_1[h, 1] X[1, 2] = path_x_1[h, 2] + # Appearance signature of h: mean grey sum over its correspondent + # targets in frame 1. app_h < 0 marks "no correspondent targets". + app_h = -1.0 + if app_weight > 0.0: + app_h = 0.0 + app_n = 0 + for j in range(num_cams): + tidx = corres_p_1[h, j] + if tidx != CORRES_NONE_K: + app_h += targ_sumg_1[j, tidx] + app_n += 1 + if app_n > 0: + app_h /= app_n + else: + app_h = -1.0 + prev_h = path_prev_1[h] + # Per-particle gate freedom: scale this particle's search box and + # base-box acc gate by its own factor (1.0 = configured behaviour + # exactly; IEEE *1.0 is bit-identical). The rr COST denominators stay + # UNSCALED so bids remain comparable across particles in a contest. + # Dense-data safety is scheduler-side (crowd gate in phase_scheduler: + # freedom is granted only where no crowd can manufacture smooth lies), + # not by blunting the gate -- the scaled shell keeps its tight + # confirmation (acc < dacc*0.5, angle < dangle*0.5). + gs_p: cython.double = gate_scale[h] + gd_p: cython.double = dacc * gs_p + bvxmin: cython.double = dvxmin + bvxmax: cython.double = dvxmax + bvymin: cython.double = dvymin + bvymax: cython.double = dvymax + bvzmin: cython.double = dvzmin + bvzmax: cython.double = dvzmax + in_base: cython.int = 0 + gate_ok: cython.int = 0 + dvxmin = dvxmin * gs_p + dvxmax = dvxmax * gs_p + dvymin = dvymin * gs_p + dvymax = dvymax * gs_p + dvzmin = dvzmin * gs_p + dvzmax = dvzmax * gs_p + if prev_h >= 0: X[0, 0] = path_x_0[prev_h, 0] X[0, 1] = path_x_0[prev_h, 1] @@ -360,6 +352,9 @@ def _trackcorr_particle_fast( nb_n += 1 prev_h = path_prev_1[h] # restore -- overwritten by the scan above + if cold_start_neighbour == 0: + nb_n = 0 # track.c parity: X2 = X1, no neighbour-velocity prior + if nb_n > 0: nb_inv = 1.0 / nb_n X[2, 0] = X[1, 0] + nb_vx * nb_inv @@ -462,6 +457,12 @@ def _trackcorr_particle_fast( _wc_buf1, pt_buf, _pp_mv, + use_grid, + grid_head_2, + grid_next_2, + grid_nx, + grid_ny, + grid_cell, ) if w_nc == 0: @@ -473,6 +474,12 @@ def _trackcorr_particle_fast( X[3, 1] = path_x_2[ftnr_mm, 1] X[3, 2] = path_x_2[ftnr_mm, 2] + # Appearance signature of the frame-2 candidate, precomputed per + # step in the loop (mean grey sum over its claimed targets). + # app_c < 0 marks "no correspondent targets". + app_c = cand_app_2[ftnr_mm] if app_weight > 0.0 and app_h >= 0.0 \ + else -1.0 + if prev_h >= 0: for j in range(3): X[5, j] = 0.5 * (5.0 * X[3, j] - 4.0 * X[1, j] + X[0, j]) @@ -563,6 +570,12 @@ def _trackcorr_particle_fast( _wc_buf2, pt_buf, _pp_mv, + use_grid, + grid_head_3, + grid_next_3, + grid_nx, + grid_ny, + grid_cell, ) if wn_nc > 0: @@ -576,7 +589,18 @@ def _trackcorr_particle_fast( dp1 = X[4, 1] - X[3, 1] dp2 = X[4, 2] - X[3, 2] + # Two-tier admission: inside the configured box the normal + # gate applies; in the extended shell (per-particle freedom) + # only tightly-confirmed motion passes -- freedom to be + # FOUND, discipline to be CHOSEN. + in_base = 0 if ( + bvxmin < dp0 < bvxmax + and bvymin < dp1 < bvymax + and bvzmin < dp2 < bvzmax + ): + in_base = 1 + if in_base == 1 or ( dvxmin < dp0 < dvxmax and dvymin < dp1 < dvymax and dvzmin < dp2 < dvzmax @@ -618,7 +642,13 @@ def _trackcorr_particle_fast( angle = (angle0 + angle1) * 0.5 quali = _freq_buf2[kk] + _freq_buf1[mm] - if (acc < dacc and angle < dangle) or acc < dacc * 0.1: + gate_ok = 0 + if in_base == 1: + if (acc < gd_p and angle < dangle) or acc < gd_p * 0.1: + gate_ok = 1 + elif (acc < dacc * 0.5 and angle < dangle * 0.5): + gate_ok = 1 + if gate_ok == 1: d13 = c_sqrt( (X[1, 0] - X[3, 0]) ** 2 + (X[1, 1] - X[3, 1]) ** 2 @@ -631,6 +661,11 @@ def _trackcorr_particle_fast( ) dl = (d13 + d43) * 0.5 rr = (dl / lmax + acc / dacc + angle / dangle) / quali + if app_weight > 0.0 and app_h >= 0.0 and app_c >= 0.0: + app_term = app_c - app_h + if app_term < 0.0: + app_term = -app_term + rr += app_weight * app_term / (app_h + app_c + 1.0) inlist = path_inlist_1[h] if inlist < POSI_K: @@ -691,9 +726,9 @@ def _trackcorr_particle_fast( if ( in_volume == 1 - and dvxmin < dp0 < dvxmax - and dvymin < dp1 < dvymax - and dvzmin < dp2 < dvzmax + and bvxmin < dp0 < bvxmax + and bvymin < dp1 < bvymax + and bvzmin < dp2 < bvzmax ): _angle_acc_out( X[3, 0], @@ -725,6 +760,11 @@ def _trackcorr_particle_fast( rr = (dl / lmax + acc / dacc + angle / dangle) / ( quali + _freq_buf1[mm] ) + if app_weight > 0.0 and app_h >= 0.0 and app_c >= 0.0: + app_term = app_c - app_h + if app_term < 0.0: + app_term = -app_term + rr += app_weight * app_term / (app_h + app_c + 1.0) inlist = path_inlist_1[h] if inlist < POSI_K: @@ -763,7 +803,14 @@ def _trackcorr_particle_fast( dp1 = X[3, 1] - X[1, 1] dp2 = X[3, 2] - X[1, 2] + in_base = 0 if ( + bvxmin < dp0 < bvxmax + and bvymin < dp1 < bvymax + and bvzmin < dp2 < bvzmax + ): + in_base = 1 + if in_base == 1 or ( dvxmin < dp0 < dvxmax and dvymin < dp1 < dvymax and dvzmin < dp2 < dvzmax @@ -783,7 +830,13 @@ def _trackcorr_particle_fast( angle = _pp_mv[0] acc = _pp_mv[1] - if (acc < dacc and angle < dangle) or acc < dacc * 0.1: + gate_ok = 0 + if in_base == 1: + if (acc < gd_p and angle < dangle) or acc < gd_p * 0.1: + gate_ok = 1 + elif (acc < dacc * 0.5 and angle < dangle * 0.5): + gate_ok = 1 + if gate_ok == 1: quali_f = _freq_buf1[mm] d13 = c_sqrt( (X[1, 0] - X[3, 0]) ** 2 @@ -797,6 +850,11 @@ def _trackcorr_particle_fast( ) dl = (d13 + d01) * 0.5 rr = (dl / lmax + acc / dacc + angle / dangle) / quali_f + if app_weight > 0.0 and app_h >= 0.0 and app_c >= 0.0: + app_term = app_c - app_h + if app_term < 0.0: + app_term = -app_term + rr += app_weight * app_term / (app_h + app_c + 1.0) inlist = path_inlist_1[h] if inlist < POSI_K: @@ -861,9 +919,9 @@ def _trackcorr_particle_fast( if ( in_volume == 1 - and dvxmin < dp0 < dvxmax - and dvymin < dp1 < dvymax - and dvzmin < dp2 < dvzmax + and bvxmin < dp0 < bvxmax + and bvymin < dp1 < bvymax + and bvzmin < dp2 < bvzmax ): _angle_acc_out( X[1, 0], @@ -893,6 +951,20 @@ def _trackcorr_particle_fast( ) dl = (d13 + d01) * 0.5 rr = (dl / lmax + acc / dacc + angle / dangle) / quali2 + if app_weight > 0.0 and app_h >= 0.0: + app_c = 0.0 + app_n = 0 + for ci2 in range(num_cams): + tidx = _assess_inds2[ci2] + if tidx != PT_UNUSED: + app_c += targ_sumg_2[ci2, tidx] + app_n += 1 + if app_n > 0: + app_c /= app_n + app_term = app_c - app_h + if app_term < 0.0: + app_term = -app_term + rr += app_weight * app_term / (app_h + app_c + 1.0) claimed_ok = 1 for ci in range(num_cams): @@ -938,6 +1010,7 @@ def trackcorr_loop_fast( targ_x_1: cython.double[:, ::1], targ_y_1: cython.double[:, ::1], targ_tnr_1: cython.int[:, ::1], + targ_sumg_1: cython.double[:, ::1], # Frame 2 (next — read/write) path_x_2: cython.double[:, ::1], path_prev_2: cython.int[:], @@ -952,6 +1025,8 @@ def trackcorr_loop_fast( targ_x_2: cython.double[:, ::1], targ_y_2: cython.double[:, ::1], targ_tnr_2: cython.int[:, ::1], + targ_sumg_2: cython.double[:, ::1], + cand_app_2: cython.double[:], num_targets_2: cython.int[:], num_parts_2: cython.int[:], # Frame 3 (next-next — read/write) @@ -1008,6 +1083,11 @@ def trackcorr_loop_fast( pix_y: cython.double, flatten_tol: cython.double, num_threads: cython.int = 1, + loser_retry: cython.int = 1, + cold_start_neighbour: cython.int = 1, + app_weight: cython.double = 0.0, + gate_scale: cython.double[:] = None, + use_grid: cython.int = 0, ): """Full per-particle tracking loop + link resolution — single compiled entry. @@ -1035,6 +1115,16 @@ def trackcorr_loop_fast( ti: cython.int cand: cython.int flag: cython.bint + grid_head_2: cython.int[:, :] + grid_next_2: cython.int[:, :] + grid_head_3: cython.int[:, :] + grid_next_3: cython.int[:, :] + grid_nx: cython.int = 0 + grid_ny: cython.int = 0 + grid_cell: cython.double = 16.0 + maxnt2: cython.int = 0 + maxnt3: cython.int = 0 + _gc: cython.int n_sc: cython.int = num_cams * MAX_CANDS_K @@ -1140,6 +1230,44 @@ def trackcorr_loop_fast( added_cand_2: cython.int[:, ::1] = _added_cand_2 added_rr_2: cython.double[:] = _added_rr_2 + if gate_scale is None: + gate_scale = np.ones(orig_parts_1, dtype=np.float64) + + # Optional uniform-grid acceleration of the 2D candidate search + # (built once per step for the frame-2/frame-3 target sets, reused by + # every particle). use_grid=0 keeps the legacy y-band scan exactly. + if use_grid: + grid_nx = int(imx / grid_cell) + 1 + grid_ny = int(imy / grid_cell) + 1 + maxnt2 = 0 + maxnt3 = 0 + for _gc in range(num_cams): + if num_targets_2[_gc] > maxnt2: + maxnt2 = num_targets_2[_gc] + if num_targets_3[_gc] > maxnt3: + maxnt3 = num_targets_3[_gc] + if maxnt2 < 1: + maxnt2 = 1 + if maxnt3 < 1: + maxnt3 = 1 + _gh2 = np.full((num_cams, grid_nx * grid_ny), -1, dtype=np.int32) + _gn2 = np.empty((num_cams, maxnt2), dtype=np.int32) + _gh3 = np.full((num_cams, grid_nx * grid_ny), -1, dtype=np.int32) + _gn3 = np.empty((num_cams, maxnt3), dtype=np.int32) + grid_head_2 = _gh2 + grid_next_2 = _gn2 + grid_head_3 = _gh3 + grid_next_3 = _gn3 + for _gc in range(num_cams): + _grid_build_nogil( + targ_x_2[_gc], targ_y_2[_gc], num_targets_2[_gc], + grid_cell, grid_nx, grid_ny, + grid_head_2[_gc], grid_next_2[_gc]) + _grid_build_nogil( + targ_x_3[_gc], targ_y_3[_gc], num_targets_3[_gc], + grid_cell, grid_nx, grid_ny, + grid_head_3[_gc], grid_next_3[_gc]) + # Serial particle loop for h in range(orig_parts_1): _trackcorr_particle_fast( @@ -1199,9 +1327,12 @@ def trackcorr_loop_fast( corres_p_1, targ_x_1, targ_y_1, + targ_sumg_1, targ_x_2, targ_y_2, targ_tnr_2, + targ_sumg_2, + cand_app_2, num_targets_2, dvxmin, dvxmax, @@ -1232,6 +1363,17 @@ def trackcorr_loop_fast( Zmin_lay_0, Zmax_lay_1, add_flag, + cold_start_neighbour, + app_weight, + gate_scale, + use_grid, + grid_head_2, + grid_next_2, + grid_head_3, + grid_next_3, + grid_nx, + grid_ny, + grid_cell, ) # Sequential post-loop actual appending to global arrays @@ -1341,16 +1483,21 @@ def trackcorr_loop_fast( else: path_next_1[h] = NEXT_NONE_K - # Phase 3: Losers retry with fallback candidates (claim unclaimed only) - for h in range(orig_parts_1): - if path_inlist_1[h] > 1 and path_next_1[h] == NEXT_NONE_K: - for ti in range(1, path_inlist_1[h]): - cand = path_linkdecis_1[h, ti] - if path_prev_2[cand] == PREV_NONE_K: - path_next_1[h] = cand - path_finaldecis_1[h] = path_decis_1[h, ti] - path_prev_2[cand] = h - break + # Phase 3: Losers retry with fallback candidates (claim unclaimed only). + # NOT in the original 3dptv track.c -- there (lines 598-646) a particle that + # loses a contested candidate gets next = -2 and is done; so is the one it + # evicted. Neither falls back to its second choice. Set loser_retry=0 for + # track.c parity. Default 1 preserves existing openptv2 behaviour. + if loser_retry != 0: + for h in range(orig_parts_1): + if path_inlist_1[h] > 1 and path_next_1[h] == NEXT_NONE_K: + for ti in range(1, path_inlist_1[h]): + cand = path_linkdecis_1[h, ti] + if path_prev_2[cand] == PREV_NONE_K: + path_next_1[h] = cand + path_finaldecis_1[h] = path_decis_1[h, ti] + path_prev_2[cand] = h + break for h in range(orig_parts_1): if path_next_1[h] != NEXT_NONE_K: diff --git a/src/openptv2/algorithms/track_kernels_geom.py b/src/openptv2/algorithms/track_kernels_geom.py index e1f8a82e..487047db 100644 --- a/src/openptv2/algorithms/track_kernels_geom.py +++ b/src/openptv2/algorithms/track_kernels_geom.py @@ -8,15 +8,6 @@ import numpy as np if cython.compiled: - from cython.cimports.libc.math import ( - acos as c_acos, - ) - from cython.cimports.libc.math import ( - asin as c_asin, - ) - from cython.cimports.libc.math import ( - atan as c_atan, - ) from cython.cimports.libc.math import ( cos as c_cos, ) @@ -26,19 +17,7 @@ from cython.cimports.libc.math import ( sqrt as c_sqrt, ) - from cython.cimports.libc.math import ( - tan as c_tan, - ) else: - from math import ( - acos as c_acos, - ) - from math import ( - asin as c_asin, - ) - from math import ( - atan as c_atan, - ) from math import ( cos as c_cos, ) @@ -48,12 +27,18 @@ from math import ( sqrt as c_sqrt, ) - from math import ( - tan as c_tan, - ) _M_PI: cython.double = 3.141592653589793 +if cython.compiled: + from cython.cimports.openptv2.algorithms.track_kernels_pixel import ( + _multimed_r_nlay_1layer, + ) +else: + from .track_kernels_pixel import ( + _multimed_r_nlay_1layer, + ) + # Cal array layout (31 float64): # 0-2: ext_x0, ext_y0, ext_z0 @@ -69,85 +54,6 @@ CAL_ARRAY_SIZE = 31 -@cython.ccall -@cython.nogil -def _multimed_r_nlay_1layer( - pos_x: cython.double, - pos_y: cython.double, - pos_z: cython.double, - ext_x0: cython.double, - ext_y0: cython.double, - ext_z0: cython.double, - mm_n1: cython.double, - mm_n2_0: cython.double, - mm_n3: cython.double, - mm_d0: cython.double, -) -> cython.double: - """Single-layer iterative radial shift.""" - zout: cython.double - dx: cython.double - dy: cython.double - r: cython.double - rq: cython.double - it: cython.int - denom: cython.double - beta1: cython.double - sin_beta1: cython.double - arg: cython.double - beta2_0: cython.double - arg3: cython.double - beta3: cython.double - rbeta: cython.double - rdiff: cython.double - if mm_n1 == 1.0 and mm_n2_0 == 1.0 and mm_n3 == 1.0: - return 1.0 - - zout = pos_z - dx = pos_x - ext_x0 - dy = pos_y - ext_y0 - r = c_sqrt(dx * dx + dy * dy) - rq = r - - for it in range(40): - denom = ext_z0 - pos_z - if denom == 0.0: - return 1.0 - beta1 = c_atan(rq / denom) - sin_beta1 = c_sin(beta1) - - arg = sin_beta1 * mm_n1 / mm_n2_0 - if arg > 1.0: - arg = 1.0 - elif arg < -1.0: - arg = -1.0 - beta2_0 = c_asin(arg) - - arg3 = sin_beta1 * mm_n1 / mm_n3 - if arg3 > 1.0: - arg3 = 1.0 - elif arg3 < -1.0: - arg3 = -1.0 - beta3 = c_asin(arg3) - - rbeta = ( - (ext_z0 - mm_d0) * c_tan(beta1) - + mm_d0 * c_tan(beta2_0) - - zout * c_tan(beta3) - ) - - rdiff = r - rbeta - rq += rdiff - - if abs(rdiff) < 0.001: - break - else: - return 1.0 - - if r != 0.0: - return rq / r - return 1.0 - - @cython.ccall def point_to_pixel_fast( pos: cython.double[:], @@ -427,277 +333,6 @@ def point_to_pixel_fast( return x_pixel, y_pixel -@cython.ccall -@cython.inline -@cython.boundscheck(False) -@cython.wraparound(False) -@cython.cdivision(True) -@cython.profile(False) -@cython.nogil -def _point_to_pixel_out( - pos: cython.double[:], - cal: cython.double[:], - mmlut_data: cython.double[:], - mmlut_origin: cython.double[:], - mmlut_nr: cython.int, - mmlut_nz: cython.int, - mmlut_rw: cython.double, - has_mmlut: cython.int, - imx_half: cython.double, - imy_half: cython.double, - inv_pix_x: cython.double, - inv_pix_y: cython.double, - chfield: cython.int, - out: cython.double[:], -) -> cython.int: - """Write pixel coordinates to out[0], out[1] — no tuple creation.""" - pos0: cython.double - pos1: cython.double - pos2: cython.double - ext_x0: cython.double - ext_y0: cython.double - ext_z0: cython.double - dm00: cython.double - dm10: cython.double - dm20: cython.double - dm01: cython.double - dm11: cython.double - dm21: cython.double - dm02: cython.double - dm12: cython.double - dm22: cython.double - int_cc: cython.double - xh: cython.double - yh: cython.double - gx: cython.double - gy: cython.double - gz: cython.double - inv_dog: cython.double - mm_n1: cython.double - mm_n2_0: cython.double - mm_n3: cython.double - mm_d0: cython.double - k1: cython.double - k2: cython.double - k3: cython.double - p1: cython.double - p2: cython.double - scx: cython.double - she: cython.double - dot_cam: cython.double - dist_o_glas: cython.double - dist_cam_glas: cython.double - dot_pos: cython.double - dist_point_glas: cython.double - s_cam: cython.double - cc_x: cython.double - cc_y: cython.double - cc_z: cython.double - s_pt: cython.double - cp_x: cython.double - cp_y: cython.double - cp_z: cython.double - ext_t_z0: cython.double - s_d: cython.double - ag_x: cython.double - ag_y: cython.double - ag_z: cython.double - tmp_x: cython.double - tmp_y: cython.double - tmp_z: cython.double - pos_t_0: cython.double - pos_t_2: cython.double - radial_shift: cython.double - tx: cython.double - ty: cython.double - tz: cython.double - sz: cython.double - iz: cython.int - R: cython.double - sr: cython.double - ir: cython.int - v0: cython.int - v3: cython.int - mmf: cython.double - X_t: cython.double - s_z: cython.double - bx: cython.double - by: cython.double - bz: cython.double - s_x: cython.double - dx: cython.double - dy: cython.double - dz: cython.double - deno: cython.double - x: cython.double - y: cython.double - r: cython.double - r2: cython.double - r4: cython.double - radial_factor: cython.double - xd: cython.double - yd: cython.double - sin_she: cython.double - cos_she: cython.double - x_dist: cython.double - y_dist: cython.double - x_pixel: cython.double - y_pixel: cython.double - pos0 = pos[0] - pos1 = pos[1] - pos2 = pos[2] - - ext_x0 = cal[0] - ext_y0 = cal[1] - ext_z0 = cal[2] - dm00 = cal[3] - dm10 = cal[4] - dm20 = cal[5] - dm01 = cal[6] - dm11 = cal[7] - dm21 = cal[8] - dm02 = cal[9] - dm12 = cal[10] - dm22 = cal[11] - int_cc = cal[12] - xh = cal[13] - yh = cal[14] - gx = cal[15] - gy = cal[16] - gz = cal[17] - inv_dog = cal[19] - mm_n1 = cal[20] - mm_n2_0 = cal[21] - mm_n3 = cal[22] - mm_d0 = cal[23] - k1 = cal[24] - k2 = cal[25] - k3 = cal[26] - p1 = cal[27] - p2 = cal[28] - scx = cal[29] - she = cal[30] - - # trans_cam_point - dot_cam = ext_x0 * gx + ext_y0 * gy + ext_z0 * gz - dist_o_glas = cal[18] - dist_cam_glas = dot_cam * inv_dog - dist_o_glas - mm_d0 - - dot_pos = pos0 * gx + pos1 * gy + pos2 * gz - dist_point_glas = dot_pos * inv_dog - dist_o_glas - - s_cam = dist_cam_glas * inv_dog - cc_x = ext_x0 - gx * s_cam - cc_y = ext_y0 - gy * s_cam - cc_z = ext_z0 - gz * s_cam - - s_pt = dist_point_glas * inv_dog - cp_x = pos0 - gx * s_pt - cp_y = pos1 - gy * s_pt - cp_z = pos2 - gz * s_pt - - ext_t_z0 = dist_cam_glas + mm_d0 - - s_d = mm_d0 * inv_dog - ag_x = cc_x - gx * s_d - ag_y = cc_y - gy * s_d - ag_z = cc_z - gz * s_d - tmp_x = cp_x - ag_x - tmp_y = cp_y - ag_y - tmp_z = cp_z - ag_z - - pos_t_0 = c_sqrt(tmp_x * tmp_x + tmp_y * tmp_y + tmp_z * tmp_z) - pos_t_2 = dist_point_glas - - # mmlut lookup + multimed_nlay - radial_shift = 1.0 - if has_mmlut: - tx = pos_t_0 - mmlut_origin[0] - ty = -mmlut_origin[1] - tz = pos_t_2 - mmlut_origin[2] - sz = tz / mmlut_rw - iz = int(sz) - sz -= iz - R = c_sqrt(tx * tx + ty * ty) - sr = R / mmlut_rw - ir = int(sr) - sr -= ir - if ir <= mmlut_nr and iz >= 0 and iz <= mmlut_nz: - v0 = ir * mmlut_nz + iz - v3 = v0 + mmlut_nz + 1 - if v0 >= 0 and v3 <= mmlut_nr * mmlut_nz: - mmf = ( - mmlut_data[v0] * (1.0 - sr) * (1.0 - sz) - + mmlut_data[v0 + 1] * (1.0 - sr) * sz - + mmlut_data[v0 + mmlut_nz] * sr * (1.0 - sz) - + mmlut_data[v3] * sr * sz - ) - if mmf > 0.0: - radial_shift = mmf - if radial_shift == 1.0: - radial_shift = _multimed_r_nlay_1layer( - pos_t_0, - 0.0, - pos_t_2, - 0.0, - 0.0, - ext_t_z0, - mm_n1, - mm_n2_0, - mm_n3, - mm_d0, - ) - X_t = pos_t_0 * radial_shift - - # back_trans_point - s_z = -pos_t_2 * inv_dog - bx = ag_x - gx * s_z - by = ag_y - gy * s_z - bz = ag_z - gz * s_z - if pos_t_0 > 0.0: - s_x = -X_t / pos_t_0 - bx -= tmp_x * s_x - by -= tmp_y * s_x - bz -= tmp_z * s_x - - # perspective projection - dx = bx - ext_x0 - dy = by - ext_y0 - dz = bz - ext_z0 - deno = dm02 * dx + dm12 * dy + dm22 * dz - x = -int_cc * (dm00 * dx + dm10 * dy + dm20 * dz) / deno - y = -int_cc * (dm01 * dx + dm11 * dy + dm21 * dz) / deno - - # flat_to_dist + distort_brown_affin - x += xh - y += yh - r = c_sqrt(x * x + y * y) - if r < 1e-10: - x_dist = 0.0 - y_dist = 0.0 - else: - r2 = r * r - r4 = r2 * r2 - radial_factor = 1.0 + k1 * r2 + k2 * r4 + k3 * r4 * r2 - xd = x * radial_factor + p1 * (r2 + 2.0 * x * x) + 2.0 * p2 * x * y - yd = y * radial_factor + p2 * (r2 + 2.0 * y * y) + 2.0 * p1 * x * y - sin_she = c_sin(she) - cos_she = c_cos(she) - x_dist = scx * (xd - sin_she * yd) - y_dist = scx * cos_she * yd - - # metric_to_pixel - x_pixel = x_dist * inv_pix_x + imx_half - y_pixel = imy_half - y_dist * inv_pix_y - if chfield == 1: - y_pixel = (y_pixel - 1.0) * 0.5 - elif chfield == 2: - y_pixel = y_pixel * 0.5 - out[0] = x_pixel - out[1] = y_pixel - return 0 - - PT_UNUSED = -999 @@ -836,492 +471,3 @@ def searchquader_fast( return xr, xl, yd, yu - -@cython.ccall -@cython.boundscheck(False) -@cython.wraparound(False) -def angle_acc_fast( - start_x: cython.double, - start_y: cython.double, - start_z: cython.double, - pred_x: cython.double, - pred_y: cython.double, - pred_z: cython.double, - cand_x: cython.double, - cand_y: cython.double, - cand_z: cython.double, -): - """Compute angle and acceleration between predicted and candidate.""" - v0x: cython.double - v0y: cython.double - v0z: cython.double - v1x: cython.double - v1y: cython.double - v1z: cython.double - angle: cython.double - norm0: cython.double - norm1: cython.double - dot: cython.double - dx: cython.double - dy: cython.double - dz: cython.double - acc: cython.double - v0x = pred_x - start_x - v0y = pred_y - start_y - v0z = pred_z - start_z - v1x = cand_x - start_x - v1y = cand_y - start_y - v1z = cand_z - start_z - - if v0x == -v1x and v0y == -v1y and v0z == -v1z: - angle = 200.0 - elif v0x == v1x and v0y == v1y and v0z == v1z: - angle = 0.0 - else: - norm0 = c_sqrt(v0x * v0x + v0y * v0y + v0z * v0z) - norm1 = c_sqrt(v1x * v1x + v1y * v1y + v1z * v1z) - if norm0 == 0.0 or norm1 == 0.0: - angle = 0.0 - else: - dot = (v0x * v1x + v0y * v1y + v0z * v1z) / (norm0 * norm1) - if dot > 1.0: - dot = 1.0 - elif dot < -1.0: - dot = -1.0 - angle = c_acos(dot) * 200.0 / _M_PI - - dx = v1x - v0x - dy = v1y - v0y - dz = v1z - v0z - acc = c_sqrt(dx * dx + dy * dy + dz * dz) - return angle, acc - - -@cython.ccall -@cython.inline -@cython.boundscheck(False) -@cython.wraparound(False) -@cython.cdivision(True) -@cython.profile(False) -@cython.nogil -def _angle_acc_out( - start_x: cython.double, - start_y: cython.double, - start_z: cython.double, - pred_x: cython.double, - pred_y: cython.double, - pred_z: cython.double, - cand_x: cython.double, - cand_y: cython.double, - cand_z: cython.double, - out: cython.double[:], -) -> cython.int: - """Write angle and acc to out[0], out[1] — no tuple creation.""" - v0x: cython.double - v0y: cython.double - v0z: cython.double - v1x: cython.double - v1y: cython.double - v1z: cython.double - angle: cython.double - norm0: cython.double - norm1: cython.double - dot: cython.double - dx: cython.double - dy: cython.double - dz: cython.double - acc: cython.double - v0x = pred_x - start_x - v0y = pred_y - start_y - v0z = pred_z - start_z - v1x = cand_x - start_x - v1y = cand_y - start_y - v1z = cand_z - start_z - - if v0x == -v1x and v0y == -v1y and v0z == -v1z: - angle = 200.0 - elif v0x == v1x and v0y == v1y and v0z == v1z: - angle = 0.0 - else: - norm0 = c_sqrt(v0x * v0x + v0y * v0y + v0z * v0z) - norm1 = c_sqrt(v1x * v1x + v1y * v1y + v1z * v1z) - if norm0 == 0.0 or norm1 == 0.0: - angle = 0.0 - else: - dot = (v0x * v1x + v0y * v1y + v0z * v1z) / (norm0 * norm1) - if dot > 1.0: - dot = 1.0 - elif dot < -1.0: - dot = -1.0 - angle = c_acos(dot) * 200.0 / 3.141592653589793 - - dx = v1x - v0x - dy = v1y - v0y - dz = v1z - v0z - acc = c_sqrt(dx * dx + dy * dy + dz * dz) - out[0] = angle - out[1] = acc - return 0 - - -@cython.boundscheck(False) -@cython.wraparound(False) -def _ray_tracing_fast(x: cython.double, y: cython.double, cal: cython.double[:]): - """Trace ray through multi-media interface. - - Returns (Xx, Xy, Xz, ox, oy, oz) — crossing point and direction. - """ - ext_x0: cython.double - ext_y0: cython.double - ext_z0: cython.double - dm00: cython.double - dm10: cython.double - dm20: cython.double - dm01: cython.double - dm11: cython.double - dm21: cython.double - dm02: cython.double - dm12: cython.double - dm22: cython.double - int_cc: cython.double - gx: cython.double - gy: cython.double - gz: cython.double - mm_n1: cython.double - mm_n2_0: cython.double - mm_n3: cython.double - mm_d0: cython.double - t0: cython.double - t1: cython.double - t2: cython.double - tn: cython.double - sd0: cython.double - sd1: cython.double - sd2: cython.double - gn: cython.double - gd0: cython.double - gd1: cython.double - gd2: cython.double - c: cython.double - dcg: cython.double - denom: cython.double - d1: cython.double - Xb0: cython.double - Xb1: cython.double - Xb2: cython.double - n: cython.double - bp0: cython.double - bp1: cython.double - bp2: cython.double - bpn: cython.double - p: cython.double - n_glass: cython.double - a2_0: cython.double - a2_1: cython.double - a2_2: cython.double - d2_denom: cython.double - d2: cython.double - Xx: cython.double - Xy: cython.double - Xz: cython.double - n_a2: cython.double - p2: cython.double - n_final: cython.double - ox: cython.double - oy: cython.double - oz: cython.double - ext_x0 = cal[0] - ext_y0 = cal[1] - ext_z0 = cal[2] - dm00 = cal[3] - dm10 = cal[4] - dm20 = cal[5] - dm01 = cal[6] - dm11 = cal[7] - dm21 = cal[8] - dm02 = cal[9] - dm12 = cal[10] - dm22 = cal[11] - int_cc = cal[12] - gx = cal[15] - gy = cal[16] - gz = cal[17] - mm_n1 = cal[20] - mm_n2_0 = cal[21] - mm_n3 = cal[22] - mm_d0 = cal[23] - - # tmp1 = unit_vector([x, y, -int_cc]) - t0 = x - t1 = y - t2 = -int_cc - tn = c_sqrt(t0 * t0 + t1 * t1 + t2 * t2) - if tn > 0.0: - t0 /= tn - t1 /= tn - t2 /= tn - - # start_dir = dm @ tmp1 - sd0 = dm00 * t0 + dm01 * t1 + dm02 * t2 - sd1 = dm10 * t0 + dm11 * t1 + dm12 * t2 - sd2 = dm20 * t0 + dm21 * t1 + dm22 * t2 - - # glass_dir = unit_vector(glass_vec) - gn = c_sqrt(gx * gx + gy * gy + gz * gz) - if gn > 0.0: - gd0 = gx / gn - gd1 = gy / gn - gd2 = gz / gn - else: - gd0 = 0.0 - gd1 = 0.0 - gd2 = 0.0 - c = gn + mm_d0 - - # dist_cam_glass, d1 - dcg = gd0 * ext_x0 + gd1 * ext_y0 + gd2 * ext_z0 - c - denom = gd0 * sd0 + gd1 * sd1 + gd2 * sd2 - d1 = -dcg / denom - - # Xb = primary_point + start_dir * d1 - Xb0 = ext_x0 + sd0 * d1 - Xb1 = ext_y0 + sd1 * d1 - Xb2 = ext_z0 + sd2 * d1 - - # Decompose ray: n = dot(start_dir, glass_dir) - n = sd0 * gd0 + sd1 * gd1 + sd2 * gd2 - # bp = unit_vector(start_dir - glass_dir * n) - bp0 = sd0 - gd0 * n - bp1 = sd1 - gd1 * n - bp2 = sd2 - gd2 * n - bpn = c_sqrt(bp0 * bp0 + bp1 * bp1 + bp2 * bp2) - if bpn > 0.0: - bp0 /= bpn - bp1 /= bpn - bp2 /= bpn - - # Snell's law: air -> glass - p = c_sqrt(1.0 - n * n) * mm_n1 / mm_n2_0 - n_glass = c_sqrt(1.0 - p * p) if n >= 0 else -c_sqrt(1.0 - p * p) - - # a2 = bp * p + glass_dir * n_glass - a2_0 = bp0 * p + gd0 * n_glass - a2_1 = bp1 * p + gd1 * n_glass - a2_2 = bp2 * p + gd2 * n_glass - - d2_denom = gd0 * a2_0 + gd1 * a2_1 + gd2 * a2_2 - d2 = mm_d0 / abs(d2_denom) - - # X = Xb + a2 * d2 - Xx = Xb0 + a2_0 * d2 - Xy = Xb1 + a2_1 * d2 - Xz = Xb2 + a2_2 * d2 - - # Direction in next medium: Snell glass -> water - n_a2 = a2_0 * gd0 + a2_1 * gd1 + a2_2 * gd2 - # bp = unit_vector(a2 - glass_dir * n_glass) - bp0 = a2_0 - gd0 * n_glass - bp1 = a2_1 - gd1 * n_glass - bp2 = a2_2 - gd2 * n_glass - bpn = c_sqrt(bp0 * bp0 + bp1 * bp1 + bp2 * bp2) - if bpn > 0.0: - bp0 /= bpn - bp1 /= bpn - bp2 /= bpn - - p2 = c_sqrt(1.0 - n_a2 * n_a2) * mm_n2_0 / mm_n3 - n_final = c_sqrt(1.0 - p2 * p2) if n_a2 >= 0 else -c_sqrt(1.0 - p2 * p2) - - ox = bp0 * p2 + gd0 * n_final - oy = bp1 * p2 + gd1 * n_final - oz = bp2 * p2 + gd2 * n_final - - return Xx, Xy, Xz, ox, oy, oz - - -@cython.ccall -@cython.inline -@cython.boundscheck(False) -@cython.wraparound(False) -@cython.cdivision(True) -@cython.profile(False) -@cython.nogil -def _ray_tracing_out( - x: cython.double, - y: cython.double, - cal: cython.double[:], - out: cython.double[:], -) -> cython.int: - """Write ray tracing results into out[0:6] — no tuple creation.""" - ext_x0: cython.double - ext_y0: cython.double - ext_z0: cython.double - dm00: cython.double - dm10: cython.double - dm20: cython.double - dm01: cython.double - dm11: cython.double - dm21: cython.double - dm02: cython.double - dm12: cython.double - dm22: cython.double - int_cc: cython.double - gx: cython.double - gy: cython.double - gz: cython.double - mm_n1: cython.double - mm_n2_0: cython.double - mm_n3: cython.double - mm_d0: cython.double - t0: cython.double - t1: cython.double - t2: cython.double - tn: cython.double - sd0: cython.double - sd1: cython.double - sd2: cython.double - gn: cython.double - gd0: cython.double - gd1: cython.double - gd2: cython.double - c: cython.double - dcg: cython.double - denom: cython.double - d1: cython.double - Xb0: cython.double - Xb1: cython.double - Xb2: cython.double - n: cython.double - bp0: cython.double - bp1: cython.double - bp2: cython.double - bpn: cython.double - p: cython.double - n_glass: cython.double - a2_0: cython.double - a2_1: cython.double - a2_2: cython.double - d2_denom: cython.double - d2: cython.double - Xx: cython.double - Xy: cython.double - Xz: cython.double - n_a2: cython.double - p2: cython.double - n_final: cython.double - ox: cython.double - oy: cython.double - oz: cython.double - ext_x0 = cal[0] - ext_y0 = cal[1] - ext_z0 = cal[2] - dm00 = cal[3] - dm10 = cal[4] - dm20 = cal[5] - dm01 = cal[6] - dm11 = cal[7] - dm21 = cal[8] - dm02 = cal[9] - dm12 = cal[10] - dm22 = cal[11] - int_cc = cal[12] - gx = cal[15] - gy = cal[16] - gz = cal[17] - mm_n1 = cal[20] - mm_n2_0 = cal[21] - mm_n3 = cal[22] - mm_d0 = cal[23] - - # tmp1 = unit_vector([x, y, -int_cc]) - t0 = x - t1 = y - t2 = -int_cc - tn = c_sqrt(t0 * t0 + t1 * t1 + t2 * t2) - if tn > 0.0: - t0 /= tn - t1 /= tn - t2 /= tn - - # start_dir = dm @ tmp1 - sd0 = dm00 * t0 + dm01 * t1 + dm02 * t2 - sd1 = dm10 * t0 + dm11 * t1 + dm12 * t2 - sd2 = dm20 * t0 + dm21 * t1 + dm22 * t2 - - # glass_dir = unit_vector(glass_vec) - gn = c_sqrt(gx * gx + gy * gy + gz * gz) - if gn > 0.0: - gd0 = gx / gn - gd1 = gy / gn - gd2 = gz / gn - else: - gd0 = 0.0 - gd1 = 0.0 - gd2 = 0.0 - c = gn + mm_d0 - - # dist_cam_glass, d1 - dcg = gd0 * ext_x0 + gd1 * ext_y0 + gd2 * ext_z0 - c - denom = gd0 * sd0 + gd1 * sd1 + gd2 * sd2 - d1 = -dcg / denom - - # Xb = primary_point + start_dir * d1 - Xb0 = ext_x0 + sd0 * d1 - Xb1 = ext_y0 + sd1 * d1 - Xb2 = ext_z0 + sd2 * d1 - - # Decompose ray: n = dot(start_dir, glass_dir) - n = sd0 * gd0 + sd1 * gd1 + sd2 * gd2 - # bp = unit_vector(start_dir - glass_dir * n) - bp0 = sd0 - gd0 * n - bp1 = sd1 - gd1 * n - bp2 = sd2 - gd2 * n - bpn = c_sqrt(bp0 * bp0 + bp1 * bp1 + bp2 * bp2) - if bpn > 0.0: - bp0 /= bpn - bp1 /= bpn - bp2 /= bpn - - # Snell's law: air -> glass - p = c_sqrt(1.0 - n * n) * mm_n1 / mm_n2_0 - n_glass = c_sqrt(1.0 - p * p) if n >= 0 else -c_sqrt(1.0 - p * p) - - # a2 = bp * p + glass_dir * n_glass - a2_0 = bp0 * p + gd0 * n_glass - a2_1 = bp1 * p + gd1 * n_glass - a2_2 = bp2 * p + gd2 * n_glass - - d2_denom = gd0 * a2_0 + gd1 * a2_1 + gd2 * a2_2 - d2 = mm_d0 / abs(d2_denom) - - # X = Xb + a2 * d2 - Xx = Xb0 + a2_0 * d2 - Xy = Xb1 + a2_1 * d2 - Xz = Xb2 + a2_2 * d2 - - # Direction in next medium: Snell glass -> water - n_a2 = a2_0 * gd0 + a2_1 * gd1 + a2_2 * gd2 - # bp = unit_vector(a2 - glass_dir * n_glass) - bp0 = a2_0 - gd0 * n_glass - bp1 = a2_1 - gd1 * n_glass - bp2 = a2_2 - gd2 * n_glass - bpn = c_sqrt(bp0 * bp0 + bp1 * bp1 + bp2 * bp2) - if bpn > 0.0: - bp0 /= bpn - bp1 /= bpn - bp2 /= bpn - - p2 = c_sqrt(1.0 - n_a2 * n_a2) * mm_n2_0 / mm_n3 - n_final = c_sqrt(1.0 - p2 * p2) if n_a2 >= 0 else -c_sqrt(1.0 - p2 * p2) - - ox = bp0 * p2 + gd0 * n_final - oy = bp1 * p2 + gd1 * n_final - oz = bp2 * p2 + gd2 * n_final - - out[0] = Xx - out[1] = Xy - out[2] = Xz - out[3] = ox - out[4] = oy - out[5] = oz - return 0 diff --git a/src/openptv2/algorithms/track_kernels_pixel.pxd b/src/openptv2/algorithms/track_kernels_pixel.pxd index ebc0ae23..10d28959 100644 --- a/src/openptv2/algorithms/track_kernels_pixel.pxd +++ b/src/openptv2/algorithms/track_kernels_pixel.pxd @@ -61,6 +61,40 @@ cpdef int _dist_to_flat_out( double[:] out, ) noexcept nogil +cpdef int _grid_build_nogil( + double[:] targ_x, + double[:] targ_y, + int num_targets, + double cell, + int gnx, + int gny, + int[:] head, + int[:] nxt, +) noexcept nogil + +cpdef int _grid_candsearch_nogil( + double[:] targ_x, + double[:] targ_y, + int[:] targ_tnr, + int[:] head, + int[:] nxt, + int gnx, + int gny, + double cell, + double cent_x, + double cent_y, + double dl, + double dr, + double du, + double dd, + double imx, + double imy, + int tr_unused, + int max_cands, + int[:] out_indices, + double[:] out_dists, +) noexcept nogil + cpdef int _sorted_candidates_fast_out_nogil( double[:] center, double[:] center_proj_x, @@ -103,6 +137,23 @@ cpdef int _sorted_candidates_fast_out_nogil( int[:, :] whichcam_out, double[:] pt_buf, double[:] _pp, + int use_grid, + int[:, :] grid_head, + int[:, :] grid_next, + int grid_nx, + int grid_ny, + double grid_cell, ) noexcept nogil - +cpdef double _multimed_r_nlay_1layer( + double pos_x, + double pos_y, + double pos_z, + double ext_x0, + double ext_y0, + double ext_z0, + double mm_n1, + double mm_n2_0, + double mm_n3, + double mm_d0, +) noexcept nogil diff --git a/src/openptv2/algorithms/track_kernels_pixel.py b/src/openptv2/algorithms/track_kernels_pixel.py index a120e3ef..c87332d5 100644 --- a/src/openptv2/algorithms/track_kernels_pixel.py +++ b/src/openptv2/algorithms/track_kernels_pixel.py @@ -43,8 +43,182 @@ ) +#: Uniform-grid cell size (pixels) for the optional grid-accelerated +#: candidate search. Any value preserves results: the grid only selects a +#: superset (cells overlapping the query window) and the exact legacy +#: window filter + distance ordering run unchanged afterwards. +GRID_CELL_PX = 16.0 + + @cython.ccall +@cython.boundscheck(False) +@cython.wraparound(False) @cython.nogil +@cython.exceptval(check=False) +def _grid_build_nogil( + targ_x: cython.double[:], + targ_y: cython.double[:], + num_targets: cython.int, + cell: cython.double, + gnx: cython.int, + gny: cython.int, + head: cython.int[:], + nxt: cython.int[:], +) -> cython.int: + """Bucket 2D targets into a uniform grid (head/next index chains). + + head has length gnx*gny (filled with -1); nxt has length >= num_targets. + Out-of-image coordinates are clamped into the border cells; the exact + window filter at query time keeps results identical to a full scan. + """ + c: cython.int + i: cython.int + cx: cython.int + cy: cython.int + for c in range(gnx * gny): + head[c] = -1 + for i in range(num_targets): + cx = int(targ_x[i] / cell) + if cx < 0: + cx = 0 + elif cx >= gnx: + cx = gnx - 1 + cy = int(targ_y[i] / cell) + if cy < 0: + cy = 0 + elif cy >= gny: + cy = gny - 1 + c = cy * gnx + cx + nxt[i] = head[c] + head[c] = i + return 0 + + +@cython.ccall +@cython.boundscheck(False) +@cython.wraparound(False) +@cython.nogil +@cython.exceptval(check=False) +def _grid_candsearch_nogil( + targ_x: cython.double[:], + targ_y: cython.double[:], + targ_tnr: cython.int[:], + head: cython.int[:], + nxt: cython.int[:], + gnx: cython.int, + gny: cython.int, + cell: cython.double, + cent_x: cython.double, + cent_y: cython.double, + dl: cython.double, + dr: cython.double, + du: cython.double, + dd: cython.double, + imx: cython.double, + imy: cython.double, + tr_unused: cython.int, + max_cands: cython.int, + out_indices: cython.int[:], + out_dists: cython.double[:], +) -> cython.int: + """Grid twin of :func:`candsearch_in_pix_fast_nogil`: nearest `max_cands` + targets within the search box, nearest first. + + Window computation, image guards, validity filter and distance are + identical to the linear scan; only the traversal differs (cells + overlapping the window instead of the y-band). Ties in distance resolve + by ascending target index (deterministic); the legacy scan resolves them + by y-band order, so results are identical up to measure-zero ties + (verified empirically by the wp1 neutrality check). + """ + xmin: cython.double + xmax: cython.double + ymin: cython.double + ymax: cython.double + cx0: cython.int + cx1: cython.int + cy0: cython.int + cy1: cython.int + cx: cython.int + cy: cython.int + j: cython.int + tx: cython.double + ty: cython.double + dx: cython.double + dy: cython.double + d: cython.double + ci: cython.int + pos: cython.int + + xmin = cent_x - dl + xmax = cent_x + dr + ymin = cent_y - du + ymax = cent_y + dd + + if xmin < 0.0: + xmin = 0.0 + if xmax > imx: + xmax = imx + if ymin < 0.0: + ymin = 0.0 + if ymax > imy: + ymax = imy + + for ci in range(max_cands): + out_indices[ci] = -999 # PT_UNUSED + out_dists[ci] = 1e20 + + if not (0.0 <= cent_x <= imx and 0.0 <= cent_y <= imy): + return 0 + + cx0 = int(xmin / cell) + if cx0 < 0: + cx0 = 0 + cx1 = int(xmax / cell) + if cx1 >= gnx: + cx1 = gnx - 1 + cy0 = int(ymin / cell) + if cy0 < 0: + cy0 = 0 + cy1 = int(ymax / cell) + if cy1 >= gny: + cy1 = gny - 1 + + for cy in range(cy0, cy1 + 1): + for cx in range(cx0, cx1 + 1): + j = head[cy * gnx + cx] + while j >= 0: + if targ_tnr[j] != tr_unused: + tx = targ_x[j] + ty = targ_y[j] + if tx > xmin and tx < xmax and ty > ymin and ty < ymax: + dx = cent_x - tx + dy = cent_y - ty + d = c_sqrt(dx * dx + dy * dy) + if d < out_dists[max_cands - 1] or ( + d == out_dists[max_cands - 1] + and j < out_indices[max_cands - 1] + ): + pos = max_cands - 1 + while pos > 0 and ( + out_dists[pos - 1] > d + or ( + out_dists[pos - 1] == d + and out_indices[pos - 1] > j + ) + ): + out_dists[pos] = out_dists[pos - 1] + out_indices[pos] = out_indices[pos - 1] + pos -= 1 + out_dists[pos] = d + out_indices[pos] = j + j = nxt[j] + return 0 + + +@cython.ccall +@cython.nogil +@cython.exceptval(check=False) def _multimed_r_nlay_1layer( pos_x: cython.double, pos_y: cython.double, @@ -547,6 +721,12 @@ def _sorted_candidates_fast_out_nogil( whichcam_out: cython.int[:, :], pt_buf: cython.double[:], _pp: cython.double[:], + use_grid: cython.int, + grid_head: cython.int[:, :], + grid_next: cython.int[:, :], + grid_nx: cython.int, + grid_ny: cython.int, + grid_cell: cython.double, ) -> cython.int: n: cython.int px: cython.double @@ -699,24 +879,48 @@ def _sorted_candidates_fast_out_nogil( # --- candsearch per camera, write directly into ftnr_out/whichcam_out --- for cam in range(num_cams): - candsearch_in_pix_fast_nogil( - targ_x[cam], - targ_y[cam], - targ_tnr[cam], - num_targets[cam], - center_proj_x[cam], - center_proj_y[cam], - xl[cam], - xr[cam], - yu[cam], - yd[cam], - imx, - imy, - tr_unused, - max_cands, - cands_buf, - cand_dists_buf, - ) + if use_grid: + _grid_candsearch_nogil( + targ_x[cam], + targ_y[cam], + targ_tnr[cam], + grid_head[cam], + grid_next[cam], + grid_nx, + grid_ny, + grid_cell, + center_proj_x[cam], + center_proj_y[cam], + xl[cam], + xr[cam], + yu[cam], + yd[cam], + imx, + imy, + tr_unused, + max_cands, + cands_buf, + cand_dists_buf, + ) + else: + candsearch_in_pix_fast_nogil( + targ_x[cam], + targ_y[cam], + targ_tnr[cam], + num_targets[cam], + center_proj_x[cam], + center_proj_y[cam], + xl[cam], + xr[cam], + yu[cam], + yd[cam], + imx, + imy, + tr_unused, + max_cands, + cands_buf, + cand_dists_buf, + ) base = cam * max_cands for ci in range(max_cands): diff --git a/src/openptv2/algorithms/track_kernels_position.pxd b/src/openptv2/algorithms/track_kernels_position.pxd index 2de4ac27..65638675 100644 --- a/src/openptv2/algorithms/track_kernels_position.pxd +++ b/src/openptv2/algorithms/track_kernels_position.pxd @@ -41,4 +41,22 @@ cpdef int assess_new_position_fast_nogil( double[:] scratch, ) noexcept nogil +cpdef int _ray_tracing_out( + double x, + double y, + double[:] cal, + double[:] out, +) noexcept nogil +cpdef int _angle_acc_out( + double start_x, + double start_y, + double start_z, + double pred_x, + double pred_y, + double pred_z, + double cand_x, + double cand_y, + double cand_z, + double[:] out, +) noexcept nogil diff --git a/src/openptv2/algorithms/track_kernels_position.py b/src/openptv2/algorithms/track_kernels_position.py index 5c3d7d81..51958775 100644 --- a/src/openptv2/algorithms/track_kernels_position.py +++ b/src/openptv2/algorithms/track_kernels_position.py @@ -5,10 +5,16 @@ import numpy as np if cython.compiled: + from cython.cimports.libc.math import ( + acos as c_acos, + ) from cython.cimports.libc.math import ( sqrt as c_sqrt, ) else: + from math import ( + acos as c_acos, + ) from math import ( sqrt as c_sqrt, ) @@ -55,6 +61,75 @@ @cython.cdivision(True) @cython.profile(False) @cython.nogil +@cython.exceptval(check=False) +def _angle_acc_out( + start_x: cython.double, + start_y: cython.double, + start_z: cython.double, + pred_x: cython.double, + pred_y: cython.double, + pred_z: cython.double, + cand_x: cython.double, + cand_y: cython.double, + cand_z: cython.double, + out: cython.double[:], +) -> cython.int: + """Write angle and acc to out[0], out[1] — no tuple creation.""" + v0x: cython.double + v0y: cython.double + v0z: cython.double + v1x: cython.double + v1y: cython.double + v1z: cython.double + angle: cython.double + norm0: cython.double + norm1: cython.double + dot: cython.double + dx: cython.double + dy: cython.double + dz: cython.double + acc: cython.double + v0x = pred_x - start_x + v0y = pred_y - start_y + v0z = pred_z - start_z + v1x = cand_x - start_x + v1y = cand_y - start_y + v1z = cand_z - start_z + + if v0x == -v1x and v0y == -v1y and v0z == -v1z: + angle = 200.0 + elif v0x == v1x and v0y == v1y and v0z == v1z: + angle = 0.0 + else: + norm0 = c_sqrt(v0x * v0x + v0y * v0y + v0z * v0z) + norm1 = c_sqrt(v1x * v1x + v1y * v1y + v1z * v1z) + if norm0 == 0.0 or norm1 == 0.0: + angle = 0.0 + else: + dot = (v0x * v1x + v0y * v1y + v0z * v1z) / (norm0 * norm1) + if dot > 1.0: + dot = 1.0 + elif dot < -1.0: + dot = -1.0 + angle = c_acos(dot) * 200.0 / 3.141592653589793 + + dx = v1x - v0x + dy = v1y - v0y + dz = v1z - v0z + acc = c_sqrt(dx * dx + dy * dy + dz * dz) + out[0] = angle + out[1] = acc + return 0 + + +@cython.ccall +@cython.inline +@cython.boundscheck(False) +@cython.wraparound(False) +@cython.cdivision(True) +@cython.profile(False) +@cython.nogil +@cython.exceptval(check=False) def _ray_tracing_out( x: cython.double, y: cython.double, diff --git a/src/openptv2/algorithms/track_kernels_search.py b/src/openptv2/algorithms/track_kernels_search.py index eee820bd..63153d2a 100644 --- a/src/openptv2/algorithms/track_kernels_search.py +++ b/src/openptv2/algorithms/track_kernels_search.py @@ -7,392 +7,24 @@ import numpy as np if cython.compiled: - from cython.cimports.libc.math import ( - asin as c_asin, - ) - from cython.cimports.libc.math import ( - atan as c_atan, - ) - from cython.cimports.libc.math import ( - cos as c_cos, - ) - from cython.cimports.libc.math import ( - sin as c_sin, - ) from cython.cimports.libc.math import ( sqrt as c_sqrt, ) - from cython.cimports.libc.math import ( - tan as c_tan, - ) else: - from math import ( - asin as c_asin, - ) - from math import ( - atan as c_atan, - ) - from math import ( - cos as c_cos, - ) - from math import ( - sin as c_sin, - ) from math import ( sqrt as c_sqrt, ) - from math import ( - tan as c_tan, - ) _M_PI: cython.double = 3.141592653589793 - -@cython.cfunc -@cython.nogil -def _multimed_r_nlay_1layer( - pos_x: cython.double, - pos_y: cython.double, - pos_z: cython.double, - ext_x0: cython.double, - ext_y0: cython.double, - ext_z0: cython.double, - mm_n1: cython.double, - mm_n2_0: cython.double, - mm_n3: cython.double, - mm_d0: cython.double, -) -> cython.double: - """Single-layer iterative radial shift.""" - zout: cython.double - dx: cython.double - dy: cython.double - r: cython.double - rq: cython.double - it: cython.int - denom: cython.double - beta1: cython.double - sin_beta1: cython.double - arg: cython.double - beta2_0: cython.double - arg3: cython.double - beta3: cython.double - rbeta: cython.double - rdiff: cython.double - if mm_n1 == 1.0 and mm_n2_0 == 1.0 and mm_n3 == 1.0: - return 1.0 - - zout = pos_z - dx = pos_x - ext_x0 - dy = pos_y - ext_y0 - r = c_sqrt(dx * dx + dy * dy) - rq = r - - for it in range(40): - denom = ext_z0 - pos_z - if denom == 0.0: - return 1.0 - beta1 = c_atan(rq / denom) - sin_beta1 = c_sin(beta1) - - arg = sin_beta1 * mm_n1 / mm_n2_0 - if arg > 1.0: - arg = 1.0 - elif arg < -1.0: - arg = -1.0 - beta2_0 = c_asin(arg) - - arg3 = sin_beta1 * mm_n1 / mm_n3 - if arg3 > 1.0: - arg3 = 1.0 - elif arg3 < -1.0: - arg3 = -1.0 - beta3 = c_asin(arg3) - - rbeta = ( - (ext_z0 - mm_d0) * c_tan(beta1) - + mm_d0 * c_tan(beta2_0) - - zout * c_tan(beta3) - ) - - rdiff = r - rbeta - rq += rdiff - - if abs(rdiff) < 0.001: - break - else: - return 1.0 - - if r != 0.0: - return rq / r - return 1.0 - - -@cython.cfunc -@cython.profile(False) -@cython.nogil -def _point_to_pixel_out( - pos: cython.double[:], - cal: cython.double[:], - mmlut_data: cython.double[:], - mmlut_origin: cython.double[:], - mmlut_nr: cython.int, - mmlut_nz: cython.int, - mmlut_rw: cython.double, - has_mmlut: cython.int, - imx_half: cython.double, - imy_half: cython.double, - inv_pix_x: cython.double, - inv_pix_y: cython.double, - chfield: cython.int, - out: cython.double[:], -) -> cython.int: - """Write pixel coordinates to out[0], out[1] — no tuple creation.""" - pos0: cython.double - pos1: cython.double - pos2: cython.double - ext_x0: cython.double - ext_y0: cython.double - ext_z0: cython.double - dm00: cython.double - dm10: cython.double - dm20: cython.double - dm01: cython.double - dm11: cython.double - dm21: cython.double - dm02: cython.double - dm12: cython.double - dm22: cython.double - int_cc: cython.double - xh: cython.double - yh: cython.double - gx: cython.double - gy: cython.double - gz: cython.double - inv_dog: cython.double - mm_n1: cython.double - mm_n2_0: cython.double - mm_n3: cython.double - mm_d0: cython.double - k1: cython.double - k2: cython.double - k3: cython.double - p1: cython.double - p2: cython.double - scx: cython.double - she: cython.double - dot_cam: cython.double - dist_o_glas: cython.double - dist_cam_glas: cython.double - dot_pos: cython.double - dist_point_glas: cython.double - s_cam: cython.double - cc_x: cython.double - cc_y: cython.double - cc_z: cython.double - s_pt: cython.double - cp_x: cython.double - cp_y: cython.double - cp_z: cython.double - ext_t_z0: cython.double - s_d: cython.double - ag_x: cython.double - ag_y: cython.double - ag_z: cython.double - tmp_x: cython.double - tmp_y: cython.double - tmp_z: cython.double - pos_t_0: cython.double - pos_t_2: cython.double - radial_shift: cython.double - tx: cython.double - ty: cython.double - tz: cython.double - sz: cython.double - iz: cython.int - R: cython.double - sr: cython.double - ir: cython.int - v0: cython.int - v3: cython.int - mmf: cython.double - X_t: cython.double - s_z: cython.double - bx: cython.double - by: cython.double - bz: cython.double - s_x: cython.double - dx: cython.double - dy: cython.double - dz: cython.double - deno: cython.double - x: cython.double - y: cython.double - r: cython.double - r2: cython.double - r4: cython.double - radial_factor: cython.double - xd: cython.double - yd: cython.double - sin_she: cython.double - cos_she: cython.double - x_dist: cython.double - y_dist: cython.double - x_pixel: cython.double - y_pixel: cython.double - - pos0 = pos[0] - pos1 = pos[1] - pos2 = pos[2] - - ext_x0 = cal[0] - ext_y0 = cal[1] - ext_z0 = cal[2] - dm00 = cal[3] - dm10 = cal[4] - dm20 = cal[5] - dm01 = cal[6] - dm11 = cal[7] - dm21 = cal[8] - dm02 = cal[9] - dm12 = cal[10] - dm22 = cal[11] - int_cc = cal[12] - xh = cal[13] - yh = cal[14] - gx = cal[15] - gy = cal[16] - gz = cal[17] - inv_dog = cal[19] - mm_n1 = cal[20] - mm_n2_0 = cal[21] - mm_n3 = cal[22] - mm_d0 = cal[23] - k1 = cal[24] - k2 = cal[25] - k3 = cal[26] - p1 = cal[27] - p2 = cal[28] - scx = cal[29] - she = cal[30] - - # trans_cam_point - dot_cam = ext_x0 * gx + ext_y0 * gy + ext_z0 * gz - dist_o_glas = cal[18] - dist_cam_glas = dot_cam * inv_dog - dist_o_glas - mm_d0 - - dot_pos = pos0 * gx + pos1 * gy + pos2 * gz - dist_point_glas = dot_pos * inv_dog - dist_o_glas - - s_cam = dist_cam_glas * inv_dog - cc_x = ext_x0 - gx * s_cam - cc_y = ext_y0 - gy * s_cam - cc_z = ext_z0 - gz * s_cam - - s_pt = dist_point_glas * inv_dog - cp_x = pos0 - gx * s_pt - cp_y = pos1 - gy * s_pt - cp_z = pos2 - gz * s_pt - - ext_t_z0 = dist_cam_glas + mm_d0 - - s_d = mm_d0 * inv_dog - ag_x = cc_x - gx * s_d - ag_y = cc_y - gy * s_d - ag_z = cc_z - gz * s_d - tmp_x = cp_x - ag_x - tmp_y = cp_y - ag_y - tmp_z = cp_z - ag_z - - pos_t_0 = c_sqrt(tmp_x * tmp_x + tmp_y * tmp_y + tmp_z * tmp_z) - pos_t_2 = dist_point_glas - - # mmlut lookup + multimed_nlay - radial_shift = 1.0 - if has_mmlut: - tx = pos_t_0 - mmlut_origin[0] - ty = -mmlut_origin[1] - tz = pos_t_2 - mmlut_origin[2] - sz = tz / mmlut_rw - iz = int(sz) - sz -= iz - R = c_sqrt(tx * tx + ty * ty) - sr = R / mmlut_rw - ir = int(sr) - sr -= ir - if ir <= mmlut_nr and iz >= 0 and iz <= mmlut_nz: - v0 = ir * mmlut_nz + iz - v3 = v0 + mmlut_nz + 1 - if v0 >= 0 and v3 <= mmlut_nr * mmlut_nz: - mmf = ( - mmlut_data[v0] * (1.0 - sr) * (1.0 - sz) - + mmlut_data[v0 + 1] * (1.0 - sr) * sz - + mmlut_data[v0 + mmlut_nz] * sr * (1.0 - sz) - + mmlut_data[v3] * sr * sz - ) - if mmf > 0.0: - radial_shift = mmf - if radial_shift == 1.0: - radial_shift = _multimed_r_nlay_1layer( - pos_t_0, - 0.0, - pos_t_2, - 0.0, - 0.0, - ext_t_z0, - mm_n1, - mm_n2_0, - mm_n3, - mm_d0, - ) - X_t = pos_t_0 * radial_shift - - # back_trans_point - s_z = -pos_t_2 * inv_dog - bx = ag_x - gx * s_z - by = ag_y - gy * s_z - bz = ag_z - gz * s_z - if pos_t_0 > 0.0: - s_x = -X_t / pos_t_0 - bx -= tmp_x * s_x - by -= tmp_y * s_x - bz -= tmp_z * s_x - - # perspective projection - dx = bx - ext_x0 - dy = by - ext_y0 - dz = bz - ext_z0 - deno = dm02 * dx + dm12 * dy + dm22 * dz - x = -int_cc * (dm00 * dx + dm10 * dy + dm20 * dz) / deno - y = -int_cc * (dm01 * dx + dm11 * dy + dm21 * dz) / deno - - # flat_to_dist + distort_brown_affin - x += xh - y += yh - r = c_sqrt(x * x + y * y) - if r < 1e-10: - x_dist = 0.0 - y_dist = 0.0 - else: - r2 = r * r - r4 = r2 * r2 - radial_factor = 1.0 + k1 * r2 + k2 * r4 + k3 * r4 * r2 - xd = x * radial_factor + p1 * (r2 + 2.0 * x * x) + 2.0 * p2 * x * y - yd = y * radial_factor + p2 * (r2 + 2.0 * y * y) + 2.0 * p1 * x * y - sin_she = c_sin(she) - cos_she = c_cos(she) - x_dist = scx * (xd - sin_she * yd) - y_dist = scx * cos_she * yd - - # metric_to_pixel - x_pixel = x_dist * inv_pix_x + imx_half - y_pixel = imy_half - y_dist * inv_pix_y - if chfield == 1: - y_pixel = (y_pixel - 1.0) * 0.5 - elif chfield == 2: - y_pixel = y_pixel * 0.5 - out[0] = x_pixel - out[1] = y_pixel - return 0 +if cython.compiled: + from cython.cimports.openptv2.algorithms.track_kernels_pixel import ( + _point_to_pixel_out, + ) +else: + from .track_kernels_pixel import ( + _point_to_pixel_out, + ) # Sentinel values for unused particle/candidate indices — typed C int @@ -622,89 +254,6 @@ def candsearch_in_pix_rest_fast( return best, counter -@cython.ccall -@cython.boundscheck(False) -@cython.wraparound(False) -def sort_candidates_by_freq_fast( - ftnr: cython.int[:], - freq: cython.int[:], - whichcam: cython.int[:, ::1], - n: cython.int, - num_cams: cython.int, - max_cands: cython.int, -): - """Sort candidates by frequency, matches C algorithm. - - Args: - ftnr: (n,) int32 — candidate target numbers (TR_UNUSED = -1). - freq: (n,) int32 — frequency counts (zeroed on entry). - whichcam: (n, num_cams) int32 — camera flags. - n: total number of entries (num_cams * max_cands). - num_cams: number of cameras. - max_cands: candidates per camera (4). - - Returns: - num_valid: number of valid candidates after sort. - """ - i: cython.int - j: cython.int - m: cython.int - k: cython.int - ftnr_i: cython.int - num_valid: cython.int - tr_unused = -1 - - for i in range(n): - ftnr_i = ftnr[i] - if ftnr_i == tr_unused: - continue - for j in range(num_cams): - for m in range(max_cands): - if ftnr_i == ftnr[max_cands * j + m]: - whichcam[i, j] = 1 - - for i in range(n): - if ftnr[i] != tr_unused: - for j in range(num_cams): - if whichcam[i, j] == 1: - freq[i] += 1 - - for i in range(1, n): - for j in range(n - 1, i - 1, -1): - if freq[j - 1] < freq[j]: - ftnr[j - 1], ftnr[j] = ftnr[j], ftnr[j - 1] - freq[j - 1], freq[j] = freq[j], freq[j - 1] - for k in range(num_cams): - whichcam[j - 1, k], whichcam[j, k] = ( - whichcam[j, k], - whichcam[j - 1, k], - ) - - for i in range(n): - ftnr_i = ftnr[i] - for j in range(i + 1, n): - if ftnr[j] == ftnr_i or freq[j] < 2: - freq[j] = 0 - ftnr[j] = tr_unused - - for i in range(1, n): - for j in range(n - 1, i - 1, -1): - if freq[j - 1] < freq[j]: - ftnr[j - 1], ftnr[j] = ftnr[j], ftnr[j - 1] - freq[j - 1], freq[j] = freq[j], freq[j - 1] - for k in range(num_cams): - whichcam[j - 1, k], whichcam[j, k] = ( - whichcam[j, k], - whichcam[j - 1, k], - ) - - num_valid = 0 - for i in range(n): - if freq[i] != 0: - num_valid += 1 - return num_valid - - @cython.ccall @cython.boundscheck(False) @cython.wraparound(False) @@ -1028,406 +577,3 @@ def _sorted_candidates_fast_out( num_valid += 1 return num_valid - -@cython.ccall -@cython.boundscheck(False) -@cython.wraparound(False) -@cython.nogil -def candsearch_in_pix_fast_nogil( - targ_x: cython.double[:], - targ_y: cython.double[:], - targ_tnr: cython.int[:], - num_targets: cython.int, - cent_x: cython.double, - cent_y: cython.double, - dl: cython.double, - dr: cython.double, - du: cython.double, - dd: cython.double, - imx: cython.double, - imy: cython.double, - tr_unused: cython.int, - out_indices: cython.int[:], -) -> cython.int: - xmin: cython.double - xmax: cython.double - ymin: cython.double - ymax: cython.double - p1: cython.int - p2: cython.int - p3: cython.int - p4: cython.int - d1: cython.double - d2: cython.double - d3: cython.double - d4: cython.double - j0: cython.int - dj: cython.int - j: cython.int - ty: cython.double - tx: cython.double - dx: cython.double - dy: cython.double - d: cython.double - - xmin = cent_x - dl - xmax = cent_x + dr - ymin = cent_y - du - ymax = cent_y + dd - - if xmin < 0.0: - xmin = 0.0 - if xmax > imx: - xmax = imx - if ymin < 0.0: - ymin = 0.0 - if ymax > imy: - ymax = imy - - p1 = -999 # PT_UNUSED is -999 - p2 = -999 - p3 = -999 - p4 = -999 - d1 = 1e20 - d2 = 1e20 - d3 = 1e20 - d4 = 1e20 - - if not (0.0 <= cent_x <= imx and 0.0 <= cent_y <= imy): - out_indices[0] = p1 - out_indices[1] = p2 - out_indices[2] = p3 - out_indices[3] = p4 - return 0 - - j0 = num_targets // 2 - dj = num_targets // 4 - while dj > 1: - if targ_y[j0] < ymin: - j0 += dj - else: - j0 -= dj - dj //= 2 - - j0 -= 12 - if j0 < 0: - j0 = 0 - - for j in range(j0, num_targets): - ty = targ_y[j] - if targ_tnr[j] != tr_unused: - if ty > ymax: - break - tx = targ_x[j] - if tx > xmin and tx < xmax and ty > ymin and ty < ymax: - dx = cent_x - tx - dy = cent_y - ty - d = c_sqrt(dx * dx + dy * dy) - - if d < d1: - p4 = p3 - p3 = p2 - p2 = p1 - p1 = j - d4 = d3 - d3 = d2 - d2 = d1 - d1 = d - elif d < d2: - p4 = p3 - p3 = p2 - p2 = j - d4 = d3 - d3 = d2 - d2 = d - elif d < d3: - p4 = p3 - p3 = j - d4 = d3 - d3 = d - elif d < d4: - p4 = j - d4 = d - - out_indices[0] = p1 - out_indices[1] = p2 - out_indices[2] = p3 - out_indices[3] = p4 - return 0 - - -@cython.ccall -@cython.inline -@cython.boundscheck(False) -@cython.wraparound(False) -@cython.nogil -def _sorted_candidates_fast_out_nogil( - center: cython.double[:], - center_proj_x: cython.double[:], - center_proj_y: cython.double[:], - num_cams: cython.int, - max_cands: cython.int, - cal_arr: cython.double[:, ::1], - md0: cython.double[:], - md1: cython.double[:], - md2: cython.double[:], - md3: cython.double[:], - md4: cython.double[:], - md5: cython.double[:], - md6: cython.double[:], - md7: cython.double[:], - mo_arr: cython.double[:, ::1], - mnr_arr: cython.int[:], - mnz_arr: cython.int[:], - mrw_arr: cython.double[:], - targ_x: cython.double[:, ::1], - targ_y: cython.double[:, ::1], - targ_tnr: cython.int[:, ::1], - num_targets: cython.int[:], - dvxmin: cython.double, - dvxmax: cython.double, - dvymin: cython.double, - dvymax: cython.double, - dvzmin: cython.double, - dvzmax: cython.double, - imx_half: cython.double, - imy_half: cython.double, - inv_pix_x: cython.double, - inv_pix_y: cython.double, - chfield: cython.int, - imx: cython.double, - imy: cython.double, - tr_unused: cython.int, - ftnr_out: cython.int[:], - freq_out: cython.int[:], - whichcam_out: cython.int[:, :], -) -> cython.int: - n: cython.int - px: cython.double - py: cython.double - pz: cython.double - i: cython.int - pt: cython.int - xr_i: cython.double - xl_i: cython.double - yd_i: cython.double - yu_i: cython.double - cx: cython.double - cy: cython.double - corner_x: cython.double - corner_y: cython.double - mrw: cython.double - mnr: cython.int - mnz: cython.int - has_mmlut: cython.int - cam: cython.int - base: cython.int - ci: cython.int - idx: cython.int - ftnr_i: cython.int - num_valid: cython.int - j: cython.int - m: cython.int - k: cython.int - _pp: cython.double[:] - quader_buf: cython.double[:] - pt_buf: cython.double[:] - with cython.gil: - _pp_buf = np.zeros(2, dtype=np.float64) - _pp = _pp_buf - _quader_buf = np.zeros(24, dtype=np.float64) - quader_buf = _quader_buf - _pt_buf = np.zeros(3, dtype=np.float64) - pt_buf = _pt_buf - - n = num_cams * max_cands - - # --- searchquader inlined --- - px = center[0] - py = center[1] - pz = center[2] - for pt in range(8): - quader_buf[pt * 3 + 0] = px + (dvxmax if pt & 1 else dvxmin) - quader_buf[pt * 3 + 1] = py + (dvymax if pt & 2 else dvymin) - quader_buf[pt * 3 + 2] = pz + (dvzmax if pt & 4 else dvzmin) - - xr: cython.double[:] - xl: cython.double[:] - yd: cython.double[:] - yu: cython.double[:] - with cython.gil: - _xr_buf = np.zeros(8, dtype=np.float64) - xr = _xr_buf - _xl_buf = np.zeros(8, dtype=np.float64) - xl = _xl_buf - _yd_buf = np.zeros(8, dtype=np.float64) - yd = _yd_buf - _yu_buf = np.zeros(8, dtype=np.float64) - yu = _yu_buf - - for i in range(num_cams): - cal = cal_arr[i] - mo = mo_arr[i] - mnr = mnr_arr[i] - mnz = mnz_arr[i] - mrw = mrw_arr[i] - has_mmlut = mnr > 0 - - # Select pre-unpacked md memoryview without GIL - md: cython.double[:] - if i == 0: - md = md0 - elif i == 1: - md = md1 - elif i == 2: - md = md2 - elif i == 3: - md = md3 - elif i == 4: - md = md4 - elif i == 5: - md = md5 - elif i == 6: - md = md6 - else: - md = md7 - - xr_i = 0.0 - xl_i = float(imx) - yd_i = 0.0 - yu_i = float(imy) - # Use pre-computed center projection - cx = center_proj_x[i] - cy = center_proj_y[i] - for pt in range(8): - pt_buf[0] = quader_buf[pt * 3 + 0] - pt_buf[1] = quader_buf[pt * 3 + 1] - pt_buf[2] = quader_buf[pt * 3 + 2] - _point_to_pixel_out( - pt_buf, - cal, - md, - mo, - mnr, - mnz, - mrw, - has_mmlut, - imx_half, - imy_half, - inv_pix_x, - inv_pix_y, - chfield, - _pp, - ) - corner_x = _pp[0] - corner_y = _pp[1] - if corner_x < xl_i: - xl_i = corner_x - if corner_y < yu_i: - yu_i = corner_y - if corner_x > xr_i: - xr_i = corner_x - if corner_y > yd_i: - yd_i = corner_y - if xl_i < 0.0: - xl_i = 0.0 - if yu_i < 0.0: - yu_i = 0.0 - if xr_i > imx: - xr_i = imx - if yd_i > imy: - yd_i = imy - xr[i] = xr_i - cx - xl[i] = cx - xl_i - yd[i] = yd_i - cy - yu[i] = cy - yu_i - - # --- initialize output buffers --- - for i in range(n): - ftnr_out[i] = tr_unused - freq_out[i] = 0 - for j in range(num_cams): - whichcam_out[i, j] = 0 - - # Local buffer for candsearch_in_pix_fast_nogil - cands_buf: cython.int[:] - with cython.gil: - _cands_buf = np.zeros(4, dtype=np.int32) - cands_buf = _cands_buf - - # --- candsearch per camera, write directly into ftnr_out/whichcam_out --- - for cam in range(num_cams): - candsearch_in_pix_fast_nogil( - targ_x[cam], - targ_y[cam], - targ_tnr[cam], - num_targets[cam], - center_proj_x[cam], - center_proj_y[cam], - xl[cam], - xr[cam], - yu[cam], - yd[cam], - imx, - imy, - tr_unused, - cands_buf, - ) - - base = cam * max_cands - for ci in range(4): - idx = cands_buf[ci] - if idx != -999: # PT_UNUSED is -999 - whichcam_out[base + ci, cam] = 1 - ftnr_out[base + ci] = targ_tnr[cam, idx] - - # --- sort_candidates_by_freq inlined --- - for i in range(n): - ftnr_i = ftnr_out[i] - if ftnr_i == tr_unused: - continue - for j in range(num_cams): - for m in range(max_cands): - if ftnr_i == ftnr_out[max_cands * j + m]: - whichcam_out[i, j] = 1 - - for i in range(n): - if ftnr_out[i] != tr_unused: - for j in range(num_cams): - if whichcam_out[i, j] == 1: - freq_out[i] += 1 - - for i in range(1, n): - for j in range(n - 1, i - 1, -1): - if freq_out[j - 1] < freq_out[j]: - ftnr_out[j - 1], ftnr_out[j] = ftnr_out[j], ftnr_out[j - 1] - freq_out[j - 1], freq_out[j] = freq_out[j], freq_out[j - 1] - for k in range(num_cams): - whichcam_out[j - 1, k], whichcam_out[j, k] = ( - whichcam_out[j, k], - whichcam_out[j - 1, k], - ) - - for i in range(n): - ftnr_i = ftnr_out[i] - for j in range(i + 1, n): - if ftnr_out[j] == ftnr_i or freq_out[j] < 2: - freq_out[j] = 0 - ftnr_out[j] = tr_unused - - for i in range(1, n): - for j in range(n - 1, i - 1, -1): - if freq_out[j - 1] < freq_out[j]: - ftnr_out[j - 1], ftnr_out[j] = ftnr_out[j], ftnr_out[j - 1] - freq_out[j - 1], freq_out[j] = freq_out[j], freq_out[j - 1] - for k in range(num_cams): - whichcam_out[j - 1, k], whichcam_out[j, k] = ( - whichcam_out[j, k], - whichcam_out[j - 1, k], - ) - - num_valid = 0 - for i in range(n): - if freq_out[i] != 0: - num_valid += 1 - return num_valid diff --git a/src/openptv2/algorithms/track_kernels_track3d.py b/src/openptv2/algorithms/track_kernels_track3d.py index 7918b580..29a421e6 100644 --- a/src/openptv2/algorithms/track_kernels_track3d.py +++ b/src/openptv2/algorithms/track_kernels_track3d.py @@ -8,9 +8,13 @@ UNSUPPORTED_PENALTY = 1e6 if cython.compiled: - from cython.cimports.libc.math import floor as c_floor, sqrt as c_sqrt, isnan as c_isnan + from cython.cimports.libc.math import floor as c_floor + from cython.cimports.libc.math import isnan as c_isnan + from cython.cimports.libc.math import sqrt as c_sqrt else: - from math import floor as c_floor, sqrt as c_sqrt, isnan as c_isnan + from math import floor as c_floor + from math import isnan as c_isnan + from math import sqrt as c_sqrt @cython.cfunc @@ -137,9 +141,9 @@ def _find_closest_in_3d( ) -> cython.int: """Find up to max_cands closest candidates by distance within a 3D box. - @cython.ccall rather than @cython.cfunc: track_kernels_tracking re-exports - this one, so it has to stay importable from Python while still being - C-callable from _find_closest_in_3d_grid's small-frame fallback. + @cython.ccall rather than @cython.cfunc: this one has to stay importable + from Python (tests import it) while still being C-callable from + _find_closest_in_3d_grid's small-frame fallback. """ s: cython.int k: cython.int @@ -202,6 +206,19 @@ def track3d_loop_fast( dz: cython.double, max_cands: cython.int, cold_start_gate: cython.double = 1.0, + # Prototype shared-observation (Level 1 only): + # share_tol > 0 enables recording (not claiming) of contested edges: + # when the cheapest remaining edge (i -> k) finds k already taken AND + # both this edge and the winner's edge cost less than share_tol, the + # pair is recorded into shared_i/shared_k (capacity len(shared_i)) + # instead of stealing. The driver materializes each pair as a virtual + # carrier particle (shared coordinates, own history) so the track + # continues with an honest velocity. Cold levels never share: a track + # with no history of its own must not twin. + share_tol: cython.double = 0.0, + shared_count: cython.int[:] = None, + shared_i: cython.int[:] = None, + shared_k: cython.int[:] = None, ): """Full track3d loop (3 levels) — single compiled entry. @@ -254,9 +271,27 @@ def track3d_loop_fast( oi: cython.int e: cython.int order: cython.int[:] + use_share: cython.bint + share_cap: cython.int + w: cython.int count1 = 0 np2 = num_parts_2 + use_share = ( + share_tol > 0.0 + and shared_count is not None + and shared_i is not None + and shared_k is not None + ) + share_cap = shared_i.shape[0] if use_share else 0 + + claim_cost_2: cython.double[:] + if use_share: + # Allocated only when sharing is enabled: the disabled path keeps + # its original allocation profile exactly (merge hygiene -- the + # prototype allocated this unconditionally on every call). + _claim_cost_2 = np.full(np2 if np2 > 0 else 1, np.inf, dtype=np.float64) + claim_cost_2 = _claim_cost_2 _cand_inds = np.empty(max_cands, dtype=np.int32) _cand_dists = np.empty(max_cands, dtype=np.float64) @@ -370,7 +405,34 @@ def track3d_loop_fast( if path_next_1[i] < 0 and path_prev_2[k] < 0: path_next_1[i] = k path_prev_2[k] = i + if use_share: + claim_cost_2[k] = edge_cost[e] count1 += 1 + elif ( + use_share + and path_next_1[i] < 0 + and path_prev_2[k] >= 0 + and edge_cost[e] < share_tol + and claim_cost_2[k] < share_tol + and shared_count[0] < share_cap + ): + # Prototype shared-observation (Level 1 only): contested and + # mutually well-predicted -> share, don't steal. Cold levels + # never share: a track with no history of its own must not + # twin. A sharer that later claims normally keeps the claim; + # its share record dies below (no forks). + shared_i[shared_count[0]] = i + shared_k[shared_count[0]] = k + shared_count[0] += 1 + + if use_share: + w = 0 + for e in range(shared_count[0]): + if path_next_1[shared_i[e]] < 0: + shared_i[w] = shared_i[e] + shared_k[w] = shared_k[e] + w += 1 + shared_count[0] = w # ===== Level 2: No previous link, neighbor velocity ===== n_edges = 0 diff --git a/src/openptv2/algorithms/track_kernels_tracking.py b/src/openptv2/algorithms/track_kernels_tracking.py deleted file mode 100644 index 0dfc50e1..00000000 --- a/src/openptv2/algorithms/track_kernels_tracking.py +++ /dev/null @@ -1,38 +0,0 @@ -# ruff: noqa: E402 -"""Compatibility re-export — content split into focused sub-modules 2026-07-10.""" - -# These mirror the cython.declare() C-level constants in track_kernels_corr, -# which are not importable from Python when compiled. -PT_UNUSED = -999 -POSI_K = 80 -MAX_CANDS_K = 32 -TR_UNUSED_K = -1 # noqa: E702 -CORRES_NONE_K = -1 -PREV_NONE_K = -1 -NEXT_NONE_K = -2 # noqa: E702 -COORD_UNUSED_K = -1e10 -ADD_PART_K = 3.0 # noqa: E702 - -from .track_kernels_corr import ( # noqa: F401, E402 - trackback_loop_fast, - trackcorr_loop_fast, -) -from .track_kernels_geom import _angle_acc_out, _ray_tracing_out # noqa: F401 -from .track_kernels_pixel import ( # noqa: F401 - _candsearch_in_pix_rest_nogil, - _dist_to_flat_out, - _multimed_r_nlay_1layer, - _pixel_to_metric_out, - _point_to_pixel_out, - _sorted_candidates_fast_out_nogil, - candsearch_in_pix_fast_nogil, -) -from .track_kernels_position import ( # noqa: F401 - _point_position_out, - assess_new_position_fast_nogil, -) -from .track_kernels_track3d import ( # noqa: F401 - _find_closest_in_3d, - track3d_loop_fast, - track4be_loop_fast, -) diff --git a/src/openptv2/algorithms/track_kernels_transform.py b/src/openptv2/algorithms/track_kernels_transform.py index 2c81b607..bd897238 100644 --- a/src/openptv2/algorithms/track_kernels_transform.py +++ b/src/openptv2/algorithms/track_kernels_transform.py @@ -8,50 +8,25 @@ import numpy as np if cython.compiled: - from cython.cimports.libc.math import ( - asin as c_asin, - ) - from cython.cimports.libc.math import ( - atan as c_atan, - ) - from cython.cimports.libc.math import ( - cos as c_cos, - ) - from cython.cimports.libc.math import ( - sin as c_sin, - ) - from cython.cimports.libc.math import ( - sqrt as c_sqrt, - ) - from cython.cimports.libc.math import ( - tan as c_tan, - ) + pass else: - from math import ( - asin as c_asin, - ) - from math import ( - atan as c_atan, - ) - from math import ( - cos as c_cos, - ) - from math import ( - sin as c_sin, - ) - from math import ( - sqrt as c_sqrt, - ) - from math import ( - tan as c_tan, - ) + pass _M_PI: cython.double = 3.141592653589793 -from .track_kernels_geom import ( - _point_to_pixel_out, -) +if cython.compiled: + from cython.cimports.openptv2.algorithms.track_kernels_pixel import ( + _dist_to_flat_out, + _pixel_to_metric_out, + _point_to_pixel_out, + ) +else: + from .track_kernels_pixel import ( + _dist_to_flat_out, + _pixel_to_metric_out, + _point_to_pixel_out, + ) from .track_kernels_search import ( candsearch_in_pix_rest_fast, ) @@ -65,655 +40,6 @@ COORD_UNUSED = -1e10 -@cython.ccall -@cython.nogil -def _multimed_r_nlay_1layer( - pos_x: cython.double, - pos_y: cython.double, - pos_z: cython.double, - ext_x0: cython.double, - ext_y0: cython.double, - ext_z0: cython.double, - mm_n1: cython.double, - mm_n2_0: cython.double, - mm_n3: cython.double, - mm_d0: cython.double, -) -> cython.double: - """Single-layer iterative radial shift.""" - zout: cython.double - dx: cython.double - dy: cython.double - r: cython.double - rq: cython.double - it: cython.int - denom: cython.double - beta1: cython.double - sin_beta1: cython.double - arg: cython.double - beta2_0: cython.double - arg3: cython.double - beta3: cython.double - rbeta: cython.double - rdiff: cython.double - if mm_n1 == 1.0 and mm_n2_0 == 1.0 and mm_n3 == 1.0: - return 1.0 - - zout = pos_z - dx = pos_x - ext_x0 - dy = pos_y - ext_y0 - r = c_sqrt(dx * dx + dy * dy) - rq = r - - for it in range(40): - denom = ext_z0 - pos_z - if denom == 0.0: - return 1.0 - beta1 = c_atan(rq / denom) - sin_beta1 = c_sin(beta1) - - arg = sin_beta1 * mm_n1 / mm_n2_0 - if arg > 1.0: - arg = 1.0 - elif arg < -1.0: - arg = -1.0 - beta2_0 = c_asin(arg) - - arg3 = sin_beta1 * mm_n1 / mm_n3 - if arg3 > 1.0: - arg3 = 1.0 - elif arg3 < -1.0: - arg3 = -1.0 - beta3 = c_asin(arg3) - - rbeta = ( - (ext_z0 - mm_d0) * c_tan(beta1) - + mm_d0 * c_tan(beta2_0) - - zout * c_tan(beta3) - ) - - rdiff = r - rbeta - rq += rdiff - - if abs(rdiff) < 0.001: - break - else: - return 1.0 - - if r != 0.0: - return rq / r - return 1.0 - - -@cython.ccall -@cython.inline -@cython.boundscheck(False) -@cython.wraparound(False) -@cython.cdivision(True) -@cython.profile(False) -@cython.nogil -def _ray_tracing_out( - x: cython.double, - y: cython.double, - cal: cython.double[:], - out: cython.double[:], -) -> cython.int: - """Write ray tracing results into out[0:6] — no tuple creation.""" - ext_x0: cython.double - ext_y0: cython.double - ext_z0: cython.double - dm00: cython.double - dm10: cython.double - dm20: cython.double - dm01: cython.double - dm11: cython.double - dm21: cython.double - dm02: cython.double - dm12: cython.double - dm22: cython.double - int_cc: cython.double - gx: cython.double - gy: cython.double - gz: cython.double - mm_n1: cython.double - mm_n2_0: cython.double - mm_n3: cython.double - mm_d0: cython.double - t0: cython.double - t1: cython.double - t2: cython.double - tn: cython.double - sd0: cython.double - sd1: cython.double - sd2: cython.double - gn: cython.double - gd0: cython.double - gd1: cython.double - gd2: cython.double - c: cython.double - dcg: cython.double - denom: cython.double - d1: cython.double - Xb0: cython.double - Xb1: cython.double - Xb2: cython.double - n: cython.double - bp0: cython.double - bp1: cython.double - bp2: cython.double - bpn: cython.double - p: cython.double - n_glass: cython.double - a2_0: cython.double - a2_1: cython.double - a2_2: cython.double - d2_denom: cython.double - d2: cython.double - Xx: cython.double - Xy: cython.double - Xz: cython.double - n_a2: cython.double - p2: cython.double - n_final: cython.double - ox: cython.double - oy: cython.double - oz: cython.double - ext_x0 = cal[0] - ext_y0 = cal[1] - ext_z0 = cal[2] - dm00 = cal[3] - dm10 = cal[4] - dm20 = cal[5] - dm01 = cal[6] - dm11 = cal[7] - dm21 = cal[8] - dm02 = cal[9] - dm12 = cal[10] - dm22 = cal[11] - int_cc = cal[12] - gx = cal[15] - gy = cal[16] - gz = cal[17] - mm_n1 = cal[20] - mm_n2_0 = cal[21] - mm_n3 = cal[22] - mm_d0 = cal[23] - - t0 = x - t1 = y - t2 = -int_cc - tn = c_sqrt(t0 * t0 + t1 * t1 + t2 * t2) - if tn > 0.0: - t0 /= tn - t1 /= tn - t2 /= tn - - sd0 = dm00 * t0 + dm01 * t1 + dm02 * t2 - sd1 = dm10 * t0 + dm11 * t1 + dm12 * t2 - sd2 = dm20 * t0 + dm21 * t1 + dm22 * t2 - - gn = c_sqrt(gx * gx + gy * gy + gz * gz) - if gn > 0.0: - gd0 = gx / gn - gd1 = gy / gn - gd2 = gz / gn - else: - gd0 = 0.0 - gd1 = 0.0 - gd2 = 0.0 - c = gn + mm_d0 - - dcg = gd0 * ext_x0 + gd1 * ext_y0 + gd2 * ext_z0 - c - denom = gd0 * sd0 + gd1 * sd1 + gd2 * sd2 - d1 = -dcg / denom - - Xb0 = ext_x0 + sd0 * d1 - Xb1 = ext_y0 + sd1 * d1 - Xb2 = ext_z0 + sd2 * d1 - - n = sd0 * gd0 + sd1 * gd1 + sd2 * gd2 - bp0 = sd0 - gd0 * n - bp1 = sd1 - gd1 * n - bp2 = sd2 - gd2 * n - bpn = c_sqrt(bp0 * bp0 + bp1 * bp1 + bp2 * bp2) - if bpn > 0.0: - bp0 /= bpn - bp1 /= bpn - bp2 /= bpn - - p = c_sqrt(1.0 - n * n) * mm_n1 / mm_n2_0 - n_glass = c_sqrt(1.0 - p * p) if n >= 0 else -c_sqrt(1.0 - p * p) - - a2_0 = bp0 * p + gd0 * n_glass - a2_1 = bp1 * p + gd1 * n_glass - a2_2 = bp2 * p + gd2 * n_glass - - d2_denom = gd0 * a2_0 + gd1 * a2_1 + gd2 * a2_2 - d2 = mm_d0 / abs(d2_denom) - - Xx = Xb0 + a2_0 * d2 - Xy = Xb1 + a2_1 * d2 - Xz = Xb2 + a2_2 * d2 - - n_a2 = a2_0 * gd0 + a2_1 * gd1 + a2_2 * gd2 - bp0 = a2_0 - gd0 * n_glass - bp1 = a2_1 - gd1 * n_glass - bp2 = a2_2 - gd2 * n_glass - bpn = c_sqrt(bp0 * bp0 + bp1 * bp1 + bp2 * bp2) - if bpn > 0.0: - bp0 /= bpn - bp1 /= bpn - bp2 /= bpn - - p2 = c_sqrt(1.0 - n_a2 * n_a2) * mm_n2_0 / mm_n3 - n_final = c_sqrt(1.0 - p2 * p2) if n_a2 >= 0 else -c_sqrt(1.0 - p2 * p2) - - ox = bp0 * p2 + gd0 * n_final - oy = bp1 * p2 + gd1 * n_final - oz = bp2 * p2 + gd2 * n_final - - out[0] = Xx - out[1] = Xy - out[2] = Xz - out[3] = ox - out[4] = oy - out[5] = oz - return 0 - - -@cython.ccall -@cython.boundscheck(False) -@cython.wraparound(False) -@cython.nogil -def _point_position_out( - targets: cython.double[:, ::1], - num_cams: cython.int, - cal_arr: cython.double[:, ::1], - out: cython.double[:], - scratch_ray: cython.double[:], -) -> cython.double: - """Internal — writes 3D position to out[0:3], returns avg_dist (scalar). - - Pure C entry — zero Python object creation, zero tuple overhead. - """ - cam: cython.int - pair: cython.int - tx: cython.double - ty: cython.double - Xx: cython.double - Xy: cython.double - Xz: cython.double - ox: cython.double - oy: cython.double - oz: cython.double - dtot: cython.double - num_used: cython.int - px: cython.double - py: cython.double - pz: cython.double - v1x: cython.double - v1y: cython.double - v1z: cython.double - d1x: cython.double - d1y: cython.double - d1z: cython.double - v2x: cython.double - v2y: cython.double - v2z: cython.double - d2x: cython.double - d2y: cython.double - d2z: cython.double - sp0: cython.double - sp1: cython.double - sp2: cython.double - pb0: cython.double - pb1: cython.double - pb2: cython.double - scale: cython.double - dist: cython.double - mx: cython.double - my: cython.double - mz: cython.double - t0: cython.double - t1: cython.double - t2: cython.double - s1: cython.double - on1x: cython.double - on1y: cython.double - on1z: cython.double - s2: cython.double - on2x: cython.double - on2y: cython.double - on2z: cython.double - ddx: cython.double - ddy: cython.double - ddz: cython.double - verts_x: cython.double[:] - verts_y: cython.double[:] - verts_z: cython.double[:] - dirs_x: cython.double[:] - dirs_y: cython.double[:] - dirs_z: cython.double[:] - valid: cython.int[:] - _vi: cython.int - with cython.gil: - _verts_x_buf = np.zeros(8, dtype=np.float64) - verts_x = _verts_x_buf - _verts_y_buf = np.zeros(8, dtype=np.float64) - verts_y = _verts_y_buf - _verts_z_buf = np.zeros(8, dtype=np.float64) - verts_z = _verts_z_buf - _dirs_x_buf = np.zeros(8, dtype=np.float64) - dirs_x = _dirs_x_buf - _dirs_y_buf = np.zeros(8, dtype=np.float64) - dirs_y = _dirs_y_buf - _dirs_z_buf = np.zeros(8, dtype=np.float64) - dirs_z = _dirs_z_buf - _valid_buf = np.zeros(8, dtype=np.int32) - valid = _valid_buf - - for _vi in range(8): - valid[_vi] = 0 - - for cam in range(num_cams): - tx = targets[cam, 0] - ty = targets[cam, 1] - if tx == COORD_UNUSED: - continue - _ray_tracing_out(tx, ty, cal_arr[cam], scratch_ray) - verts_x[cam] = scratch_ray[0] - verts_y[cam] = scratch_ray[1] - verts_z[cam] = scratch_ray[2] - dirs_x[cam] = scratch_ray[3] - dirs_y[cam] = scratch_ray[4] - dirs_z[cam] = scratch_ray[5] - valid[cam] = 1 - - dtot = 0.0 - num_used = 0 - px = 0.0 - py = 0.0 - pz = 0.0 - - for cam in range(num_cams): - if valid[cam] == 0: - continue - for pair in range(cam + 1, num_cams): - if valid[pair] == 0: - continue - - v1x = verts_x[cam] - v1y = verts_y[cam] - v1z = verts_z[cam] - d1x = dirs_x[cam] - d1y = dirs_y[cam] - d1z = dirs_z[cam] - v2x = verts_x[pair] - v2y = verts_y[pair] - v2z = verts_z[pair] - d2x = dirs_x[pair] - d2y = dirs_y[pair] - d2z = dirs_z[pair] - - sp0 = v2x - v1x - sp1 = v2y - v1y - sp2 = v2z - v1z - - pb0 = d1y * d2z - d1z * d2y - pb1 = d1z * d2x - d1x * d2z - pb2 = d1x * d2y - d1y * d2x - scale = pb0 * pb0 + pb1 * pb1 + pb2 * pb2 - - if scale < 1e-20: - dist = c_sqrt(sp0 * sp0 + sp1 * sp1 + sp2 * sp2) - mx = (v1x + v2x) * 0.5 - my = (v1y + v2y) * 0.5 - mz = (v1z + v2z) * 0.5 - else: - t0 = sp1 * d2z - sp2 * d2y - t1 = sp2 * d2x - sp0 * d2z - t2 = sp0 * d2y - sp1 * d2x - s1 = (pb0 * t0 + pb1 * t1 + pb2 * t2) / scale - on1x = v1x + d1x * s1 - on1y = v1y + d1y * s1 - on1z = v1z + d1z * s1 - - t0 = sp1 * d1z - sp2 * d1y - t1 = sp2 * d1x - sp0 * d1z - t2 = sp0 * d1y - sp1 * d1x - s2 = (pb0 * t0 + pb1 * t1 + pb2 * t2) / scale - on2x = v2x + d2x * s2 - on2y = v2y + d2y * s2 - on2z = v2z + d2z * s2 - - ddx = on1x - on2x - ddy = on1y - on2y - ddz = on1z - on2z - dist = c_sqrt(ddx * ddx + ddy * ddy + ddz * ddz) - mx = (on1x + on2x) * 0.5 - my = (on1y + on2y) * 0.5 - mz = (on1z + on2z) * 0.5 - - num_used += 1 - dtot += dist - px += mx - py += my - pz += mz - - if num_used > 0: - inv = 1.0 / num_used - out[0] = px * inv - out[1] = py * inv - out[2] = pz * inv - return dtot * inv - else: - out[0] = 0.0 - out[1] = 0.0 - out[2] = 0.0 - return 0.0 - - -@cython.ccall -def point_position_fast( - targets: cython.double[:, ::1], num_cams: cython.int, cal_arr: cython.double[:, ::1] -): - """Compute 3D position from multiple camera rays. - - Returns: - (pos, avg_dist) — (3,) float64 position and average ray distance. - """ - pos = np.zeros(3, dtype=np.float64) - pos_mv: cython.double[:] = pos - scratch_ray = np.zeros(6, dtype=np.float64) - dtot = _point_position_out(targets, num_cams, cal_arr, pos_mv, scratch_ray) - return pos, dtot - - -@cython.ccall -def pixel_to_metric_fast( - x_pixel: cython.double, - y_pixel: cython.double, - imx: cython.int, - imy: cython.int, - pix_x: cython.double, - pix_y: cython.double, - chfield: cython.int, -): - """Convert pixel to metric coordinates.""" - yp: cython.double - x_metric: cython.double - y_metric: cython.double - yp = y_pixel - if chfield == 1: - yp = 2.0 * yp + 1.0 - elif chfield == 2: - yp = 2.0 * yp - x_metric = (x_pixel - imx * 0.5) * pix_x - y_metric = (imy * 0.5 - yp) * pix_y - return x_metric, y_metric - - -@cython.ccall -@cython.inline -@cython.cdivision(True) -@cython.profile(False) -@cython.nogil -def _pixel_to_metric_out( - x_pixel: cython.double, - y_pixel: cython.double, - imx: cython.int, - imy: cython.int, - pix_x: cython.double, - pix_y: cython.double, - chfield: cython.int, - out: cython.double[:], -) -> cython.int: - """Write pixel-to-metric coords to out[0], out[1].""" - yp: cython.double - yp = y_pixel - if chfield == 1: - yp = 2.0 * yp + 1.0 - elif chfield == 2: - yp = 2.0 * yp - out[0] = (x_pixel - imx * 0.5) * pix_x - out[1] = (imy * 0.5 - yp) * pix_y - return 0 - - -@cython.ccall -@cython.boundscheck(False) -@cython.wraparound(False) -def dist_to_flat_fast( - dist_x: cython.double, - dist_y: cython.double, - xh: cython.double, - yh: cython.double, - k1: cython.double, - k2: cython.double, - k3: cython.double, - p1: cython.double, - p2: cython.double, - scx: cython.double, - she: cython.double, - tol: cython.double, -): - """Inverse Brown distortion.""" - r_init: cython.double - sin_she: cython.double - cos_she: cython.double - inv_scx: cython.double - xq: cython.double - yq: cython.double - _: cython.int - r2: cython.double - r4: cython.double - r6: cython.double - radial_factor: cython.double - dx: cython.double - dy: cython.double - xq_new: cython.double - yq_new: cython.double - dx_change: cython.double - dy_change: cython.double - r_init = c_sqrt(dist_x * dist_x + dist_y * dist_y) - if r_init < 1e-10: - return -xh, -yh - - sin_she = c_sin(she) - cos_she = c_cos(she) - inv_scx = 1.0 / scx - - xq = (dist_x + dist_y * sin_she) * inv_scx - yq = dist_y / cos_she - - for _ in range(50): - r2 = xq * xq + yq * yq - r4 = r2 * r2 - r6 = r4 * r2 - - radial_factor = k1 * r2 + k2 * r4 + k3 * r6 - - dx = xq * radial_factor + p1 * (r2 + 2.0 * xq * xq) + 2.0 * p2 * xq * yq - dy = yq * radial_factor + p2 * (r2 + 2.0 * yq * yq) + 2.0 * p1 * xq * yq - - xq_new = (dist_x + dist_y * sin_she) * inv_scx - dx - yq_new = dist_y / cos_she - dy - - dx_change = xq_new - xq - dy_change = yq_new - yq - - xq += 0.5 * dx_change - yq += 0.5 * dy_change - - if c_sqrt(dx_change * dx_change + dy_change * dy_change) < tol: - break - - return xq - xh, yq - yh - - -@cython.cfunc -@cython.inline -@cython.boundscheck(False) -@cython.wraparound(False) -@cython.cdivision(True) -@cython.profile(False) -@cython.nogil -def _dist_to_flat_out( - dist_x: cython.double, - dist_y: cython.double, - xh: cython.double, - yh: cython.double, - k1: cython.double, - k2: cython.double, - k3: cython.double, - p1: cython.double, - p2: cython.double, - scx: cython.double, - she: cython.double, - tol: cython.double, - out: cython.double[:], -) -> cython.int: - """Write dist-to-flat coords to out[0], out[1].""" - r_init: cython.double = c_sqrt(dist_x * dist_x + dist_y * dist_y) - if r_init < 1e-10: - out[0] = -xh - out[1] = -yh - return 0 - sin_she: cython.double = c_sin(she) - cos_she: cython.double = c_cos(she) - inv_scx: cython.double = 1.0 / scx - xq: cython.double = (dist_x + dist_y * sin_she) * inv_scx - yq: cython.double = dist_y / cos_she - _: cython.int - r2: cython.double - r4: cython.double - r6: cython.double - radial_factor: cython.double - dx: cython.double - dy: cython.double - xq_new: cython.double - yq_new: cython.double - dx_change: cython.double - dy_change: cython.double - for _ in range(50): - r2 = xq * xq + yq * yq - r4 = r2 * r2 - r6 = r4 * r2 - radial_factor = k1 * r2 + k2 * r4 + k3 * r6 - dx = xq * radial_factor + p1 * (r2 + 2.0 * xq * xq) + 2.0 * p2 * xq * yq - dy = yq * radial_factor + p2 * (r2 + 2.0 * yq * yq) + 2.0 * p1 * xq * yq - xq_new = (dist_x + dist_y * sin_she) * inv_scx - dx - yq_new = dist_y / cos_she - dy - dx_change = xq_new - xq - dy_change = yq_new - yq - xq += 0.5 * dx_change - yq += 0.5 * dy_change - if c_sqrt(dx_change * dx_change + dy_change * dy_change) < tol: - break - out[0] = xq - xh - out[1] = yq - yh - return 0 - - @cython.ccall @cython.boundscheck(False) @cython.wraparound(False) @@ -875,204 +201,6 @@ def assess_new_position_fast( return targ_pos, cand_inds, valid_cams -@cython.ccall -@cython.boundscheck(False) -@cython.wraparound(False) -@cython.nogil -def _candsearch_in_pix_rest_nogil( - targ_x: cython.double[:], - targ_y: cython.double[:], - targ_tnr: cython.int[:], - num_targets: cython.int, - cent_x: cython.double, - cent_y: cython.double, - dl: cython.double, - dr: cython.double, - du: cython.double, - dd: cython.double, - imx: cython.double, - imy: cython.double, - tr_unused: cython.int, -) -> cython.int: - """Find closest unused candidate GIL-free.""" - xmin: cython.double - xmax: cython.double - ymin: cython.double - ymax: cython.double - best: cython.int - dmin: cython.double - j0: cython.int - dj: cython.int - j: cython.int - ty: cython.double - tx: cython.double - dx: cython.double - dy: cython.double - d: cython.double - xmin = cent_x - dl - xmax = cent_x + dr - ymin = cent_y - du - ymax = cent_y + dd - - if xmin < 0.0: - xmin = 0.0 - if xmax > imx: - xmax = imx - if ymin < 0.0: - ymin = 0.0 - if ymax > imy: - ymax = imy - - best = tr_unused - dmin = 1e20 - - if not (0.0 <= cent_x <= imx and 0.0 <= cent_y <= imy): - return best - - j0 = num_targets // 2 - dj = num_targets // 4 - while dj > 1: - if targ_y[j0] < ymin: - j0 += dj - else: - j0 -= dj - dj //= 2 - - j0 -= 12 - if j0 < 0: - j0 = 0 - - for j in range(j0, num_targets): - ty = targ_y[j] - if targ_tnr[j] == tr_unused: - if ty > ymax: - break - tx = targ_x[j] - if tx > xmin and tx < xmax and ty > ymin and ty < ymax: - dx = cent_x - tx - dy = cent_y - ty - d = c_sqrt(dx * dx + dy * dy) - if d < dmin: - dmin = d - best = j - - return best - - -@cython.ccall -@cython.boundscheck(False) -@cython.wraparound(False) -@cython.nogil -def assess_new_position_fast_nogil( - pos: cython.double[:], - num_cams: cython.int, - add_part: cython.double, - cal_arr: cython.double[:, ::1], - mo_arr: cython.double[:, ::1], - mnr_arr: cython.int[:], - mnz_arr: cython.int[:], - mrw_arr: cython.double[:], - targ_x: cython.double[:, ::1], - targ_y: cython.double[:, ::1], - targ_tnr: cython.int[:, ::1], - num_targets: cython.int[:], - imx_half: cython.double, - imy_half: cython.double, - inv_pix_x: cython.double, - inv_pix_y: cython.double, - chfield: cython.int, - imx: cython.int, - imy: cython.int, - pix_x: cython.double, - pix_y: cython.double, - flatten_tol: cython.double, - tr_unused: cython.int, - coord_unused: cython.double, - proj_x: cython.double[:], - proj_y: cython.double[:], - targ_pos_out: cython.double[:, :], - cand_inds_out: cython.int[:], - scratch: cython.double[:], -) -> cython.int: - """Assess new position GIL-free. Assumes use_proj=True.""" - cam: cython.int - valid_cams: cython.int - best: cython.int - px: cython.double - py: cython.double - mx: cython.double - my: cython.double - - for cam in range(num_cams): - cand_inds_out[cam] = tr_unused - targ_pos_out[cam, 0] = coord_unused - targ_pos_out[cam, 1] = coord_unused - - for cam in range(num_cams): - px = proj_x[cam] - py = proj_y[cam] - - best = _candsearch_in_pix_rest_nogil( - targ_x[cam], - targ_y[cam], - targ_tnr[cam], - num_targets[cam], - px, - py, - add_part, - add_part, - add_part, - add_part, - imx, - imy, - tr_unused, - ) - - if best != tr_unused: - cand_inds_out[cam] = best - targ_pos_out[cam, 0] = targ_x[cam, best] - targ_pos_out[cam, 1] = targ_y[cam, best] - - valid_cams = 0 - for cam in range(num_cams): - if targ_pos_out[cam, 0] != coord_unused: - _pixel_to_metric_out( - targ_pos_out[cam, 0], - targ_pos_out[cam, 1], - imx, - imy, - pix_x, - pix_y, - chfield, - scratch, - ) - mx = scratch[0] - my = scratch[1] - - cal = cal_arr[cam] - _dist_to_flat_out( - mx, - my, - cal[13], - cal[14], - cal[24], - cal[25], - cal[26], - cal[27], - cal[28], - cal[29], - cal[30], - flatten_tol, - scratch, - ) - - targ_pos_out[cam, 0] = scratch[0] - targ_pos_out[cam, 1] = scratch[1] - valid_cams += 1 - - return valid_cams - - POSI_K = 80 MAX_CANDS_K = 32 TR_UNUSED_K = -1 @@ -1082,368 +210,3 @@ def assess_new_position_fast_nogil( COORD_UNUSED_K = -1e10 ADD_PART_K = 3.0 - -@cython.ccall -@cython.boundscheck(False) -@cython.wraparound(False) -def metric_to_pixel_fast( - x_metric: cython.double, - y_metric: cython.double, - imx: cython.int, - imy: cython.int, - pix_x: cython.double, - pix_y: cython.double, - chfield: cython.int, -): - """Convert metric to pixel coordinates.""" - x_pixel: cython.double - y_pixel: cython.double - x_pixel = x_metric / pix_x + imx * 0.5 - y_pixel = imy * 0.5 - y_metric / pix_y - if chfield == 1: - y_pixel = (y_pixel - 1.0) * 0.5 - elif chfield == 2: - y_pixel = y_pixel * 0.5 - return x_pixel, y_pixel - - -@cython.ccall -@cython.inline -@cython.cdivision(True) -@cython.profile(False) -def _metric_to_pixel_out( - x_metric: cython.double, - y_metric: cython.double, - imx: cython.int, - imy: cython.int, - pix_x: cython.double, - pix_y: cython.double, - chfield: cython.int, - out: cython.double[:], -): - """Write metric-to-pixel coords to out[0], out[1].""" - x_pixel: cython.double = x_metric / pix_x + imx * 0.5 - y_pixel: cython.double = imy * 0.5 - y_metric / pix_y - if chfield == 1: - y_pixel = (y_pixel - 1.0) * 0.5 - elif chfield == 2: - y_pixel = y_pixel * 0.5 - out[0] = x_pixel - out[1] = y_pixel - - -@cython.boundscheck(False) -@cython.wraparound(False) -def _flat_image_coord_fast( - pos: cython.double[:], - cal: cython.double[:], - mmlut_data: cython.double[:], - mmlut_origin: cython.double[:], - mmlut_nr: cython.int, - mmlut_nz: cython.int, - mmlut_rw: cython.double, -): - """Project 3D to flat metric image coordinates. - - Returns (x, y) without distortion or pixel conversion. - """ - pos0: cython.double - pos1: cython.double - pos2: cython.double - ext_x0: cython.double - ext_y0: cython.double - ext_z0: cython.double - dm00: cython.double - dm10: cython.double - dm20: cython.double - dm01: cython.double - dm11: cython.double - dm21: cython.double - dm02: cython.double - dm12: cython.double - dm22: cython.double - int_cc: cython.double - gx: cython.double - gy: cython.double - gz: cython.double - inv_dog: cython.double - mm_n1: cython.double - mm_n2_0: cython.double - mm_n3: cython.double - mm_d0: cython.double - dot_cam: cython.double - dist_o_glas: cython.double - dist_cam_glas: cython.double - dot_pos: cython.double - dist_point_glas: cython.double - s_cam: cython.double - cc_x: cython.double - cc_y: cython.double - cc_z: cython.double - s_pt: cython.double - cp_x: cython.double - cp_y: cython.double - cp_z: cython.double - ext_t_z0: cython.double - s_d: cython.double - ag_x: cython.double - ag_y: cython.double - ag_z: cython.double - tmp_x: cython.double - tmp_y: cython.double - tmp_z: cython.double - pos_t_0: cython.double - pos_t_2: cython.double - radial_shift: cython.double - has_mmlut: cython.bint - tx: cython.double - ty: cython.double - tz: cython.double - sz: cython.double - iz: cython.int - R: cython.double - sr: cython.double - ir: cython.int - v0: cython.int - v3: cython.int - mmf: cython.double - X_t: cython.double - s_z: cython.double - bx: cython.double - by: cython.double - bz: cython.double - s_x: cython.double - dx: cython.double - dy: cython.double - dz: cython.double - deno: cython.double - x: cython.double - y: cython.double - pos0 = pos[0] - pos1 = pos[1] - pos2 = pos[2] - - ext_x0 = cal[0] - ext_y0 = cal[1] - ext_z0 = cal[2] - dm00 = cal[3] - dm10 = cal[4] - dm20 = cal[5] - dm01 = cal[6] - dm11 = cal[7] - dm21 = cal[8] - dm02 = cal[9] - dm12 = cal[10] - dm22 = cal[11] - int_cc = cal[12] - gx = cal[15] - gy = cal[16] - gz = cal[17] - inv_dog = cal[19] - mm_n1 = cal[20] - mm_n2_0 = cal[21] - mm_n3 = cal[22] - mm_d0 = cal[23] - - dot_cam = ext_x0 * gx + ext_y0 * gy + ext_z0 * gz - dist_o_glas = cal[18] - dist_cam_glas = dot_cam * inv_dog - dist_o_glas - mm_d0 - - dot_pos = pos0 * gx + pos1 * gy + pos2 * gz - dist_point_glas = dot_pos * inv_dog - dist_o_glas - - s_cam = dist_cam_glas * inv_dog - cc_x = ext_x0 - gx * s_cam - cc_y = ext_y0 - gy * s_cam - cc_z = ext_z0 - gz * s_cam - - s_pt = dist_point_glas * inv_dog - cp_x = pos0 - gx * s_pt - cp_y = pos1 - gy * s_pt - cp_z = pos2 - gz * s_pt - - ext_t_z0 = dist_cam_glas + mm_d0 - - s_d = mm_d0 * inv_dog - ag_x = cc_x - gx * s_d - ag_y = cc_y - gy * s_d - ag_z = cc_z - gz * s_d - tmp_x = cp_x - ag_x - tmp_y = cp_y - ag_y - tmp_z = cp_z - ag_z - - pos_t_0 = c_sqrt(tmp_x * tmp_x + tmp_y * tmp_y + tmp_z * tmp_z) - pos_t_2 = dist_point_glas - - radial_shift = 1.0 - has_mmlut = len(mmlut_data) > 0 - if has_mmlut: - tx = pos_t_0 - mmlut_origin[0] - ty = -mmlut_origin[1] - tz = pos_t_2 - mmlut_origin[2] - sz = tz / mmlut_rw - iz = int(sz) - sz -= iz - R = c_sqrt(tx * tx + ty * ty) - sr = R / mmlut_rw - ir = int(sr) - sr -= ir - if ir <= mmlut_nr and iz >= 0 and iz <= mmlut_nz: - v0 = ir * mmlut_nz + iz - v3 = v0 + mmlut_nz + 1 - if v0 >= 0 and v3 <= mmlut_nr * mmlut_nz: - mmf = ( - mmlut_data[v0] * (1.0 - sr) * (1.0 - sz) - + mmlut_data[v0 + 1] * (1.0 - sr) * sz - + mmlut_data[v0 + mmlut_nz] * sr * (1.0 - sz) - + mmlut_data[v3] * sr * sz - ) - if mmf > 0.0: - radial_shift = mmf - if radial_shift == 1.0: - radial_shift = _multimed_r_nlay_1layer( - pos_t_0, - 0.0, - pos_t_2, - 0.0, - 0.0, - ext_t_z0, - mm_n1, - mm_n2_0, - mm_n3, - mm_d0, - ) - X_t = pos_t_0 * radial_shift - - s_z = -pos_t_2 * inv_dog - bx = ag_x - gx * s_z - by = ag_y - gy * s_z - bz = ag_z - gz * s_z - if pos_t_0 > 0.0: - s_x = -X_t / pos_t_0 - bx -= tmp_x * s_x - by -= tmp_y * s_x - bz -= tmp_z * s_x - - dx = bx - ext_x0 - dy = by - ext_y0 - dz = bz - ext_z0 - deno = dm02 * dx + dm12 * dy + dm22 * dz - x = -int_cc * (dm00 * dx + dm10 * dy + dm20 * dz) / deno - y = -int_cc * (dm01 * dx + dm11 * dy + dm21 * dz) / deno - - return x, y - - -@cython.boundscheck(False) -@cython.wraparound(False) -def _img_coord_fast( - pos: cython.double[:], - cal: cython.double[:], - mmlut_data: cython.double[:], - mmlut_origin: cython.double[:], - mmlut_nr: cython.int, - mmlut_nz: cython.int, - mmlut_rw: cython.double, -): - """Project 3D to distorted metric image coordinates.""" - xh: cython.double - yh: cython.double - k1: cython.double - k2: cython.double - k3: cython.double - p1: cython.double - p2: cython.double - scx: cython.double - she: cython.double - x: cython.double - y: cython.double - r: cython.double - r2: cython.double - r4: cython.double - radial_factor: cython.double - xd: cython.double - yd: cython.double - sin_she: cython.double - cos_she: cython.double - x_dist: cython.double - y_dist: cython.double - x, y = _flat_image_coord_fast( - pos, cal, mmlut_data, mmlut_origin, mmlut_nr, mmlut_nz, mmlut_rw - ) - - xh = cal[13] - yh = cal[14] - k1 = cal[24] - k2 = cal[25] - k3 = cal[26] - p1 = cal[27] - p2 = cal[28] - scx = cal[29] - she = cal[30] - - x += xh - y += yh - r = c_sqrt(x * x + y * y) - if r < 1e-10: - return 0.0, 0.0 - - r2 = r * r - r4 = r2 * r2 - radial_factor = 1.0 + k1 * r2 + k2 * r4 + k3 * r4 * r2 - xd = x * radial_factor + p1 * (r2 + 2.0 * x * x) + 2.0 * p2 * x * y - yd = y * radial_factor + p2 * (r2 + 2.0 * y * y) + 2.0 * p1 * x * y - sin_she = c_sin(she) - cos_she = c_cos(she) - x_dist = scx * (xd - sin_she * yd) - y_dist = scx * cos_she * yd - - return x_dist, y_dist - - -@cython.ccall -@cython.boundscheck(False) -@cython.wraparound(False) -def img_coord_batch_fast( - positions: cython.double[:, ::1], - cal: cython.double[:], - mmlut_data: cython.double[:], - mmlut_origin: cython.double[:], - mmlut_nr: cython.int, - mmlut_nz: cython.int, - mmlut_rw: cython.double, -): - """Project N 3D positions to distorted metric coords.""" - n: cython.Py_ssize_t - i: cython.Py_ssize_t - n = positions.shape[0] - result = np.empty((n, 2), dtype=np.float64) - for i in range(n): - result[i, 0], result[i, 1] = _img_coord_fast( - positions[i], cal, mmlut_data, mmlut_origin, mmlut_nr, mmlut_nz, mmlut_rw - ) - return result - - -@cython.ccall -@cython.boundscheck(False) -@cython.wraparound(False) -def flat_image_coord_batch_fast( - positions: cython.double[:, ::1], - cal: cython.double[:], - mmlut_data: cython.double[:], - mmlut_origin: cython.double[:], - mmlut_nr: cython.int, - mmlut_nz: cython.int, - mmlut_rw: cython.double, -): - """Project N 3D positions to flat metric coords.""" - n: cython.Py_ssize_t - i: cython.Py_ssize_t - n = positions.shape[0] - result = np.empty((n, 2), dtype=np.float64) - for i in range(n): - result[i, 0], result[i, 1] = _flat_image_coord_fast( - positions[i], cal, mmlut_data, mmlut_origin, mmlut_nr, mmlut_nz, mmlut_rw - ) - return result diff --git a/src/openptv2/algorithms/tracking_frame_buf.py b/src/openptv2/algorithms/tracking_frame_buf.py index 7349d198..6b4ccfad 100644 --- a/src/openptv2/algorithms/tracking_frame_buf.py +++ b/src/openptv2/algorithms/tracking_frame_buf.py @@ -697,6 +697,7 @@ class Frame: targ_x = cython.declare(object, visibility="public") targ_y = cython.declare(object, visibility="public") targ_tnr = cython.declare(object, visibility="public") + targ_sumg = cython.declare(object, visibility="public") path_x = cython.declare(object, visibility="public") path_prev = cython.declare(object, visibility="public") path_next = cython.declare(object, visibility="public") @@ -722,6 +723,7 @@ def __init__(self, num_cams=4, max_targets=1000, **kwargs): self.targ_x = np.full((num_cams, max_targets), COORD_UNUSED, dtype=np.float64) self.targ_y = np.full((num_cams, max_targets), COORD_UNUSED, dtype=np.float64) self.targ_tnr = np.full((num_cams, max_targets), PT_UNUSED, dtype=np.int32) + self.targ_sumg = np.zeros((num_cams, max_targets), dtype=np.float64) # SoA for Pathinfo self.path_x = np.zeros((max_targets, 3), dtype=np.float64) @@ -949,11 +951,13 @@ def read(self, corres_file_base, linkage_file_base, *args, **kwargs): tx = self.targ_x[cam] ty = self.targ_y[cam] ttnr = self.targ_tnr[cam] + tsumg = self.targ_sumg[cam] for j, t in enumerate(targets): self.targets[cam][j] = t tx[j] = t.x ty[j] = t.y ttnr[j] = t.tnr + tsumg[j] = t.sumg return True diff --git a/src/openptv2/algorithms/tracking_run.py b/src/openptv2/algorithms/tracking_run.py index e2e6597e..ffbfb739 100644 --- a/src/openptv2/algorithms/tracking_run.py +++ b/src/openptv2/algorithms/tracking_run.py @@ -30,6 +30,23 @@ class TrackingRun: # frame written/read during tracking also goes through the unified store, # replacing the old OPENPTV_STORAGE env var. store: object = None + # Forward link resolution: 1 = openptv2 default (a particle that loses a + # contested candidate retries its remaining choices), 0 = original 3dptv + # track.c behaviour (the loser is dropped). See track_kernels_corr.py + # "Phase 3" and scripts/diag_conflict_parity.py. + loser_retry: int = 1 + # Cold-start prediction: 1 = openptv2 default (borrow a neighbour's + # velocity when the particle has no previous link), 0 = original 3dptv + # track.c behaviour (X2 = X1, zero-velocity guess). + cold_start_neighbour: int = 1 + # Appearance cost weight: multiplies the relative grey-sum difference + # between a particle's own targets and a candidate's targets, added to + # the rr cost. 0.0 = original 3dptv track.c behaviour (purely + # kinematic cost); track.c never used brightness in tracking. + app_weight: float = 0.0 + # Uniform-grid acceleration of the 2D candidate search (built once per + # step, reused by every particle). 0 = legacy y-band scan exactly. + use_grid: int = 0 def __post_init__(self): self.fb = FrameBuf( diff --git a/src/openptv2/plugins/two_phase_tracking.py b/src/openptv2/plugins/two_phase_tracking.py index b2c8c965..f6908175 100644 --- a/src/openptv2/plugins/two_phase_tracking.py +++ b/src/openptv2/plugins/two_phase_tracking.py @@ -36,12 +36,217 @@ class TwoPhaseTrackerConfig: leaf_weight : float Weight for 2D leaf distances in the cost matrix. If 0, falls back to pure 3D matching. + use_velocity : bool + Predict each track forward with its constant-velocity estimate and + match predictions (not current positions) against new detections. + Required to cross steady trajectories correctly; without it every + X-crossing resolves as a bounce. Default True. + cost_mode : str + "projected": leaf costs are evaluated at re-projected predicted + positions (needs project_fn) -- the benchmarked fix for + maneuver-at-crossing scenes. "3d": cost is the 3D distance between + prediction and candidate. Falls back to "3d" when no project_fn + is available. + allow_shared : bool + Prototype (shared-observation): in a contested component with more + tracks than candidates (detector undercount = occlusion), let the + losing tracks SHARE the winner's detection instead of dying. + Shared links update position but never velocity (each track's speed + comes only from its own points). Default False (legacy behaviour). + max_shared : int + Maximum consecutive shared frames per track before it must match + alone again. Default 2. + share_tol : float | None + Maximum edge cost (same units as the cost matrix) for a shared + claim. Sharing without it hijacks strangers: any unassigned track + inside the gate would co-opt a foreign detection (seen live: a + gap-stranded track shared two unrelated detections). None disables + the gate. Default 1.0. + max_group_size : int + Groups bigger than this skip the cubic Hungarian and fall back to + greedy claiming inside the group (sharing still applies). At + production density the frame percolates into giant components; + without the cap one frame stalls the run. Default 128. + confirm_tol : float | None + Two-hop confirmation (trackcorr's X4 lesson, ported): a consecutive + link is kept only if its successor continues within this velocity + kink (mm/frame, same scale as dacc) -- lies rarely confirm twice. + None disables (legacy behaviour). Default None. + confirm_ends : bool + Also sever consecutive links into dead ends (no onward link, not + the last frame): dying tracks grabbing strangers. Only meaningful + with confirm_tol set. Default False. + bidirectional : bool + Run forward tracking, backward tracking on reversed frames, and + merge the two sets (reciprocal-first core, non-conflicting links + added greedily by 3D distance). Closes ~75% of the accuracy gap + to 4-frame trackcorr on dense data in a fraction of the time. + Default False (unidirectional forward). + bwd_v_max : float | None + Optional search radius for the backward pass in bidirectional mode. + None defaults to v_max. Setting a slightly wider bwd_v_max (e.g. 2.5 + when v_max=2.0) allows backward tracking to reach fast particles that + forward missed, safely protected by the Forward-First lock. """ v_max: float = 5.0 max_gap: int = 2 dt: float = 1.0 leaf_weight: float = 1.0 + use_velocity: bool = True + cost_mode: str = "projected" + allow_shared: bool = False + max_shared: int = 2 + share_tol: float | None = 1.0 + max_group_size: int = 128 + confirm_tol: float | None = None + confirm_ends: bool = False + bidirectional: bool = False + bwd_v_max: float | None = None + + +def _confirm_links( + links: list[tuple[int, int, int, int]], + frame_particles: list[np.ndarray], + tol: float | None, + ends: bool, +) -> tuple[list[tuple[int, int, int, int]], set]: + """Two-hop confirmation post-pass over consecutive links. + + A consecutive link (t0,r0)->(t1,r1) survives iff a consecutive onward + link from (t1,r1) continues within velocity kink ``tol`` (mm/frame). + Links into the last frame cannot be judged and are kept; gap links + pass through untouched (gap logic already decided them). With ``ends``, + dead-end consecutive links (no onward link, not last frame) are also + severed -- a dying track grabbing a stranger. Severing fragments + chains; rejoining them is gap-relink/repair's job, not assembly's. + + Returns (kept_links, severed) with severed a set of node pairs. + """ + last_t = len(frame_particles) - 1 + fwd: dict[tuple[int, int], list[tuple[int, int]]] = {} + for (t0, r0, t1, r1) in links: + if t1 - t0 == 1: + fwd.setdefault((t0, r0), []).append((t1, r1)) + severed = set() + if tol is not None: + P = [np.asarray(p, dtype=np.float64) for p in frame_particles] + for (t0, r0, t1, r1) in links: + if t1 - t0 != 1 or t1 >= last_t: + continue + if r0 >= len(P[t0]) or r1 >= len(P[t1]): + continue + onward = fwd.get((t1, r1), []) + if not onward: + if ends: + severed.add(((t0, r0), (t1, r1))) + continue + v = P[t1][r1] - P[t0][r0] + keep = False + for (t2, r2) in onward: + if t2 - t1 != 1 or r2 >= len(P[t2]): + continue + kink = float(np.linalg.norm((P[t2][r2] - P[t1][r1]) - v)) + if kink <= tol: + keep = True + break + if not keep: + severed.add(((t0, r0), (t1, r1))) + kept = [L for L in links + if not (L[2] - L[0] == 1 and ((L[0], L[1]), (L[2], L[3])) in severed)] + return kept, severed + + +def _split_hist( + hist: dict[int, list[tuple[int, int, bool]]], + severed: set, +) -> dict[int, list[tuple[int, int, bool]]]: + """Split per-track point histories where confirmation severed a link. + + A consecutive step whose node pair is in ``severed`` starts a new track + id; shared flags ride along per point. Gap steps are never severed. + """ + new_hist: dict[int, list[tuple[int, int, bool]]] = {} + nxt = max(hist) + 1 if hist else 0 + for _tid, pts in hist.items(): + cur = [] + prev = None + for (f, r, s) in pts: + if (prev is not None and f - prev[0] == 1 + and (prev, (f, r)) in severed): + if cur: + new_hist[nxt] = cur + nxt += 1 + cur = [] + cur.append((f, r, s)) + prev = (f, r) + if cur: + new_hist[nxt] = cur + nxt += 1 + return new_hist + + +def _merge_bidirectional_links( + fwd_links: list[tuple[int, int, int, int]], + bwd_links: list[tuple[int, int, int, int]], + frame_particles: list[np.ndarray], +) -> list[tuple[int, int, int, int]]: + """Merge forward and backward two-phase links (Forward-First policy). + + Forward links take precedence: they represent causal forward motion with + accumulated velocity estimates. Backward links from reversed tracking recover + dropped links, terminal ends, and gaps that forward missed, added greedily by + 3D distance provided both endpoints remain unlinked in the forward set + (strictly 1-to-1 matching; never steals from or degrades forward links). + """ + fwd_out = {(t0, r0): (t1, r1) for (t0, r0, t1, r1) in fwd_links} + fwd_in = {(t1, r1): (t0, r0) for (t0, r0, t1, r1) in fwd_links} + resolved = set(fwd_links) + + fwd_set = set(fwd_links) + bwd_cands = [L for L in bwd_links if L not in fwd_set] + fp = [np.asarray(p, dtype=np.float64) for p in frame_particles] + bwd_cands.sort( + key=lambda x: np.linalg.norm(fp[x[2]][x[3]] - fp[x[0]][x[1]]) + ) + + for t0, r0, t1, r1 in bwd_cands: + if (t0, r0) not in fwd_out and (t1, r1) not in fwd_in: + resolved.add((t0, r0, t1, r1)) + fwd_out[(t0, r0)] = (t1, r1) + fwd_in[(t1, r1)] = (t0, r0) + + return sorted(list(resolved)) + + +def _chains_from_links( + links: list[tuple[int, int, int, int]], + frame_particles: list[np.ndarray], +) -> list[dict]: + """Assemble trajectory chains from a 1-to-1 link list.""" + nxt = {(t0, r0): (t1, r1) for (t0, r0, t1, r1) in links} + tgt = set(nxt.values()) + fp = [np.asarray(p, dtype=np.float64) for p in frame_particles] + chains = [] + tid = 0 + visited = set() + for s in sorted(set(nxt) - tgt): + c = [s] + k = s + while k in nxt and nxt[k] not in visited: + visited.add(k) + k = nxt[k] + c.append(k) + fr = [t for (t, r) in c] + ps = np.array([fp[t][r] for (t, r) in c]) + chains.append({ + "tid": tid, + "frames": fr, + "pos": ps, + "shared": [False] * len(fr), + }) + tid += 1 + return chains def _match_two_phase_frame( @@ -53,25 +258,42 @@ def _match_two_phase_frame( p1: np.ndarray, radius: float, leaf_weight: float = 1.0, -) -> set[tuple[int, int]]: + cost_mode: str = "projected", + allow_shared: bool = False, + share_tol: float | None = None, + max_group_size: int = 128, +) -> tuple[set[tuple[int, int]], set[tuple[int, int]]]: """Two-phase frame-to-frame matching: 3D search + 2D ranking. Parameters ---------- - pts0, pts1 : (N, 3) and (M, 3) — 3D positions in mm + pts0, pts1 : (N, 3) and (M, 3) — 3D positions in mm. With velocity + prediction enabled these are PREDICTED positions; pass the + re-projected pixel positions as xy0 so costs are evaluated at the + prediction (stale appearance nulls the prediction -- the bounce + bias returns). xy0, xy1 : (N, D) and (M, D) — flattened 2D leaf features p0, p1 : particle IDs for frame 0 and 1 radius : float — 3D search radius in mm leaf_weight : float — weight for 2D distances in cost matrix + cost_mode : str — "projected" (2D leaf costs) or "3d" (3D distance + between pts0 and candidates; needs no calibration). + allow_shared : bool — prototype: in components with more predictors + than candidates, unassigned predictors share their best candidate + instead of going unmatched. + share_tol : float | None — maximum edge cost for a shared claim + (mutual-good-prediction gate). None disables the gate. Returns ------- links : set of (pid0, pid1) pairs + shared : set of (pid0, pid1) pairs, subset of links, observed jointly + (empty unless allow_shared) """ n_pred = len(pts0) n_cand = len(pts1) if n_pred == 0 or n_cand == 0: - return set() + return set(), set() # Phase 1: 3D KD-tree candidate search tree3d = cKDTree(pts1) @@ -79,11 +301,13 @@ def _match_two_phase_frame( # Build edge list with 2D costs rows, cols, costs = [], [], [] + use_leaves = (cost_mode == "projected" and leaf_weight > 0 + and xy0.shape[1] > 0) for pi in range(n_pred): cands = neighbours[pi] if len(cands) == 0: continue - if leaf_weight > 0 and xy0.shape[1] > 0: + if use_leaves: # 2D cost: mean Euclidean distance per camera, weighted by overlap count C = xy0.shape[1] // 2 xy0_cam = xy0[pi].reshape(C, 2) @@ -110,7 +334,7 @@ def _match_two_phase_frame( costs.append(np.linalg.norm(pts0[pi] - pts1[ci])) if len(rows) == 0: - return set() + return set(), set() rows = np.array(rows) cols = np.array(cols) @@ -125,6 +349,7 @@ def _match_two_phase_frame( n_comp, labels = connected_components(graph, directed=False) links = set() + shared: set[tuple[int, int]] = set() edge_comp = labels[rows] comp_edges = np.bincount(edge_comp, minlength=n_comp) @@ -135,6 +360,8 @@ def _match_two_phase_frame( # Non-trivial: small dense Hungarian per component rest = np.flatnonzero(~trivial) + assigned_rows: set[int] = set() + assigned_cols: set[int] = set() if len(rest): rest = rest[np.argsort(edge_comp[rest], kind="stable")] splits = np.flatnonzero(np.diff(edge_comp[rest])) + 1 @@ -144,6 +371,29 @@ def _match_two_phase_frame( c_costs = costs[group].tolist() uniq_r = sorted(set(c_rows)) uniq_c = sorted(set(c_cols)) + if len(uniq_r) + len(uniq_c) > max_group_size: + # Production-density percolation: cubic Hungarian would stall + # the frame. Greedy inside the group, sharing still applies. + order = np.argsort(np.array(c_costs), kind="stable") + for k in order.tolist(): + r, c = c_rows[k], c_cols[k] + if r not in assigned_rows and c not in assigned_cols: + links.add((int(p0[r]), int(p1[c]))) + assigned_rows.add(r) + assigned_cols.add(c) + if allow_shared: + for r in uniq_r: + if r in assigned_rows: + continue + best_c, best_d = None, np.inf + for k, (rr, cc) in enumerate(zip(c_rows, c_cols)): + if rr == r and c_costs[k] < best_d: + best_d, best_c = c_costs[k], cc + if best_c is not None and \ + (share_tol is None or best_d < share_tol): + links.add((int(p0[r]), int(p1[best_c]))) + shared.add((int(p0[r]), int(p1[best_c]))) + continue r_local = {v: i for i, v in enumerate(uniq_r)} c_local = {v: i for i, v in enumerate(uniq_c)} max_cost = max(c_costs) if c_costs else 1.0 @@ -153,10 +403,38 @@ def _match_two_phase_frame( sub[r_local[rr], c_local[cc]] = dd r_ind, c_ind = linear_sum_assignment(sub) real = sub[r_ind, c_ind] < sentinel + group_winners = [] for r_i, c_i in zip(r_ind[real], c_ind[real]): links.add((int(p0[uniq_r[r_i]]), int(p1[uniq_c[c_i]]))) - - return links + assigned_rows.add(int(uniq_r[r_i])) + assigned_cols.add(int(uniq_c[c_i])) + group_winners.append((int(p0[uniq_r[r_i]]), + int(p1[uniq_c[c_i]]))) + # Prototype shared-observation: more predictors than candidates + # = detector undercount (occlusion). Unassigned predictors share + # their best candidate instead of dying. Tracked by the caller + # via the streak cap. + group_shared = [] + if allow_shared and len(uniq_r) > len(uniq_c): + for r_i, r in enumerate(uniq_r): + if r in assigned_rows: + continue + best_c, best_d = None, sentinel + for c_i, c in enumerate(uniq_c): + if sub[r_i, c_i] < best_d: + best_d, best_c = sub[r_i, c_i], c + if best_c is not None and best_d < sentinel and \ + (share_tol is None or best_d < share_tol): + links.add((int(p0[r]), int(p1[best_c]))) + group_shared.append((int(p0[r]), int(p1[best_c]))) + if group_shared: + # The winners' points in a sharing group are joint evidence + # too: nobody updates velocity from a merged point, or the + # winner predicts from poisoned history at separation. + shared.update(group_winners) + shared.update(group_shared) + + return links, shared class TwoPhaseTracker: @@ -175,9 +453,18 @@ def track_frames( self, frame_particles: list[np.ndarray], frame_leaves: list[np.ndarray] | None = None, + project_fn=None, + return_chains: bool = False, ) -> list[tuple[int, int, int, int]]: """Track particles across frames using two-phase matching. + Stateful: every live track carries a constant-velocity estimate. + Each step matches velocity PREDICTIONS (not current positions) + against new detections, so steady crossings resolve correctly. + New detections spawn zero-velocity tracks (cold start); tracks + unmatched for more than ``max_gap`` frames retire, so a particle + occluded for a frame is re-caught by gap-spanning prediction. + Parameters ---------- frame_particles : list of (N_i, 3) arrays @@ -185,41 +472,215 @@ def track_frames( frame_leaves : list of (N_i, D) arrays, optional Flattened 2D leaf features per frame. If None, falls back to pure 3D matching. + project_fn : callable, optional + ``(N, 3) -> (N, D)`` mapping predicted 3D positions to leaf + features (re-projection through the camera models). Required + for ``cost_mode="projected"``; without it costs fall back to + 3D distance (see ``cost_mode``). Returns ------- links : list of (t0, pid0, t1, pid1) tuples - Frame-to-frame particle links (0-based time indices). + Frame-to-frame particle links (0-based time indices, row ids + within each frame's arrays). Gap-spanning links reference the + track's last seen frame/row. + (if return_chains) chains : list of dicts with keys tid, frames, + rows, pos, shared — per-track point histories; shared flags + mark jointly-observed points. Row-links alone cannot represent + sharing (one node, two owners), hence chains. """ num_frames = len(frame_particles) if num_frames < 2: - return [] + return ([], []) if return_chains else [] + + if not self.cfg.bidirectional: + return self._track_unidirectional( + frame_particles, frame_leaves, project_fn, return_chains + ) + + # Bidirectional tracking: forward + backward on reversed frames + fwd_links = self._track_unidirectional( + frame_particles, frame_leaves, project_fn, return_chains=False + ) + + rev_particles = frame_particles[::-1] + rev_leaves = frame_leaves[::-1] if frame_leaves is not None else None + bwd_vmax = self.cfg.bwd_v_max if self.cfg.bwd_v_max is not None else self.cfg.v_max + bwd_links_raw = self._track_unidirectional( + rev_particles, rev_leaves, project_fn, return_chains=False, v_max_override=bwd_vmax + ) + bwd_links = [ + (num_frames - 1 - rt1, rr1, num_frames - 1 - rt0, rr0) + for (rt0, rr0, rt1, rr1) in bwd_links_raw + ] + + merged_links = _merge_bidirectional_links( + fwd_links, bwd_links, frame_particles + ) + if not return_chains: + return merged_links + + chains = _chains_from_links(merged_links, frame_particles) + return merged_links, chains + + def _track_unidirectional( + self, + frame_particles: list[np.ndarray], + frame_leaves: list[np.ndarray] | None = None, + project_fn=None, + return_chains: bool = False, + v_max_override: float | None = None, + ): + num_frames = len(frame_particles) + if num_frames < 2: + return ([], []) if return_chains else [] + + eff_vmax = v_max_override if v_max_override is not None else self.cfg.v_max if frame_leaves is None: frame_leaves = [np.zeros((len(p), 0)) for p in frame_particles] + cost_mode = self.cfg.cost_mode + if cost_mode == "projected" and project_fn is None: + cost_mode = "3d" + + next_tid = 0 + # tid -> dict(pos, vel, last_t, last_row, misses, shared_streak) + # shared_streak counts consecutive shared observations; velocity is + # NEVER updated from a shared point (each track's speed comes only + # from its own points). + tracks: dict[int, dict] = {} + # tid -> list of (frame, row, is_shared): full point history, kept + # for retired tracks too (row-links cannot represent sharing). + hist: dict[int, list[tuple[int, int, bool]]] = {} + # (frame_idx, row) -> tid, for emitting row-based links + loc2tid: dict[tuple[int, int], int] = {} + for i, p in enumerate(frame_particles[0]): + tracks[next_tid] = { + "pos": np.asarray(p, dtype=np.float64), + "vel": np.zeros(3), + "last_t": 0, + "last_row": i, + "misses": 0, + "shared_streak": 0, + } + loc2tid[(0, i)] = next_tid + hist[next_tid] = [(0, i, False)] + next_tid += 1 + all_links = [] - for i in range(num_frames - 1): - t0, t1 = i, i + 1 - pts0, pts1 = frame_particles[t0], frame_particles[t1] - lf0, lf1 = frame_leaves[t0], frame_leaves[t1] - p0 = np.arange(len(pts0), dtype=np.int32) - p1 = np.arange(len(pts1), dtype=np.int32) - - links = _match_two_phase_frame( - pts0, - pts1, - lf0, - lf1, - p0, - p1, - self.cfg.v_max, + for t in range(num_frames - 1): + pts1 = np.asarray(frame_particles[t + 1], dtype=np.float64) + lf1 = frame_leaves[t + 1] + n1 = len(pts1) + + # Active tracks: seen within max_gap frames. + active = [tid for tid, tr in tracks.items() + if t + 1 - tr["last_t"] <= self.cfg.max_gap] + if not active or n1 == 0: + pred_pts = np.zeros((0, 3)) + pred_xy = np.zeros((0, lf1.shape[1] if n1 else 0)) + tids: list[int] = [] + else: + steps = np.array([t + 1 - tracks[tid]["last_t"] + for tid in active], dtype=np.float64) + if self.cfg.use_velocity: + pred_pts = np.array( + [tracks[tid]["pos"] + + tracks[tid]["vel"] * steps[k] * self.cfg.dt + for k, tid in enumerate(active)]) + else: + pred_pts = np.array([tracks[tid]["pos"] for tid in active]) + if cost_mode == "projected": + pred_xy = np.asarray(project_fn(pred_pts)) + else: + pred_xy = np.zeros((len(active), 0)) + tids = active + + got, got_shared = _match_two_phase_frame( + np.asarray(pred_pts, dtype=np.float64), + np.asarray(pts1, dtype=np.float64), + np.asarray(pred_xy, dtype=np.float64), + np.asarray(lf1, dtype=np.float64), + np.arange(len(tids), dtype=np.int32), + np.arange(n1, dtype=np.int32), + eff_vmax, self.cfg.leaf_weight, + cost_mode=cost_mode, + allow_shared=self.cfg.allow_shared, + share_tol=self.cfg.share_tol, ) - for pid0, pid1 in links: - all_links.append((t0, pid0, t1, pid1)) - - return all_links + matched_det = set() + for ai, det in got: + tid = tids[int(ai)] + tr = tracks[tid] + gap = t + 1 - tr["last_t"] + old_pos = tr["pos"] + if (int(ai), int(det)) in got_shared and \ + tr.get("shared_streak", 0) < self.cfg.max_shared: + # Shared observation: follow the point, keep own speed. + tr["pos"] = pts1[det].copy() + tr["shared_streak"] = tr.get("shared_streak", 0) + 1 + is_shared = True + else: + tr["vel"] = (pts1[det] - old_pos) / (gap * self.cfg.dt) + tr["pos"] = pts1[det].copy() + tr["shared_streak"] = 0 + is_shared = False + all_links.append((tr["last_t"], tr["last_row"], t + 1, det)) + tr["last_t"] = t + 1 + tr["last_row"] = int(det) + tr["misses"] = 0 + hist[tid].append((t + 1, int(det), is_shared)) + loc2tid[(t + 1, int(det))] = tid + matched_det.add(int(det)) + + # Age every unseen track (including ones already outside the + # active window) and retire the exhausted. + for tid in list(tracks.keys()): + if tracks[tid]["last_t"] <= t: + tracks[tid]["misses"] += 1 + if tracks[tid]["misses"] > self.cfg.max_gap: + del tracks[tid] + + # Cold start: unmatched detections become zero-velocity tracks. + for det in range(n1): + if det not in matched_det: + tracks[next_tid] = { + "pos": pts1[det].copy(), + "vel": np.zeros(3), + "last_t": t + 1, + "last_row": det, + "misses": 0, + "shared_streak": 0, + } + loc2tid[(t + 1, det)] = next_tid + hist[next_tid] = [(t + 1, det, False)] + next_tid += 1 + + if not return_chains: + if self.cfg.confirm_tol is not None: + all_links, _ = _confirm_links( + all_links, frame_particles, self.cfg.confirm_tol, + self.cfg.confirm_ends) + return all_links + if self.cfg.confirm_tol is not None: + all_links, _sev = _confirm_links( + all_links, frame_particles, self.cfg.confirm_tol, + self.cfg.confirm_ends) + hist = _split_hist(hist, _sev) + chains = [] + fp_arr = [np.asarray(p, dtype=np.float64) for p in frame_particles] + for tid, pts in hist.items(): + if len(pts) == 0: + continue + chains.append({ + "tid": tid, + "frames": [f for f, _, _ in pts], + "pos": np.array([fp_arr[f][r] for f, r, _ in pts]), + "shared": [s for _, _, s in pts], + }) + return all_links, chains class Tracking: @@ -234,6 +695,44 @@ def __init__(self, ptv=None, exp=None): self.ptv = ptv self.exp = exp + def _build_project_fn(self): + """Re-project predicted 3D positions to leaf pixels via exp cals. + + Returns None when calibrations are unavailable; the tracker then + falls back to 3D-distance costs (see ``cost_mode``). + """ + try: + cals = list(getattr(self.exp, "cals", None) or []) + cpar = getattr(self.exp, "cpar", None) + if not cals or cpar is None: + return None + mm = cpar.mm + imx, imy = float(cpar.imx), float(cpar.imy) + pix_x, pix_y = float(cpar.pix_x), float(cpar.pix_y) + + from openptv2.algorithms.imgcoord import img_coord_batch + + def project_fn(pred): + pred = np.asarray(pred, dtype=np.float64) + n = len(pred) + nc = len(cals) + xy = np.full((n, nc * 2), np.nan) + for i in range(n): + for ci, cal in enumerate(cals): + m = img_coord_batch(pred[i : i + 1], cal, mm)[0] + xy[i, 2 * ci] = m[0] / pix_x + imx / 2 + xy[i, 2 * ci + 1] = imy / 2 - m[1] / pix_y + return np.nan_to_num(xy) + + # Smoke-test on one point so a broken model fails here, not + # mid-run. + project_fn(np.zeros((1, 3))) + return project_fn + except Exception as exc: + print(f"TwoPhaseTracker: no projection ({exc}); " + f"falling back to 3D costs.") + return None + def do_tracking(self) -> None: if self.exp is None: raise ValueError("No experiment object provided") @@ -244,7 +743,21 @@ def do_tracking(self) -> None: track_cfg = pm.parameters.get("track", {}) if pm else {} leaf_weight = float(track_cfg.get("leaf_weight", 1.0)) - v_max = float(track_cfg.get("dvxmax", 15.5)) + v_max = float(track_cfg.get("v_max", track_cfg.get("dvxmax", 15.5))) + use_velocity = bool(track_cfg.get("use_velocity", True)) + cost_mode = str(track_cfg.get("cost_mode", "projected")) + max_gap = int(track_cfg.get("max_gap", 2)) + allow_shared = bool(track_cfg.get("allow_shared", False)) + max_shared = int(track_cfg.get("max_shared", 2)) + share_tol_raw = track_cfg.get("share_tol", 1.0) + share_tol = None if share_tol_raw is None else float(share_tol_raw) + max_group_size = int(track_cfg.get("max_group_size", 128)) + confirm_raw = track_cfg.get("confirm_tol", None) + confirm_tol = None if confirm_raw is None else float(confirm_raw) + confirm_ends = bool(track_cfg.get("confirm_ends", False)) + bidirectional = bool(track_cfg.get("bidirectional", False)) + bwd_v_max_raw = track_cfg.get("bwd_v_max", None) + bwd_v_max = None if bwd_v_max_raw is None else float(bwd_v_max_raw) store = getattr(self.exp, "_store", None) if store is None: @@ -329,9 +842,21 @@ def do_tracking(self) -> None: xy[valid, c] = t[cam_ids[valid, c], 1:3] frame_leaves.append(np.nan_to_num(xy.reshape(n, -1))) - cfg = TwoPhaseTrackerConfig(v_max=v_max, leaf_weight=leaf_weight) + cfg = TwoPhaseTrackerConfig(v_max=v_max, leaf_weight=leaf_weight, + use_velocity=use_velocity, + cost_mode=cost_mode, max_gap=max_gap, + allow_shared=allow_shared, + max_shared=max_shared, + share_tol=share_tol, + max_group_size=max_group_size, + confirm_tol=confirm_tol, + confirm_ends=confirm_ends, + bidirectional=bidirectional, + bwd_v_max=bwd_v_max) tracker = TwoPhaseTracker(cfg) - links = tracker.track_frames(frame_particles, frame_leaves) + project_fn = self._build_project_fn() + links = tracker.track_frames(frame_particles, frame_leaves, + project_fn=project_fn) # Per-step progress like trackcorr (track3d step: curr/next/links) from collections import Counter @@ -375,5 +900,8 @@ def do_tracking(self) -> None: print( f"TwoPhaseTracker: {len(links)} links across {len(frames)} frames " - f"(leaf_weight={leaf_weight}, v_max={v_max})" + f"(leaf_weight={leaf_weight}, v_max={v_max}, " + f"use_velocity={use_velocity}, cost_mode={cost_mode}, " + f"max_gap={max_gap}, " + f"project_fn={'yes' if project_fn is not None else 'no'})" ) diff --git a/src/openptv2/segmentation.py b/src/openptv2/segmentation.py index b8a03adc..f346fba8 100644 --- a/src/openptv2/segmentation.py +++ b/src/openptv2/segmentation.py @@ -198,7 +198,9 @@ def target_recognition(img, tpar, cam, cpar, subrange_x=None, subrange_y=None): ) for i in range(n) ] - return TargetArray(targets) + tarr = TargetArray(targets) + tarr.sort_y() + return tarr from openptv2.algorithms.segmentation import detect_targets_batch_parallel # noqa: E402 diff --git a/src/openptv2/tracker.py b/src/openptv2/tracker.py index 6c27bd59..40b30498 100644 --- a/src/openptv2/tracker.py +++ b/src/openptv2/tracker.py @@ -73,7 +73,19 @@ class Tracker: """ def __init__( - self, cpar, vpar, tpar, spar, cals, naming=None, flatten_tol=0.0001, store=None + self, + cpar, + vpar, + tpar, + spar, + cals, + naming=None, + flatten_tol=0.0001, + store=None, + loser_retry=1, + cold_start_neighbour=1, + app_weight=0.0, + use_grid=0, ): """ Initialize Tracker. @@ -97,6 +109,13 @@ def __init__( self._cals = cals self._flatten_tol = flatten_tol self._store = store + # 1 = openptv2 default, 0 = original 3dptv track.c link resolution. + self._loser_retry = int(loser_retry) + self._cold_start_neighbour = int(cold_start_neighbour) + # Appearance cost weight, threaded to TrackingRun (0.0 = track.c). + self._app_weight = float(app_weight) + # Uniform-grid candidate search (0 = legacy y-band scan exactly). + self._use_grid = int(use_grid) # File naming if naming is None: @@ -139,6 +158,10 @@ def restart(self): cal=self._cals_algo, flatten_tol=self._flatten_tol, store=self._store, + loser_retry=self._loser_retry, + cold_start_neighbour=self._cold_start_neighbour, + app_weight=self._app_weight, + use_grid=self._use_grid, ) # Initialize forward tracking diff --git a/src/openptv2/tracking_shared.py b/src/openptv2/tracking_shared.py new file mode 100644 index 00000000..b2d039a7 --- /dev/null +++ b/src/openptv2/tracking_shared.py @@ -0,0 +1,185 @@ +"""Shared-observation prototype for trackcorr, at linkage-file level. + +trackcorr's kernel (``track_kernels_corr._trackcorr_particle_fast``) is +compiled per-particle greedy code -- the plan is to prototype the lesson in +pure Python on its linkage files first and move it into the kernel (behind a +flag) only if it wins: + +1. :func:`mark_shared_observations` -- after the forward pass, find frames + where one 3D point serves two tracks (detector undercount = occlusion) + and mark it shared instead of letting one track die. +2. Downstream, shared points update position but never velocity (each + track's speed comes only from its own points); gap bridging + (``tracking_postprocess.relink_trajectory_gaps``) then stitches with + honest speeds. + +Linkage convention (mirrors ``tracking_postprocess``): + frames[k] = (prev_k, next_k, xyz_k) with prev/next int arrays + (PREV_NONE/NEXT_NONE = unlinked) and xyz_k (N,3) float positions. +Shared marks: dict mapping (frame, idx) -> list of track-end ids sharing it. +A "track end" here is identified by (frame, idx) of its last point. +""" + +from __future__ import annotations + +import numpy as np + +PREV_NONE = -1 +NEXT_NONE = -2 + + +def _velocity(frames, k, idx, shared, back_steps=1): + """Velocity at (k, idx) from own points only: walk back over prev links, + skipping shared marks. Returns None when fewer than two independent + points exist.""" + pts = [(k, idx)] + ck, ci = k, idx + while True: + prev, _, _ = frames[ck] + pi = int(prev[ci]) + if pi < 0: + break + ck -= 1 + if ck not in frames: + break + ci = pi + pts.append((ck, ci)) + if len(pts) >= back_steps + 2: + break + indep = [(fk, fi) for (fk, fi) in pts if (fk, fi) not in shared] + if len(indep) < 2: + return None + (f1, i1), (f0, i0) = indep[-1], indep[-2] + dt = max(f1 - f0, 1) + _, _, xyz1 = frames[f1] + _, _, xyz0 = frames[f0] + return (xyz1[i1] - xyz0[i0]) / dt + + +def mark_shared_observations(frames, first, last, tol, max_share=2): + """Find occlusions: a frame-(k+1) detection predicted well by >= 2 + distinct frame-k tracks that have a velocity (prev link), where at most + one of them actually claimed it. + + Returns shared: dict (frame, idx) -> list of (frame_k, idx_k) sharers. + Does not modify linkages; assembly/relink consume the marks. + """ + shared: dict[tuple[int, int], list[tuple[int, int]]] = {} + for k in range(first, last): + if k not in frames or (k + 1) not in frames: + continue + prev_k, next_k, xyz_k = frames[k] + _, _, xyz_n = frames[k + 1] + # predictors: frame-k particles with a prev link (have velocity) + for j in range(len(xyz_n)): + claimants = [] + for i in range(len(xyz_k)): + pi = int(prev_k[i]) + if pi < 0 or k - 1 not in frames: + continue + _, _, xyz_p = frames[k - 1] + if pi >= len(xyz_p): + continue + v = xyz_k[i] - xyz_p[pi] + if np.linalg.norm(xyz_n[j] - (xyz_k[i] + v)) < tol: + claimants.append((i, float( + np.linalg.norm(xyz_n[j] - (xyz_k[i] + v))))) + # undercount signature: >= 2 predictors, <= 1 actual claim. + # actual claims: frame-k particles with next_k[i] == j + actual = [i for i in range(len(xyz_k)) if int(next_k[i]) == j] + if len(claimants) >= 2 and len(actual) <= 1: + key = (k + 1, j) + shared.setdefault(key, []) + for i, _ in sorted(claimants, key=lambda t: t[1]): + if (k, i) not in shared[key]: + shared[key].append((k, i)) + shared[key] = shared[key][: max_share + 1] + return shared + + +def assemble_with_shared(frames, first, last, shared): + """Assemble tracks from linkage arrays, emitting a shared point into + every sharing track (position shared, histories stay distinct because + each track keeps its own prev chain). + + Returns list of dicts: {frames: [...], pos: (L,3)}. + """ + # forward chains from every unclaimed start; shared points entered + # once per sharing track. + tracks = [] + visited = set() # (frame, idx, owner-key) to allow shared re-entry + # 1. ordinary chains from particles with no prev link + for k in range(first, last + 1): + if k not in frames: + continue + prev_k, next_k, xyz_k = frames[k] + for i in range(len(xyz_k)): + if int(prev_k[i]) >= 0: + continue + chain = [] + ck, ci = k, i + while True: + chain.append((ck, ci)) + if ck not in frames: + break + _, nxt, _ = frames[ck] + ni = int(nxt[ci]) if ci < len(nxt) else NEXT_NONE + if ni < 0: + break + ck += 1 + if ck not in frames: + break + ci = ni + tracks.append(chain) + for node in chain: + visited.add((node, "main")) + # 2. sharing tracks: re-walk from each sharer end through the shared + # point, then STOP. The shared point is observed jointly, but what + # follows belongs to whoever links there next -- riding the winner's + # tail would graft the wrong identity onto the sharer (a switch). + # Continuation past separation is gap-relink's job, not assembly's. + for (fk, fj), sharers in shared.items(): + for (sk, si) in sharers: + # walk the sharer's own history up to (sk, si) + hist = [] + ck, ci = sk, si + while True: + hist.append((ck, ci)) + if ck not in frames: + break + pv, _, _ = frames[ck] + pi = int(pv[ci]) if ci < len(pv) else PREV_NONE + if pi < 0: + break + ck -= 1 + if ck not in frames: + break + ci = pi + hist.reverse() + # then the shared point itself -- and stop. Whatever continues + # past separation belongs to whoever links there next; following + # the winner's tail would graft the wrong identity (a switch). + full = hist + [(fk, fj)] if (fk, fj) not in hist else list(hist) + key = ("shared", sk, si, fk, fj) + if key in visited: + continue + visited.add(key) + # skip if an identical main chain already covers it + if any(all(n in c for n in full) for c in tracks + if len(c) >= len(full)): + continue + tracks.append(full) + out = [] + for chain in tracks: + fr, ps = [], [] + for (ck, ci) in chain: + if ck not in frames: + continue + _, _, xyz = frames[ck] + if ci >= len(xyz): + continue + fr.append(ck) + ps.append(xyz[ci]) + if len(fr) >= 1: + out.append({"frames": fr, "pos": np.array(ps)}) + return out diff --git a/tests/unit/test_track_kernels_batch_coverage.py b/tests/unit/test_track_kernels_batch_coverage.py index 7234ff45..1a7ecd76 100644 --- a/tests/unit/test_track_kernels_batch_coverage.py +++ b/tests/unit/test_track_kernels_batch_coverage.py @@ -25,10 +25,6 @@ from openptv2.algorithms.track_kernels_batch import ( # noqa: E402 init_mmlut_data_fast, - metric_to_pixel_batch_fast, - pixel_to_metric_batch_fast, - point_position_batch_fast, - ray_tracing_batch_fast, targ_rec_fast, ) @@ -89,46 +85,6 @@ def _make_cal( # ───────────────────────────────────────────────────────────────────────────── -def test_ray_tracing_batch_empty(): - """N=0 input → (0, 3) position and direction arrays.""" - xy = np.empty((0, 2), dtype=np.float64) - pos, dirs = ray_tracing_batch_fast(xy, _CAL) - assert pos.shape == (0, 3) - assert dirs.shape == (0, 3) - - -def test_ray_tracing_batch_single(): - """N=1 → (1, 3) outputs; values are finite.""" - xy = np.array([[0.0, 0.0]], dtype=np.float64) - pos, dirs = ray_tracing_batch_fast(xy, _CAL) - assert pos.shape == (1, 3) - assert dirs.shape == (1, 3) - assert np.all(np.isfinite(pos)) - assert np.all(np.isfinite(dirs)) - - -def test_ray_tracing_batch_multiple(): - """N=5 → (5, 3) outputs; all finite.""" - xy = np.array( - [[-5.0, -5.0], [-2.0, 0.0], [0.0, 0.0], [2.0, 0.0], [5.0, 5.0]], - dtype=np.float64, - ) - pos, dirs = ray_tracing_batch_fast(xy, _CAL) - assert pos.shape == (5, 3) - assert dirs.shape == (5, 3) - assert np.all(np.isfinite(pos)) - assert np.all(np.isfinite(dirs)) - - -def test_ray_tracing_batch_off_axis(): - """Non-zero x0,y0 camera position still produces finite rays.""" - cal_off = _make_cal(x0=10.0, y0=5.0, z0=80.0) - xy = np.array([[1.0, -1.0], [0.5, 0.5]], dtype=np.float64) - pos, dirs = ray_tracing_batch_fast(xy, cal_off) - assert pos.shape == (2, 3) - assert np.all(np.isfinite(pos)) - - # ───────────────────────────────────────────────────────────────────────────── # pixel_to_metric_batch_fast # ───────────────────────────────────────────────────────────────────────────── @@ -137,89 +93,11 @@ def test_ray_tracing_batch_off_axis(): _PIXX, _PIXY = 0.017, 0.017 -def test_pixel_to_metric_batch_empty(): - """N=0 → (0, 2) result, no crash.""" - xy = np.empty((0, 2), dtype=np.float64) - result = pixel_to_metric_batch_fast(xy, _IMX, _IMY, _PIXX, _PIXY, 0) - assert result.shape == (0, 2) - - -def test_pixel_to_metric_batch_chfield0(): - """chfield=0 → standard pixel-to-metric; shape (3, 2), finite.""" - xy = np.array([[320.0, 240.0], [0.0, 0.0], [640.0, 480.0]], dtype=np.float64) - result = pixel_to_metric_batch_fast(xy, _IMX, _IMY, _PIXX, _PIXY, 0) - assert result.shape == (3, 2) - assert np.all(np.isfinite(result)) - - -def test_pixel_to_metric_batch_chfield1(): - """chfield=1 → yp = 2*y + 1 branch executed.""" - xy = np.array([[100.0, 100.0]], dtype=np.float64) - result = pixel_to_metric_batch_fast(xy, _IMX, _IMY, _PIXX, _PIXY, 1) - assert result.shape == (1, 2) - assert np.isfinite(result[0, 1]) - - -def test_pixel_to_metric_batch_chfield2(): - """chfield=2 → yp = 2*y branch executed.""" - xy = np.array([[100.0, 100.0]], dtype=np.float64) - result = pixel_to_metric_batch_fast(xy, _IMX, _IMY, _PIXX, _PIXY, 2) - assert result.shape == (1, 2) - assert np.isfinite(result[0, 1]) - - -def test_pixel_to_metric_batch_center(): - """Image centre maps to metric origin (0, 0) for chfield=0.""" - xy = np.array([[_IMX / 2.0, _IMY / 2.0]], dtype=np.float64) - result = pixel_to_metric_batch_fast(xy, _IMX, _IMY, _PIXX, _PIXY, 0) - assert abs(result[0, 0]) < 1e-10 - assert abs(result[0, 1]) < 1e-10 - - # ───────────────────────────────────────────────────────────────────────────── # metric_to_pixel_batch_fast # ───────────────────────────────────────────────────────────────────────────── -def test_metric_to_pixel_batch_chfield0(): - """chfield=0 → standard metric-to-pixel; shape (3, 2), finite.""" - xy = np.array([[0.0, 0.0], [1.0, 1.0], [-1.0, -1.0]], dtype=np.float64) - result = metric_to_pixel_batch_fast(xy, _IMX, _IMY, _PIXX, _PIXY, 0) - assert result.shape == (3, 2) - assert np.all(np.isfinite(result)) - - -def test_metric_to_pixel_batch_chfield1(): - """chfield=1 → y_pixel = (y_pixel - 1) * 0.5 branch executed.""" - xy = np.array([[0.5, 0.5]], dtype=np.float64) - result = metric_to_pixel_batch_fast(xy, _IMX, _IMY, _PIXX, _PIXY, 1) - assert result.shape == (1, 2) - assert np.isfinite(result[0, 1]) - - -def test_metric_to_pixel_batch_chfield2(): - """chfield=2 → y_pixel = y_pixel * 0.5 branch executed.""" - xy = np.array([[0.5, 0.5]], dtype=np.float64) - result = metric_to_pixel_batch_fast(xy, _IMX, _IMY, _PIXX, _PIXY, 2) - assert result.shape == (1, 2) - assert np.isfinite(result[0, 1]) - - -def test_pixel_metric_roundtrip(): - """pixel→metric→pixel recovers original coordinates (chfield=0).""" - pts_px = np.array([[320.0, 240.0], [100.0, 380.0]], dtype=np.float64) - metric = pixel_to_metric_batch_fast(pts_px, _IMX, _IMY, _PIXX, _PIXY, 0) - back = metric_to_pixel_batch_fast(metric, _IMX, _IMY, _PIXX, _PIXY, 0) - assert np.allclose(back, pts_px, atol=1e-8) - - -def test_metric_pixel_empty(): - """N=0 metric_to_pixel → (0, 2) result.""" - xy = np.empty((0, 2), dtype=np.float64) - result = metric_to_pixel_batch_fast(xy, _IMX, _IMY, _PIXX, _PIXY, 0) - assert result.shape == (0, 2) - - # ───────────────────────────────────────────────────────────────────────────── # point_position_batch_fast # ───────────────────────────────────────────────────────────────────────────── @@ -232,47 +110,6 @@ def _two_cams(): return (cal1, cal2) -def test_point_position_batch_empty(): - """num_pts=0 → (0, 3) positions and (0,) distances.""" - all_targets = np.empty((0, 2, 2), dtype=np.float64) - cal_arrays = _two_cams() - positions, distances = point_position_batch_fast(all_targets, 0, 2, cal_arrays) - assert positions.shape == (0, 3) - assert distances.shape == (0,) - - -def test_point_position_batch_one_point(): - """num_pts=1, num_cams=2 → (1, 3) and (1,); finite values.""" - all_targets = np.zeros((1, 2, 2), dtype=np.float64) - cal_arrays = _two_cams() - positions, distances = point_position_batch_fast(all_targets, 1, 2, cal_arrays) - assert positions.shape == (1, 3) - assert distances.shape == (1,) - assert np.all(np.isfinite(positions)) - assert np.isfinite(distances[0]) - - -def test_point_position_batch_multiple_points(): - """num_pts=3 → (3, 3) positions and (3,) distances.""" - all_targets = np.zeros((3, 2, 2), dtype=np.float64) - cal_arrays = _two_cams() - positions, distances = point_position_batch_fast(all_targets, 3, 2, cal_arrays) - assert positions.shape == (3, 3) - assert distances.shape == (3,) - - -def test_point_position_batch_nonzero_targets(): - """Finite target coords still yield finite positions.""" - all_targets = np.array( - [[[1.0, 2.0], [-1.0, 2.0]], [[0.5, 0.5], [-0.5, 0.5]]], - dtype=np.float64, - ) - cal_arrays = _two_cams() - positions, distances = point_position_batch_fast(all_targets, 2, 2, cal_arrays) - assert positions.shape == (2, 3) - assert np.all(np.isfinite(positions)) - - # ───────────────────────────────────────────────────────────────────────────── # targ_rec_fast — helpers # ───────────────────────────────────────────────────────────────────────────── diff --git a/tests/unit/test_track_kernels_coverage.py b/tests/unit/test_track_kernels_coverage.py index e1aafce9..5adab7f4 100644 --- a/tests/unit/test_track_kernels_coverage.py +++ b/tests/unit/test_track_kernels_coverage.py @@ -31,11 +31,9 @@ is_compiled, pack_cal_array, pack_mmlut, - point_position_fast, # re-exports from sub-modules point_to_pixel_fast, searchquader_fast, - sort_candidates_by_freq_fast, sorted_candidates_fast, targ_rec_fast, track3d_loop_fast, @@ -301,9 +299,7 @@ def test_reexported_callables_exist(): searchquader_fast, candsearch_in_pix_fast, candsearch_in_pix_rest_fast, - sort_candidates_by_freq_fast, sorted_candidates_fast, - point_position_fast, trackcorr_loop_fast, trackback_loop_fast, track3d_loop_fast, diff --git a/tests/unit/test_track_kernels_geom_coverage.py b/tests/unit/test_track_kernels_geom_coverage.py index 564862e4..3f9db554 100644 --- a/tests/unit/test_track_kernels_geom_coverage.py +++ b/tests/unit/test_track_kernels_geom_coverage.py @@ -19,14 +19,16 @@ from openptv2.algorithms.track_kernels_geom import ( CAL_ARRAY_SIZE, PT_UNUSED, - _angle_acc_out, + point_to_pixel_fast, + searchquader_fast, +) +from openptv2.algorithms.track_kernels_pixel import ( _multimed_r_nlay_1layer, _point_to_pixel_out, - _ray_tracing_fast, +) +from openptv2.algorithms.track_kernels_position import ( + _angle_acc_out, _ray_tracing_out, - angle_acc_fast, - point_to_pixel_fast, - searchquader_fast, ) # --------------------------------------------------------------------------- @@ -1006,160 +1008,6 @@ def test_searchquader_zero_quader(): assert xr.shape == (1,) -# --------------------------------------------------------------------------- -# angle_acc_fast -# --------------------------------------------------------------------------- - - -def test_angle_acc_fast_same_vectors_zero(): - """v0 == v1 → angle = 0.0, acc = 0.0.""" - angle, acc = angle_acc_fast( - 0.0, - 0.0, - 0.0, # start - 1.0, - 0.0, - 0.0, # pred - 1.0, - 0.0, - 0.0, # cand (same as pred) - ) - assert angle == 0.0 - assert acc == 0.0 - - -def test_angle_acc_fast_opposite_vectors_200(): - """v0 == -v1 → angle = 200.0.""" - angle, acc = angle_acc_fast( - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, - 0.0, - -1.0, - 0.0, - 0.0, - ) - assert angle == 200.0 - - -def test_angle_acc_fast_90_degrees(): - """Perpendicular vectors → angle ≈ 100.0 (90° scaled to 200/π·rad).""" - angle, acc = angle_acc_fast( - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, - ) - assert abs(angle - 100.0) < 0.1 - assert np.isfinite(acc) - - -def test_angle_acc_fast_norm0_zero(): - """start == pred → v0 = (0,0,0) → norm0 = 0 → angle = 0.0.""" - angle, acc = angle_acc_fast( - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, - 0.0, - ) - assert angle == 0.0 - - -def test_angle_acc_fast_norm1_zero(): - """start == cand → v1 = (0,0,0) → norm1 = 0 → angle = 0.0.""" - angle, acc = angle_acc_fast( - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - ) - assert angle == 0.0 - - -def test_angle_acc_fast_nearly_parallel(): - """Almost parallel vectors — dot may be > 1 in floating point → clamped.""" - eps = 1e-14 - angle, acc = angle_acc_fast( - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, - 0.0, - 1.0 + eps, - 0.0, - 0.0, - ) - assert 0.0 <= angle <= 200.0 - - -def test_angle_acc_fast_acceleration_value(): - """Acc is the distance between v1 and v0.""" - angle, acc = angle_acc_fast( - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, - 0.0, - 2.0, - 0.0, - 0.0, - ) - # v0=(1,0,0), v1=(2,0,0) → dx=1 → acc=1 - assert abs(acc - 1.0) < 1e-10 - - -def test_angle_acc_fast_3d_vectors(): - angle, acc = angle_acc_fast( - 0.0, - 0.0, - 0.0, - 1.0, - 1.0, - 1.0, - 1.0, - -1.0, - 0.0, - ) - assert 0.0 <= angle <= 200.0 - assert np.isfinite(acc) - - -def test_angle_acc_fast_negative_dot_clamped(): - """Antiparallel but not exact → dot < -1 gets clamped to -1.""" - # Make two nearly-opposite unit vectors with floating-point excess - angle, acc = angle_acc_fast( - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, - 0.0, - -1.0, - 1e-15, - 0.0, # nearly opposite, not exactly - ) - assert 0.0 <= angle <= 200.0 - - # --------------------------------------------------------------------------- # _angle_acc_out # --------------------------------------------------------------------------- @@ -1186,14 +1034,6 @@ def test_angle_acc_out_90_degrees(): assert np.isfinite(out[1]) -def test_angle_acc_out_matches_fast(): - out = np.zeros(2, dtype=np.float64) - _angle_acc_out(0.0, 0.0, 0.0, 2.0, 1.0, 0.0, 1.0, 2.0, 0.5, out) - angle, acc = angle_acc_fast(0.0, 0.0, 0.0, 2.0, 1.0, 0.0, 1.0, 2.0, 0.5) - assert abs(out[0] - angle) < 1e-10 - assert abs(out[1] - acc) < 1e-10 - - def test_angle_acc_out_norm0_zero(): out = np.zeros(2, dtype=np.float64) _angle_acc_out(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, out) @@ -1213,75 +1053,6 @@ def test_angle_acc_out_3d(): assert np.isfinite(out[1]) -# --------------------------------------------------------------------------- -# _ray_tracing_fast -# --------------------------------------------------------------------------- - - -def test_ray_tracing_fast_on_axis_tuple_len(): - cal = _make_cal_array() - result = _ray_tracing_fast(0.0, 0.0, cal) - assert len(result) == 6 - assert all(np.isfinite(v) for v in result) - - -def test_ray_tracing_fast_off_axis(): - cal = _make_cal_array() - Xx, Xy, Xz, ox, oy, oz = _ray_tracing_fast(1.0, 0.5, cal) - assert all(np.isfinite(v) for v in [Xx, Xy, Xz, ox, oy, oz]) - - -def test_ray_tracing_fast_negative_xy(): - cal = _make_cal_array() - result = _ray_tracing_fast(-2.0, -1.0, cal) - assert all(np.isfinite(v) for v in result) - - -def test_ray_tracing_fast_tilted_glass(): - """Tilted glass → non-trivial Snell refraction.""" - cal = _make_cal_array(gx=0.5, gy=0.0, gz=1.0) - Xx, Xy, Xz, ox, oy, oz = _ray_tracing_fast(1.0, 0.0, cal) - assert np.isfinite(Xx) - assert np.isfinite(ox) - - -def test_ray_tracing_fast_on_axis_bpn_zero(): - """x=0, y=0, dm=identity, glass=[0,0,1] → start_dir·glass parallel → bpn=0.""" - cal = _make_cal_array(gx=0.0, gy=0.0, gz=1.0) - result = _ray_tracing_fast(0.0, 0.0, cal) - assert len(result) == 6 - - -def test_ray_tracing_fast_varied_indices(): - cal = _make_cal_array(mm_n1=1.33, mm_n2_0=1.5, mm_n3=1.33, mm_d0=5.0) - result = _ray_tracing_fast(2.0, 1.0, cal) - assert all(np.isfinite(v) for v in result) - - -def test_ray_tracing_fast_large_xy(): - cal = _make_cal_array() - result = _ray_tracing_fast(8.0, 6.0, cal) - assert len(result) == 6 - - -def test_ray_tracing_fast_symmetry_x(): - """_ray_tracing_fast(-x, y) mirrors _ray_tracing_fast(x, y) in X.""" - cal = _make_cal_array() - Xx_p, Xy_p, Xz_p, _, _, _ = _ray_tracing_fast(2.0, 0.0, cal) - Xx_n, Xy_n, Xz_n, _, _, _ = _ray_tracing_fast(-2.0, 0.0, cal) - assert abs(Xx_p + Xx_n) < 1e-10 - assert abs(Xy_p - Xy_n) < 1e-10 - - -def test_ray_tracing_fast_zero_glass_gn_zero_branch(): - """gx=gy=gz=0 → gn=0 branch executed (raises ZeroDivision later — acceptable).""" - cal = _make_cal_array(gx=0.0, gy=0.0, gz=0.0) - try: - _ray_tracing_fast(1.0, 0.0, cal) - except (ZeroDivisionError, ValueError): - pass # branch covered; exception is expected - - # --------------------------------------------------------------------------- # _ray_tracing_out # --------------------------------------------------------------------------- @@ -1302,19 +1073,6 @@ def test_ray_tracing_out_off_axis(): assert all(np.isfinite(out[i]) for i in range(6)) -def test_ray_tracing_out_matches_fast(): - cal = _make_cal_array() - out = np.zeros(6, dtype=np.float64) - _ray_tracing_out(1.0, 0.5, cal, out) - Xx, Xy, Xz, ox, oy, oz = _ray_tracing_fast(1.0, 0.5, cal) - assert abs(out[0] - Xx) < 1e-10 - assert abs(out[1] - Xy) < 1e-10 - assert abs(out[2] - Xz) < 1e-10 - assert abs(out[3] - ox) < 1e-10 - assert abs(out[4] - oy) < 1e-10 - assert abs(out[5] - oz) < 1e-10 - - def test_ray_tracing_out_negative_xy(): cal = _make_cal_array() out = np.zeros(6, dtype=np.float64) diff --git a/tests/unit/test_track_kernels_search_coverage.py b/tests/unit/test_track_kernels_search_coverage.py index 66a6f951..e27dbbfb 100644 --- a/tests/unit/test_track_kernels_search_coverage.py +++ b/tests/unit/test_track_kernels_search_coverage.py @@ -74,22 +74,17 @@ # --------------------------------------------------------------------------- # Imports from the module under test # --------------------------------------------------------------------------- +from openptv2.algorithms.track_kernels_pixel import ( + _multimed_r_nlay_1layer, + _point_to_pixel_out, +) from openptv2.algorithms.track_kernels_search import ( _sorted_candidates_fast_out, - _sorted_candidates_fast_out_nogil, candsearch_in_pix_fast, - candsearch_in_pix_fast_nogil, candsearch_in_pix_rest_fast, - sort_candidates_by_freq_fast, sorted_candidates_fast, ) -if not _is_compiled(): - from openptv2.algorithms.track_kernels_search import ( - _multimed_r_nlay_1layer, - _point_to_pixel_out, - ) - EPS = 1e-8 # --------------------------------------------------------------------------- @@ -887,504 +882,6 @@ def test_large_num_targets(self): assert cnt == 1 -# --------------------------------------------------------------------------- -# sort_candidates_by_freq_fast -# --------------------------------------------------------------------------- - - -class TestSortCandidatesByFreqFast: - NUM_CAMS = 4 - MAX_CANDS = 4 - - def _make_arrays(self, ftnr_vals): - n = len(ftnr_vals) - ftnr = np.asarray(ftnr_vals, dtype=np.int32) - freq = np.zeros(n, dtype=np.int32) - whichcam = np.zeros((n, self.NUM_CAMS), dtype=np.int32) - return ftnr, freq, whichcam - - def test_all_unused(self): - ftnr, freq, whichcam = self._make_arrays([-1] * 16) - nv = sort_candidates_by_freq_fast( - ftnr, freq, whichcam, 16, self.NUM_CAMS, self.MAX_CANDS - ) - assert nv == 0 - - def test_single_candidate_one_cam(self): - # One candidate in camera 0 slot, rest unused - vals = [-1] * 16 - vals[0] = 5 # cam0, slot0 - ftnr, freq, whichcam = self._make_arrays(vals) - nv = sort_candidates_by_freq_fast( - ftnr, freq, whichcam, 16, self.NUM_CAMS, self.MAX_CANDS - ) - # Position 0 entry keeps freq=1 (dedup zeroes only j > i slots) - assert nv == 1 - - def test_same_candidate_two_cams(self): - # Target 10 appears in cam0 slot0 and cam1 slot0 - vals = [-1] * 16 - vals[0] = 10 # cam0 slot0 - vals[4] = 10 # cam1 slot0 - ftnr, freq, whichcam = self._make_arrays(vals) - nv = sort_candidates_by_freq_fast( - ftnr, freq, whichcam, 16, self.NUM_CAMS, self.MAX_CANDS - ) - assert nv >= 1 - - def test_same_candidate_all_cams(self): - # Target 7 seen in all 4 cameras - vals = [-1] * 16 - vals[0] = 7 # cam0 - vals[4] = 7 # cam1 - vals[8] = 7 # cam2 - vals[12] = 7 # cam3 - ftnr, freq, whichcam = self._make_arrays(vals) - nv = sort_candidates_by_freq_fast( - ftnr, freq, whichcam, 16, self.NUM_CAMS, self.MAX_CANDS - ) - assert nv >= 1 - # First entry should be target 7 with freq=4 - assert ftnr[0] == 7 - - def test_duplicate_elimination(self): - # Same target twice in same camera → should be deduped - vals = [-1] * 16 - vals[0] = 3 - vals[1] = 3 # duplicate - vals[4] = 3 # also in cam1 - ftnr, freq, whichcam = self._make_arrays(vals) - nv = sort_candidates_by_freq_fast( - ftnr, freq, whichcam, 16, self.NUM_CAMS, self.MAX_CANDS - ) - assert nv >= 1 - # Only one unique entry should remain - active = [(ftnr[i], freq[i]) for i in range(16) if freq[i] > 0] - tnrs = [t for t, f in active] - assert tnrs.count(3) <= 1 - - def test_sorting_higher_freq_first(self): - # Target A in 3 cams, target B in 2 cams — A should sort first - vals = [-1] * 16 - vals[0] = 20 # cam0: target A - vals[4] = 20 # cam1: target A - vals[8] = 20 # cam2: target A - vals[1] = 30 # cam0 slot1: target B - vals[5] = 30 # cam1 slot1: target B - ftnr, freq, whichcam = self._make_arrays(vals) - nv = sort_candidates_by_freq_fast( - ftnr, freq, whichcam, 16, self.NUM_CAMS, self.MAX_CANDS - ) - assert nv >= 1 - if nv >= 2: - assert freq[0] >= freq[1] - - def test_whichcam_swap(self): - # Test that whichcam is correctly swapped during sort - vals = [-1] * 16 - vals[0] = 5 - vals[4] = 5 - vals[1] = 6 - vals[5] = 6 - vals[9] = 6 - vals[13] = 6 # target 6 in all 4 cams → higher freq - ftnr, freq, whichcam = self._make_arrays(vals) - nv = sort_candidates_by_freq_fast( - ftnr, freq, whichcam, 16, self.NUM_CAMS, self.MAX_CANDS - ) - # Target 6 (freq=4) should sort before target 5 (freq=2) - if nv >= 1: - assert ftnr[0] == 6 - - def test_returns_nonzero_for_freq_one_at_position_zero(self): - # Dedup loop only eliminates j > i, so index 0 always kept if freq > 0 - vals = [-1] * 16 - vals[0] = 99 # only in 1 camera → freq=1 - ftnr, freq, whichcam = self._make_arrays(vals) - nv = sort_candidates_by_freq_fast( - ftnr, freq, whichcam, 16, self.NUM_CAMS, self.MAX_CANDS - ) - # freq[0]=1 ≠ 0 → counted as valid - assert nv == 1 - - def test_freq_one_entry_after_another_gets_zeroed(self): - # Second unique candidate at freq=1 is eliminated by dedup loop at j>i - vals = [-1] * 16 - vals[0] = 10 # cam0: target 10 in 2 cams → freq=2 - vals[4] = 10 # cam1 - vals[1] = 99 # cam0 slot1: target 99 in 1 cam → freq=1 - ftnr, freq, whichcam = self._make_arrays(vals) - nv = sort_candidates_by_freq_fast( - ftnr, freq, whichcam, 16, self.NUM_CAMS, self.MAX_CANDS - ) - # Target 10 (freq=2) survives; target 99 (freq=1) is zeroed - assert nv >= 1 - - -# --------------------------------------------------------------------------- -# candsearch_in_pix_fast_nogil (cfunc — callable in pure Python) -# --------------------------------------------------------------------------- - - -@_needs_pure_python_loose_types -class TestCandsearchInPixFastNogil: - """Tests for the nogil cfunc variant. Pass a list for out_indices.""" - - IMX, IMY = 1024.0, 1024.0 - - def test_center_out_of_bounds(self): - tx = np.array([100.0], dtype=np.float64) - ty = np.array([100.0], dtype=np.float64) - tnr = np.array([0], dtype=np.int32) - out = [0, 0, 0, 0] - candsearch_in_pix_fast_nogil( - tx, - ty, - tnr, - 1, - 2000.0, - 512.0, - 10.0, - 10.0, - 10.0, - 10.0, - self.IMX, - self.IMY, - TR, - out, - ) - assert all(v == -999 for v in out) - - def test_empty_targets(self): - tx = np.zeros(0, dtype=np.float64) - ty = np.zeros(0, dtype=np.float64) - tnr = np.zeros(0, dtype=np.int32) - out = [0, 0, 0, 0] - candsearch_in_pix_fast_nogil( - tx, - ty, - tnr, - 0, - 512.0, - 512.0, - 10.0, - 10.0, - 10.0, - 10.0, - self.IMX, - self.IMY, - TR, - out, - ) - assert out[0] == -999 - - def test_single_target_found(self): - tx, ty, tnr = _make_targets([500.0], [500.0], [5]) - out = [-999, -999, -999, -999] - candsearch_in_pix_fast_nogil( - tx, - ty, - tnr, - 1, - 500.0, - 500.0, - 20.0, - 20.0, - 20.0, - 20.0, - self.IMX, - self.IMY, - TR, - out, - ) - assert out[0] == 0 # index 0 in sorted arrays - assert out[1] == -999 - - def test_unused_tnr_skipped(self): - tx, ty, tnr = _make_targets([500.0], [500.0], [TR]) - out = [-999, -999, -999, -999] - candsearch_in_pix_fast_nogil( - tx, - ty, - tnr, - 1, - 500.0, - 500.0, - 20.0, - 20.0, - 20.0, - 20.0, - self.IMX, - self.IMY, - TR, - out, - ) - assert out[0] == -999 - - def test_four_targets_fills_all_slots(self): - n = 5 - xs = [500.0, 501.0, 502.0, 503.0, 504.0] - ys = [500.0, 500.1, 500.2, 500.3, 500.4] - tnrs = [10, 11, 12, 13, 14] - tx, ty, tnr = _make_targets(xs, ys, tnrs) - out = [-999, -999, -999, -999] - candsearch_in_pix_fast_nogil( - tx, - ty, - tnr, - n, - 500.0, - 500.0, - 10.0, - 10.0, - 10.0, - 10.0, - self.IMX, - self.IMY, - TR, - out, - ) - found = [v for v in out if v != -999] - assert len(found) == 4 - - def test_d2_replacement(self): - # Target 1 is closest, target 2 is second closest - tx, ty, tnr = _make_targets([500.0, 500.5], [500.0, 500.5], [1, 2]) - out = [-999, -999, -999, -999] - candsearch_in_pix_fast_nogil( - tx, - ty, - tnr, - 2, - 500.0, - 500.0, - 10.0, - 10.0, - 10.0, - 10.0, - self.IMX, - self.IMY, - TR, - out, - ) - found = [v for v in out if v != -999] - assert len(found) == 2 - - def test_d3_replacement(self): - tx, ty, tnr = _make_targets( - [500.0, 500.5, 501.0], [500.0, 500.5, 501.0], [1, 2, 3] - ) - out = [-999, -999, -999, -999] - candsearch_in_pix_fast_nogil( - tx, - ty, - tnr, - 3, - 500.0, - 500.0, - 10.0, - 10.0, - 10.0, - 10.0, - self.IMX, - self.IMY, - TR, - out, - ) - found = [v for v in out if v != -999] - assert len(found) == 3 - - def test_ymax_early_break(self): - tx, ty, tnr = _make_targets([500.0, 500.0], [502.0, 600.0], [1, 2]) - out = [-999, -999, -999, -999] - candsearch_in_pix_fast_nogil( - tx, - ty, - tnr, - 2, - 500.0, - 500.0, - 5.0, - 5.0, - 5.0, - 5.0, - self.IMX, - self.IMY, - TR, - out, - ) - found = [v for v in out if v != -999] - assert len(found) <= 1 - - def test_large_num_targets(self): - n = 60 - xs = np.linspace(490.0, 510.0, n) - ys = np.linspace(490.0, 510.0, n) - tnrs = np.arange(n, dtype=np.int32) - tx, ty, tnr = _make_targets(xs, ys, tnrs) - out = [-999, -999, -999, -999] - candsearch_in_pix_fast_nogil( - tx, - ty, - tnr, - n, - 500.0, - 500.0, - 12.0, - 12.0, - 12.0, - 12.0, - self.IMX, - self.IMY, - TR, - out, - ) - found = [v for v in out if v != -999] - assert len(found) == 4 - - def test_ymin_clamp(self): - """cent_y < du → ymin = 0.0 (covers line 1057).""" - tx, ty, tnr = _make_targets([512.0], [5.0], [1]) - out = [-999, -999, -999, -999] - candsearch_in_pix_fast_nogil( - tx, - ty, - tnr, - 1, - 512.0, - 5.0, - 10.0, - 10.0, - 20.0, - 10.0, - self.IMX, - self.IMY, - TR, - out, - ) - assert out[0] != -999 - - def test_ymax_clamp(self): - """cent_y + dd > imy → ymax = imy (covers line 1059).""" - tx, ty, tnr = _make_targets([512.0], [1020.0], [1]) - out = [-999, -999, -999, -999] - candsearch_in_pix_fast_nogil( - tx, - ty, - tnr, - 1, - 512.0, - 1020.0, - 10.0, - 10.0, - 10.0, - 20.0, - self.IMX, - self.IMY, - TR, - out, - ) - assert out[0] != -999 - - def test_xmin_clamp(self): - """cent_x < dl → xmin = 0.0 (covers line 1053).""" - tx, ty, tnr = _make_targets([5.0], [512.0], [1]) - out = [-999, -999, -999, -999] - candsearch_in_pix_fast_nogil( - tx, - ty, - tnr, - 1, - 5.0, - 512.0, - 100.0, - 10.0, - 10.0, - 10.0, - self.IMX, - self.IMY, - TR, - out, - ) - assert out[0] != -999 - - def test_binary_search_j0_increment(self): - """Binary search j0 += dj (covers line 1081) and j0 no-clamp (1087->1090).""" - n = 40 - xs = np.full(n, 512.0) - ys = np.linspace(100.0, 200.0, n) - tnrs = np.arange(n, dtype=np.int32) - tx, ty, tnr = _make_targets(xs, ys, tnrs) - out = [-999, -999, -999, -999] - candsearch_in_pix_fast_nogil( - tx, - ty, - tnr, - n, - 512.0, - 190.0, - 5.0, - 5.0, - 5.0, - 5.0, - self.IMX, - self.IMY, - TR, - out, - ) - found = [v for v in out if v != -999] - assert len(found) >= 1 - - def test_j0_no_clamp(self): - """num_targets >= 24 → j0 - 12 >= 0 (covers branch 1087->1090).""" - n = 50 - xs = np.full(n, 512.0) - ys = np.linspace(500.0, 510.0, n) - tnrs = np.arange(n, dtype=np.int32) - tx, ty, tnr = _make_targets(xs, ys, tnrs) - out = [-999, -999, -999, -999] - candsearch_in_pix_fast_nogil( - tx, - ty, - tnr, - n, - 512.0, - 505.0, - 5.0, - 5.0, - 5.0, - 5.0, - self.IMX, - self.IMY, - TR, - out, - ) - found = [v for v in out if v != -999] - assert len(found) >= 1 - - def test_target_in_y_out_of_x(self): - """Target in y range but not x → False branch at tx check (covers 1096->1090).""" - tx, ty, tnr = _make_targets([900.0], [500.0], [1]) - out = [-999, -999, -999, -999] - candsearch_in_pix_fast_nogil( - tx, - ty, - tnr, - 1, - 500.0, - 500.0, - 10.0, - 10.0, - 10.0, - 10.0, - self.IMX, - self.IMY, - TR, - out, - ) - assert all(v == -999 for v in out) - - # --------------------------------------------------------------------------- # sorted_candidates_fast — calls _sorted_candidates_fast_out which crashes # --------------------------------------------------------------------------- @@ -1529,82 +1026,6 @@ def test_runs_with_two_cams(self): _sorted_candidates_fast_out(*args) -@_needs_pure_python_loose_types -class TestSortedCandidatesFastOutNogil: - """_sorted_candidates_fast_out_nogil runs in pure Python (C-array bug fixed 2026-07-10).""" - - def _make_nogil_args(self, num_cams=1, max_cands=4, n_targ=5): - n = num_cams * max_cands - center = np.array([0.0, 0.0, 100.0], dtype=np.float64) - center_proj_x = np.zeros(num_cams, dtype=np.float64) - center_proj_y = np.zeros(num_cams, dtype=np.float64) - - cal_arr = np.zeros((num_cams, 31), dtype=np.float64, order="C") - for i in range(num_cams): - cal_arr[i] = _make_cal() - - # nogil variant takes up to 8 separate md arrays - empty_md = np.zeros(4, dtype=np.float64) - md_list = [empty_md] * 8 - - mo_arr = np.zeros((num_cams, 3), dtype=np.float64, order="C") - mnr_arr = np.zeros(num_cams, dtype=np.int32) - mnz_arr = np.zeros(num_cams, dtype=np.int32) - mrw_arr = np.ones(num_cams, dtype=np.float64) - - targ_x = np.zeros((num_cams, n_targ), dtype=np.float64, order="C") - targ_y = np.zeros((num_cams, n_targ), dtype=np.float64, order="C") - targ_tnr = np.full((num_cams, n_targ), -1, dtype=np.int32, order="C") - num_targets = np.zeros(num_cams, dtype=np.int32) - - ftnr_out = np.full(n, -1, dtype=np.int32) - freq_out = np.zeros(n, dtype=np.int32) - whichcam_out = np.zeros((n, num_cams), dtype=np.int32) - - return ( - center, - center_proj_x, - center_proj_y, - num_cams, - max_cands, - cal_arr, - *md_list, # md0..md7 - mo_arr, - mnr_arr, - mnz_arr, - mrw_arr, - targ_x, - targ_y, - targ_tnr, - num_targets, - -1.0, - 1.0, - -1.0, - 1.0, - -1.0, - 1.0, - 512.0, - 512.0, - 1.0, - 1.0, - 0, - 1024.0, - 1024.0, - -1, - ftnr_out, - freq_out, - whichcam_out, - ) - - def test_runs_without_error(self): - args = self._make_nogil_args() - _sorted_candidates_fast_out_nogil(*args) - - def test_runs_with_two_cams(self): - args = self._make_nogil_args(num_cams=2) - _sorted_candidates_fast_out_nogil(*args) - - # --------------------------------------------------------------------------- # Compiled-mode sanity check (this module should be skipped when compiled) # --------------------------------------------------------------------------- diff --git a/tests/unit/test_track_kernels_tracking_coverage.py b/tests/unit/test_track_kernels_tracking_coverage.py index 9fa217ed..d1ea5671 100644 --- a/tests/unit/test_track_kernels_tracking_coverage.py +++ b/tests/unit/test_track_kernels_tracking_coverage.py @@ -1,18 +1,8 @@ -"""Pure-Python coverage tests for track_kernels_tracking.py. +"""Pure-Python coverage tests for the tracking kernels (corr/pixel/position/track3d). Skip when the compiled .so is active (coverage measures the .py source). -Verification command (from repo root): - cp src/openptv2/algorithms/track_kernels_tracking.py \ - /tmp/ppsrc/openptv2/algorithms/track_kernels_tracking.py - COVERAGE_FILE=/tmp/.cov_track_kernels_tracking \ - uv run pytest tests/unit/test_track_kernels_tracking_coverage.py \ - -o pythonpath=/tmp/ppsrc \ - -p no:cacheprovider \ - --cov=/tmp/ppsrc/openptv2 \ - --cov-config=/tmp/covrc \ - --cov-report=term-missing \ - -q 2>&1 | grep -E '(algorithms/track_kernels_tracking\\.|TOTAL|passed|failed|error)' +Run the interpreted-source variant per openptv2/CLAUDE.md (pure-Python fallback tests). """ import numpy as np @@ -34,34 +24,44 @@ ) import openptv2.algorithms.track_kernels_corr as _corr_mod -import openptv2.algorithms.track_kernels_tracking as _mod -from openptv2.algorithms.track_kernels_tracking import ( - ADD_PART_K, - COORD_UNUSED_K, - CORRES_NONE_K, - MAX_CANDS_K, - NEXT_NONE_K, - POSI_K, - PREV_NONE_K, - PT_UNUSED, - TR_UNUSED_K, - _angle_acc_out, +from openptv2.algorithms.track_kernels_corr import ( + trackback_loop_fast, + trackcorr_loop_fast, +) +from openptv2.algorithms.track_kernels_pixel import ( _candsearch_in_pix_rest_nogil, _dist_to_flat_out, - _find_closest_in_3d, _multimed_r_nlay_1layer, _pixel_to_metric_out, - _point_position_out, _point_to_pixel_out, - _ray_tracing_out, _sorted_candidates_fast_out_nogil, - assess_new_position_fast_nogil, candsearch_in_pix_fast_nogil, +) +from openptv2.algorithms.track_kernels_position import ( + _angle_acc_out, + _point_position_out, + _ray_tracing_out, + assess_new_position_fast_nogil, +) +from openptv2.algorithms.track_kernels_track3d import ( + _find_closest_in_3d, track3d_loop_fast, - trackback_loop_fast, - trackcorr_loop_fast, ) +# These mirror the cython.declare() C-level constants in track_kernels_corr, +# which are not importable from Python when compiled. +PT_UNUSED = -999 +POSI_K = 80 +MAX_CANDS_K = 32 +TR_UNUSED_K = -1 +CORRES_NONE_K = -1 +PREV_NONE_K = -1 +NEXT_NONE_K = -2 +COORD_UNUSED_K = -1e10 +ADD_PART_K = 3.0 + +import openptv2.algorithms.track_kernels_pixel as _mod # noqa: E402 + # --------------------------------------------------------------------------- # Helpers # --------------------------------------------------------------------------- @@ -148,6 +148,7 @@ def _frame(n, nc, n_targ, max_cands=4, x_offset=0.0): targ_x=np.zeros((nc, t_), dtype=np.float64, order="C"), targ_y=np.zeros((nc, t_), dtype=np.float64, order="C"), targ_tnr=np.full((nc, t_), -1, dtype=np.int32, order="C"), + targ_sumg=np.zeros((nc, t_), dtype=np.float64, order="C"), num_targets=np.zeros(nc, dtype=np.int32), num_parts=np.array([n], dtype=np.int32), ) @@ -184,6 +185,7 @@ def _do_call(): f1["targ_x"], f1["targ_y"], f1["targ_tnr"], + f1["targ_sumg"], # frame 2 f2["path_x"], f2["path_prev"], @@ -198,6 +200,8 @@ def _do_call(): f2["targ_x"], f2["targ_y"], f2["targ_tnr"], + f2["targ_sumg"], + np.full(f2["path_x"].shape[0], -1.0, dtype=np.float64), f2["num_targets"], f2["num_parts"], # frame 3 @@ -1373,6 +1377,7 @@ def test_trackcorr_stub_zero_with_prev(): f1["targ_x"], f1["targ_y"], f1["targ_tnr"], + f1["targ_sumg"], f2["path_x"], f2["path_prev"], f2["path_next"], @@ -1386,6 +1391,8 @@ def test_trackcorr_stub_zero_with_prev(): f2["targ_x"], f2["targ_y"], f2["targ_tnr"], + f2["targ_sumg"], + np.full(f2["path_x"].shape[0], -1.0, dtype=np.float64), f2["num_targets"], f2["num_parts"], f3["path_x"], @@ -1474,6 +1481,7 @@ def test_trackcorr_stub_zero_corres_p_path(): f1["targ_x"], f1["targ_y"], f1["targ_tnr"], + f1["targ_sumg"], f2["path_x"], f2["path_prev"], f2["path_next"], @@ -1487,6 +1495,8 @@ def test_trackcorr_stub_zero_corres_p_path(): f2["targ_x"], f2["targ_y"], f2["targ_tnr"], + f2["targ_sumg"], + np.full(f2["path_x"].shape[0], -1.0, dtype=np.float64), f2["num_targets"], f2["num_parts"], f3["path_x"], @@ -1843,6 +1853,7 @@ def _stub(*a, **k): f1["targ_x"], f1["targ_y"], f1["targ_tnr"], + f1["targ_sumg"], f2["path_x"], f2["path_prev"], f2["path_next"], @@ -1856,6 +1867,8 @@ def _stub(*a, **k): f2["targ_x"], f2["targ_y"], f2["targ_tnr"], + f2["targ_sumg"], + np.full(f2["path_x"].shape[0], -1.0, dtype=np.float64), f2["num_targets"], f2["num_parts"], f3["path_x"], diff --git a/tests/unit/test_track_kernels_transform_coverage.py b/tests/unit/test_track_kernels_transform_coverage.py index 196d8f7f..91e6eea2 100644 --- a/tests/unit/test_track_kernels_transform_coverage.py +++ b/tests/unit/test_track_kernels_transform_coverage.py @@ -52,27 +52,20 @@ # --------------------------------------------------------------------------- # Imports under test # --------------------------------------------------------------------------- -from openptv2.algorithms.track_kernels_transform import ( +from openptv2.algorithms.track_kernels_pixel import ( _candsearch_in_pix_rest_nogil, - _flat_image_coord_fast, - _img_coord_fast, - _metric_to_pixel_out, + _dist_to_flat_out, _multimed_r_nlay_1layer, _pixel_to_metric_out, +) +from openptv2.algorithms.track_kernels_position import ( _point_position_out, _ray_tracing_out, - assess_new_position_fast, assess_new_position_fast_nogil, - dist_to_flat_fast, - flat_image_coord_batch_fast, - img_coord_batch_fast, - metric_to_pixel_fast, - pixel_to_metric_fast, - point_position_fast, ) - -if not _is_compiled(): - from openptv2.algorithms.track_kernels_transform import _dist_to_flat_out +from openptv2.algorithms.track_kernels_transform import ( + assess_new_position_fast, +) # --------------------------------------------------------------------------- # Helpers: build a minimal 31-element calibration flat array @@ -443,64 +436,6 @@ def test_num_used_positive_branch(self): assert out is not None -class TestPointPositionFast: - """Cover point_position_fast (line 495) — wraps _point_position_out.""" - - def test_returns_pos_and_dist(self): - """Smoke test: returns (pos_array, dist_float).""" - cal_arr = _make_cal_arr_batch(2) - targets = np.full((2, 2), COORD_UNUSED, dtype=np.float64) - pos, dist = point_position_fast(targets, 2, cal_arr) - assert pos.shape == (3,) - assert dist == 0.0 - - def test_two_cams_valid_targets(self): - """Two cameras with valid targets → non-trivial result.""" - cal_arr = _make_cal_arr_batch(2) - targets = np.zeros((2, 2), dtype=np.float64) - pos, dist = point_position_fast(targets, 2, cal_arr) - assert pos.shape == (3,) - assert isinstance(dist, float) - - -# --------------------------------------------------------------------------- -# 4. pixel_to_metric_fast -# --------------------------------------------------------------------------- - - -class TestPixelToMetricFast: - def test_chfield_zero(self): - x_m, y_m = pixel_to_metric_fast(512.0, 384.0, 1024, 768, 0.01, 0.01, 0) - assert math.isclose(x_m, 0.0, abs_tol=1e-10) - assert math.isclose(y_m, 0.0, abs_tol=1e-10) - - def test_chfield_one(self): - """chfield==1: yp = 2*y_pixel + 1.""" - x_m, y_m = pixel_to_metric_fast(512.0, 100.0, 1024, 768, 0.01, 0.01, 1) - # yp = 2*100 + 1 = 201 - expected_y = (768 * 0.5 - 201) * 0.01 - assert math.isclose(y_m, expected_y, rel_tol=1e-9) - - def test_chfield_two(self): - """chfield==2: yp = 2*y_pixel.""" - x_m, y_m = pixel_to_metric_fast(512.0, 100.0, 1024, 768, 0.01, 0.01, 2) - yp = 2.0 * 100.0 - expected_y = (768 * 0.5 - yp) * 0.01 - assert math.isclose(y_m, expected_y, rel_tol=1e-9) - - def test_origin_pixel(self): - """Pixel at image centre → metric (0,0).""" - x_m, y_m = pixel_to_metric_fast(512.0, 384.0, 1024, 768, 0.01, 0.01, 0) - assert abs(x_m) < 1e-10 - assert abs(y_m) < 1e-10 - - def test_corner_pixel(self): - """Pixel at (0,0) → negative metric coords.""" - x_m, y_m = pixel_to_metric_fast(0.0, 0.0, 1024, 768, 0.01, 0.01, 0) - assert x_m < 0.0 - assert y_m > 0.0 - - # --------------------------------------------------------------------------- # 5. _pixel_to_metric_out # --------------------------------------------------------------------------- @@ -533,64 +468,6 @@ def test_returns_zero(self): assert ret == 0 -# --------------------------------------------------------------------------- -# 6. dist_to_flat_fast -# --------------------------------------------------------------------------- - - -class TestDistToFlatFast: - def test_r_near_zero_returns_minus_xh_yh(self): - """Very small dist_x/dist_y → returns (-xh, -yh).""" - xh, yh = 0.5, -0.3 - x, y = dist_to_flat_fast( - 1e-15, 0.0, xh, yh, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 1e-6 - ) - assert math.isclose(x, -xh, rel_tol=1e-9) - assert math.isclose(y, -yh, rel_tol=1e-9) - - def test_zero_distortion_identity(self): - """k=p=0, scx=1, she=0 → output ≈ input (modulo xh/yh).""" - x, y = dist_to_flat_fast( - 1.0, 2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 1e-8 - ) - assert math.isclose(x, 1.0, rel_tol=1e-5) - assert math.isclose(y, 2.0, rel_tol=1e-5) - - def test_with_principal_point_offset(self): - """Non-zero xh/yh shifts the result.""" - xh, yh = 0.1, 0.2 - x, y = dist_to_flat_fast( - 1.0, 2.0, xh, yh, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 1e-8 - ) - # xq starts at 1.0 and converges; result should be near 1.0 - xh - assert abs(x - (1.0 - xh)) < 0.01 - - def test_with_k1_distortion(self): - """Non-zero k1 changes the result noticeably.""" - x0, y0 = dist_to_flat_fast( - 2.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 1e-8 - ) - x1, y1 = dist_to_flat_fast( - 2.0, 1.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 1e-8 - ) - assert x0 != x1 - - def test_she_nonzero(self): - """Non-zero shear angle exercises sin/cos branches.""" - x, y = dist_to_flat_fast( - 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.01, 1e-8 - ) - assert isinstance(x, float) - assert isinstance(y, float) - - def test_convergence_tol(self): - """Tight tolerance converges; result shifts slightly due to k1 correction.""" - x, y = dist_to_flat_fast( - 0.5, 0.5, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 1e-12 - ) - assert math.isclose(x, 0.5, abs_tol=0.01) - - # --------------------------------------------------------------------------- # 7. _dist_to_flat_out # --------------------------------------------------------------------------- @@ -626,269 +503,6 @@ def test_with_distortion(self): assert math.isclose(out[0], 1.5, rel_tol=0.05) -# --------------------------------------------------------------------------- -# 8. metric_to_pixel_fast -# --------------------------------------------------------------------------- - - -class TestMetricToPixelFast: - def test_chfield_zero(self): - """metric_to_pixel_fast is inverse of pixel_to_metric_fast.""" - xp0, yp0 = 200.0, 300.0 - xm, ym = pixel_to_metric_fast(xp0, yp0, 1024, 768, 0.01, 0.01, 0) - xp1, yp1 = metric_to_pixel_fast(xm, ym, 1024, 768, 0.01, 0.01, 0) - assert math.isclose(xp1, xp0, rel_tol=1e-9) - assert math.isclose(yp1, yp0, rel_tol=1e-9) - - def test_chfield_one(self): - xp, yp = metric_to_pixel_fast(0.0, 0.0, 1024, 768, 0.01, 0.01, 1) - # x_pixel = 0/0.01 + 512 = 512 - assert math.isclose(xp, 512.0, rel_tol=1e-9) - # y_pixel raw = 768/2 - 0/0.01 = 384 → (384 - 1) / 2 = 191.5 - assert math.isclose(yp, (384.0 - 1.0) * 0.5, rel_tol=1e-9) - - def test_chfield_two(self): - xp, yp = metric_to_pixel_fast(0.0, 0.0, 1024, 768, 0.01, 0.01, 2) - # y_pixel raw = 384 → 384 * 0.5 = 192 - assert math.isclose(yp, 192.0, rel_tol=1e-9) - - def test_off_centre(self): - xp, yp = metric_to_pixel_fast(1.0, 0.0, 1024, 768, 0.01, 0.01, 0) - assert math.isclose(xp, 512.0 + 100.0, rel_tol=1e-9) - - -# --------------------------------------------------------------------------- -# 9. _metric_to_pixel_out -# --------------------------------------------------------------------------- - - -class TestMetricToPixelOut: - def test_chfield_zero(self): - out = np.zeros(2, dtype=np.float64) - _metric_to_pixel_out(0.0, 0.0, 1024, 768, 0.01, 0.01, 0, out) - assert math.isclose(out[0], 512.0, rel_tol=1e-9) - assert math.isclose(out[1], 384.0, rel_tol=1e-9) - - def test_chfield_one(self): - out = np.zeros(2, dtype=np.float64) - _metric_to_pixel_out(0.0, 0.0, 1024, 768, 0.01, 0.01, 1, out) - assert math.isclose(out[1], (384.0 - 1.0) * 0.5, rel_tol=1e-9) - - def test_chfield_two(self): - out = np.zeros(2, dtype=np.float64) - _metric_to_pixel_out(0.0, 0.0, 1024, 768, 0.01, 0.01, 2, out) - assert math.isclose(out[1], 192.0, rel_tol=1e-9) - - -# --------------------------------------------------------------------------- -# 10. _flat_image_coord_fast -# --------------------------------------------------------------------------- - - -class TestFlatImageCoordFast: - def _pos(self, x=0.0, y=0.0, z=0.0): - return np.array([x, y, z], dtype=np.float64) - - def _empty_mmlut(self): - return np.array([], dtype=np.float64), np.zeros(3, dtype=np.float64), 0, 0, 1.0 - - def _filled_mmlut(self, factor=1.0): - """2x2 LUT with constant factor.""" - data = np.full(4, factor, dtype=np.float64) - origin = np.zeros(3, dtype=np.float64) - return data, origin, 2, 2, 1000.0 - - def test_basic_no_mmlut(self): - cal = _make_cal_arr(x0=0.0, y0=0.0, z0=100.0, gz=50.0) - pos = self._pos(0.0, 0.0, 0.0) - mmlut_data, mmlut_origin, nr, nz, rw = self._empty_mmlut() - x, y = _flat_image_coord_fast(pos, cal, mmlut_data, mmlut_origin, nr, nz, rw) - assert isinstance(x, float) - assert isinstance(y, float) - - def test_with_mmlut_in_bounds(self): - """LUT in-bounds path (has_mmlut=True, mmf > 0).""" - cal = _make_cal_arr(x0=0.0, y0=0.0, z0=100.0, gz=50.0) - pos = self._pos(1.0, 0.0, -10.0) - mmlut_data, mmlut_origin, nr, nz, rw = self._filled_mmlut(1.2) - x, y = _flat_image_coord_fast(pos, cal, mmlut_data, mmlut_origin, nr, nz, rw) - assert isinstance(x, float) - - def test_with_mmlut_zero_factor(self): - """LUT path where mmf == 0 → falls back to _multimed_r_nlay_1layer.""" - cal = _make_cal_arr(x0=0.0, y0=0.0, z0=100.0, gz=50.0) - pos = self._pos(1.0, 0.0, -10.0) - mmlut_data = np.zeros(4, dtype=np.float64) # mmf == 0 - mmlut_origin = np.zeros(3, dtype=np.float64) - x, y = _flat_image_coord_fast(pos, cal, mmlut_data, mmlut_origin, 2, 2, 1000.0) - assert isinstance(x, float) - - def test_pos_t_0_zero_branch(self): - """pos_t_0 == 0 → the s_x branch is skipped.""" - # Place the point directly along the glass normal from the camera projection - cal = _make_cal_arr(x0=0.0, y0=0.0, z0=100.0, gz=50.0) - pos = self._pos(0.0, 0.0, 0.0) - mmlut_data, mmlut_origin, nr, nz, rw = self._empty_mmlut() - x, y = _flat_image_coord_fast(pos, cal, mmlut_data, mmlut_origin, nr, nz, rw) - assert isinstance(x, float) - - def test_mmlut_out_of_bounds(self): - """LUT v3 > nr*nz → skip LUT, fall back to iterative solver.""" - cal = _make_cal_arr(x0=0.0, y0=0.0, z0=100.0, gz=50.0) - pos = self._pos(500.0, 0.0, -10.0) # large R - # Small LUT so ir > mmlut_nr - data = np.ones(4, dtype=np.float64) - origin = np.zeros(3, dtype=np.float64) - x, y = _flat_image_coord_fast(pos, cal, data, origin, 1, 2, 0.001) - assert isinstance(x, float) - - def test_radial_shift_one_fallback(self): - """When mmlut lookup gives radial_shift still == 1.0, falls through to - _multimed_r_nlay_1layer.""" - cal = _make_cal_arr( - x0=0.0, y0=0.0, z0=100.0, gz=50.0, n1=1.0, n2_0=1.5, n3=1.33, d0=2.0 - ) - pos = self._pos(2.0, 1.0, -5.0) - mmlut_data, mmlut_origin, nr, nz, rw = self._empty_mmlut() - x, y = _flat_image_coord_fast(pos, cal, mmlut_data, mmlut_origin, nr, nz, rw) - assert isinstance(x, float) - - -# --------------------------------------------------------------------------- -# 11. _img_coord_fast -# --------------------------------------------------------------------------- - - -class TestImgCoordFast: - def _empty_mmlut(self): - return np.array([], dtype=np.float64), np.zeros(3, dtype=np.float64), 0, 0, 1.0 - - def test_r_near_zero_returns_zero(self): - """_flat_image_coord_fast returns x≈0, y≈0 → r < 1e-10 → (0,0).""" - cal = _make_cal_arr(x0=0.0, y0=0.0, z0=100.0, gz=50.0, xh=0.0, yh=0.0) - pos = np.array([0.0, 0.0, 0.0], dtype=np.float64) - mmlut_data, mmlut_origin, nr, nz, rw = self._empty_mmlut() - xd, yd = _img_coord_fast(pos, cal, mmlut_data, mmlut_origin, nr, nz, rw) - assert xd == 0.0 and yd == 0.0 - - def test_normal_case(self): - cal = _make_cal_arr( - x0=0.0, - y0=0.0, - z0=100.0, - gz=50.0, - xh=0.0, - yh=0.0, - k1=0.001, - scx=1.0, - she=0.0, - ) - pos = np.array([1.0, 2.0, 0.0], dtype=np.float64) - mmlut_data, mmlut_origin, nr, nz, rw = self._empty_mmlut() - xd, yd = _img_coord_fast(pos, cal, mmlut_data, mmlut_origin, nr, nz, rw) - assert isinstance(xd, float) - assert isinstance(yd, float) - - def test_with_she_nonzero(self): - cal = _make_cal_arr(x0=0.0, y0=0.0, z0=100.0, gz=50.0, she=0.05) - pos = np.array([2.0, 1.0, 0.0], dtype=np.float64) - mmlut_data, mmlut_origin, nr, nz, rw = self._empty_mmlut() - xd, yd = _img_coord_fast(pos, cal, mmlut_data, mmlut_origin, nr, nz, rw) - assert isinstance(xd, float) - - -# --------------------------------------------------------------------------- -# 12. img_coord_batch_fast -# --------------------------------------------------------------------------- - - -class TestImgCoordBatchFast: - def _empty_mmlut(self): - return np.array([], dtype=np.float64), np.zeros(3, dtype=np.float64), 0, 0, 1.0 - - def test_empty_batch(self): - cal = _make_cal_arr() - positions = np.empty((0, 3), dtype=np.float64, order="C") - mmlut_data, mmlut_origin, nr, nz, rw = self._empty_mmlut() - result = img_coord_batch_fast( - positions, cal, mmlut_data, mmlut_origin, nr, nz, rw - ) - assert result.shape == (0, 2) - - def test_single_point(self): - cal = _make_cal_arr(gz=50.0) - positions = np.array([[1.0, 2.0, 0.0]], dtype=np.float64, order="C") - mmlut_data, mmlut_origin, nr, nz, rw = self._empty_mmlut() - result = img_coord_batch_fast( - positions, cal, mmlut_data, mmlut_origin, nr, nz, rw - ) - assert result.shape == (1, 2) - assert result.dtype == np.float64 - - def test_multiple_points(self): - cal = _make_cal_arr(gz=50.0) - positions = np.array( - [ - [1.0, 0.0, 0.0], - [2.0, 1.0, -5.0], - [0.5, -0.5, 3.0], - ], - dtype=np.float64, - order="C", - ) - mmlut_data, mmlut_origin, nr, nz, rw = self._empty_mmlut() - result = img_coord_batch_fast( - positions, cal, mmlut_data, mmlut_origin, nr, nz, rw - ) - assert result.shape == (3, 2) - - -# --------------------------------------------------------------------------- -# 13. flat_image_coord_batch_fast -# --------------------------------------------------------------------------- - - -class TestFlatImageCoordBatchFast: - def _empty_mmlut(self): - return np.array([], dtype=np.float64), np.zeros(3, dtype=np.float64), 0, 0, 1.0 - - def test_empty_batch(self): - cal = _make_cal_arr() - positions = np.empty((0, 3), dtype=np.float64, order="C") - mmlut_data, mmlut_origin, nr, nz, rw = self._empty_mmlut() - result = flat_image_coord_batch_fast( - positions, cal, mmlut_data, mmlut_origin, nr, nz, rw - ) - assert result.shape == (0, 2) - - def test_single_point(self): - cal = _make_cal_arr(gz=50.0) - positions = np.array([[0.0, 0.0, 0.0]], dtype=np.float64, order="C") - mmlut_data, mmlut_origin, nr, nz, rw = self._empty_mmlut() - result = flat_image_coord_batch_fast( - positions, cal, mmlut_data, mmlut_origin, nr, nz, rw - ) - assert result.shape == (1, 2) - - def test_multiple_points(self): - cal = _make_cal_arr(gz=50.0) - positions = np.array( - [ - [1.0, 0.0, 0.0], - [0.0, 1.0, 0.0], - [2.0, 2.0, -10.0], - [0.0, 0.0, 5.0], - ], - dtype=np.float64, - order="C", - ) - mmlut_data, mmlut_origin, nr, nz, rw = self._empty_mmlut() - result = flat_image_coord_batch_fast( - positions, cal, mmlut_data, mmlut_origin, nr, nz, rw - ) - assert result.shape == (4, 2) - - # --------------------------------------------------------------------------- # 14. _candsearch_in_pix_rest_nogil # --------------------------------------------------------------------------- @@ -1568,130 +1182,11 @@ def test_constants(): # --------------------------------------------------------------------------- -def test_pixel_metric_pixel_roundtrip(): - """metric_to_pixel(pixel_to_metric(px, py)) == (px, py) for chfield 0.""" - for chfield in [0]: - px0, py0 = 300.0, 200.0 - xm, ym = pixel_to_metric_fast(px0, py0, 1024, 768, 0.01, 0.01, chfield) - px1, py1 = metric_to_pixel_fast(xm, ym, 1024, 768, 0.01, 0.01, chfield) - assert math.isclose(px1, px0, rel_tol=1e-9) - assert math.isclose(py1, py0, rel_tol=1e-9) - - # --------------------------------------------------------------------------- # 19. dist_to_flat / _dist_to_flat_out consistency # --------------------------------------------------------------------------- -@_needs_pure_python -def test_dist_to_flat_fast_and_out_agree(): - """fast and _out variants give same result.""" - args = (2.5, -1.0, 0.1, -0.2, 0.001, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 1e-8) - x1, y1 = dist_to_flat_fast(*args) - out = np.zeros(2, dtype=np.float64) - _dist_to_flat_out(*args, out) - assert math.isclose(x1, out[0], rel_tol=1e-12) - assert math.isclose(y1, out[1], rel_tol=1e-12) - - -# --------------------------------------------------------------------------- -# 20. _pixel_to_metric_out / pixel_to_metric_fast consistency -# --------------------------------------------------------------------------- - - -def test_pixel_to_metric_out_and_fast_agree(): - """_out and fast variants give same result for all chfields.""" - for chfield in [0, 1, 2]: - x_f, y_f = pixel_to_metric_fast(400.0, 300.0, 1024, 768, 0.01, 0.01, chfield) - out = np.zeros(2, dtype=np.float64) - _pixel_to_metric_out(400.0, 300.0, 1024, 768, 0.01, 0.01, chfield, out) - assert math.isclose(x_f, out[0], rel_tol=1e-12) - assert math.isclose(y_f, out[1], rel_tol=1e-12) - - -# --------------------------------------------------------------------------- -# 21. _metric_to_pixel_out / metric_to_pixel_fast consistency -# --------------------------------------------------------------------------- - - -def test_metric_to_pixel_out_and_fast_agree(): - """_out and fast variants give same result for all chfields.""" - for chfield in [0, 1, 2]: - xp_f, yp_f = metric_to_pixel_fast(0.5, -0.3, 1024, 768, 0.01, 0.01, chfield) - out = np.zeros(2, dtype=np.float64) - _metric_to_pixel_out(0.5, -0.3, 1024, 768, 0.01, 0.01, chfield, out) - assert math.isclose(xp_f, out[0], rel_tol=1e-12) - assert math.isclose(yp_f, out[1], rel_tol=1e-12) - - -# --------------------------------------------------------------------------- -# 22. img_coord_batch_fast vs _img_coord_fast element-wise -# --------------------------------------------------------------------------- - - -def test_img_coord_batch_matches_elementwise(): - """batch result == repeated scalar calls.""" - cal = _make_cal_arr(gz=50.0, k1=0.001) - mmlut_data = np.array([], dtype=np.float64) - mmlut_origin = np.zeros(3, dtype=np.float64) - positions = np.array( - [ - [1.0, 0.0, 0.0], - [0.0, 2.0, -5.0], - ], - dtype=np.float64, - order="C", - ) - result = img_coord_batch_fast(positions, cal, mmlut_data, mmlut_origin, 0, 0, 1.0) - for i in range(len(positions)): - xi, yi = _img_coord_fast(positions[i], cal, mmlut_data, mmlut_origin, 0, 0, 1.0) - assert math.isclose(result[i, 0], xi, rel_tol=1e-12) - assert math.isclose(result[i, 1], yi, rel_tol=1e-12) - - -# --------------------------------------------------------------------------- -# 23. flat_image_coord_batch_fast vs _flat_image_coord_fast element-wise -# --------------------------------------------------------------------------- - - -def test_flat_image_coord_batch_matches_elementwise(): - """batch result == repeated scalar calls.""" - cal = _make_cal_arr(gz=50.0) - mmlut_data = np.array([], dtype=np.float64) - mmlut_origin = np.zeros(3, dtype=np.float64) - positions = np.array( - [ - [0.5, -0.5, 0.0], - [2.0, 1.0, -3.0], - ], - dtype=np.float64, - order="C", - ) - result = flat_image_coord_batch_fast( - positions, cal, mmlut_data, mmlut_origin, 0, 0, 1.0 - ) - for i in range(len(positions)): - xi, yi = _flat_image_coord_fast( - positions[i], cal, mmlut_data, mmlut_origin, 0, 0, 1.0 - ) - assert math.isclose(result[i, 0], xi, rel_tol=1e-12) - assert math.isclose(result[i, 1], yi, rel_tol=1e-12) - - -# --------------------------------------------------------------------------- -# 24. dist_to_flat_fast / _dist_to_flat_out loop-exhaustion branches (596->618, 665->680) -# --------------------------------------------------------------------------- - - -def test_dist_to_flat_fast_loop_exhaustion(): - """tol=0.0 prevents the break from firing → all 50 iterations run (596->618 branch).""" - x, y = dist_to_flat_fast( - 1.0, 1.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0 - ) - # Result exists even without convergence - assert isinstance(x, float) - assert isinstance(y, float) - @_needs_pure_python def test_dist_to_flat_out_loop_exhaustion(): @@ -1733,30 +1228,6 @@ def test_candsearch_xmax_ymax_clamp(): assert result == 0 -# --------------------------------------------------------------------------- -# 26. _flat_image_coord_fast: branch 1262->1271 (v3 > mmlut_nr*mmlut_nz) -# --------------------------------------------------------------------------- - - -def test_flat_image_coord_fast_lut_boundary_branch(): - """ir == mmlut_nr AND iz in [0,nz] → v3 > mmlut_nr*mmlut_nz → inner if False → 1262->1271. - - Geometry (x0=0, y0=0, z0=100, gz=50): - With pos=[2.5, 0, 51] and rw=1.0, nr=2, nz=2: - pos_t_0 = 2.5 → R=2.5 → ir=2 = nr=2 (outer condition True: 2<=2) - dist_point_glas = 51-50 = 1 → iz=1 (in [0,2]) - v3 = 2*2+1+2+1 = 8 > 4 = nr*nz → inner condition False → 1262->1271 - """ - cal = _make_cal_arr(x0=0.0, y0=0.0, z0=100.0, gz=50.0, d0=0.0) - pos = np.array([2.5, 0.0, 51.0], dtype=np.float64) - nr, nz = 2, 2 - mmlut_data = np.ones(nr * nz, dtype=np.float64) # non-empty LUT - mmlut_origin = np.zeros(3, dtype=np.float64) - rw = 1.0 - x, y = _flat_image_coord_fast(pos, cal, mmlut_data, mmlut_origin, nr, nz, rw) - assert isinstance(x, float) - - # --------------------------------------------------------------------------- # 27. assess_new_position_fast: use_proj=False path (lines 765-783) # --------------------------------------------------------------------------- diff --git a/tests/unit/test_two_phase_tracking.py b/tests/unit/test_two_phase_tracking.py new file mode 100644 index 00000000..a0feaee6 --- /dev/null +++ b/tests/unit/test_two_phase_tracking.py @@ -0,0 +1,141 @@ +"""Unit tests for the stateful two-phase tracker (velocity + gaps). + +Pure array-level tests: no work dir, no calibration (identity project_fn). +""" + +import numpy as np + +from openptv2.plugins.two_phase_tracking import ( + TwoPhaseTracker, + TwoPhaseTrackerConfig, +) + + +def ident(P): + """Fake projection: leaves == 3D coords (first 2 dims used as pixels).""" + P = np.asarray(P, dtype=np.float64) + return P[:, :2] + + +def cfg(**kw): + base = {"v_max": 5.0, "leaf_weight": 1.0, "use_velocity": True, + "cost_mode": "projected", "max_gap": 2} + base.update(kw) + return TwoPhaseTrackerConfig(**base) + + +def leaves_of(frames): + return [ident(P) for P in frames] + + +def test_crossing_needs_velocity(): + """Head-on X-cross: raw bounces (swap), velocity crosses (truth).""" + frames = [ + np.array([[0.0, 0, 0], [2.0, 0, 0]]), + np.array([[0.9, 0, 0], [1.1, 0, 0]]), # near-coincident mid-frame + np.array([[2.0, 0, 0], [0.0, 0, 0]]), + ] + raw = TwoPhaseTracker(cfg(use_velocity=False)) + got = raw.track_frames(frames, leaves_of(frames), project_fn=ident) + # bounce: row0 -> row1 and row1 -> row0 at the crossing step + assert (1, 0, 2, 1) in got and (1, 1, 2, 0) in got + + vel = TwoPhaseTracker(cfg()) + got = vel.track_frames(frames, leaves_of(frames), project_fn=ident) + assert (1, 0, 2, 0) in got and (1, 1, 2, 1) in got + + +def test_gap_bridging(): + """A particle missing one frame is re-caught (max_gap=2).""" + frames = [ + np.array([[0.0, 0, 0], [9.0, 9, 9]]), + np.array([[1.0, 0, 0], [9.0, 9, 9]]), + np.array([[9.0, 9, 9]]), # pid0 occluded + np.array([[3.0, 0, 0], [9.0, 9, 9]]), + ] + tr = TwoPhaseTracker(cfg()) + got = tr.track_frames(frames, leaves_of(frames), project_fn=ident) + # gap-spanning link (1,0) -> (3,0): last seen row 0 to row 0 + assert (1, 0, 3, 0) in got + + +def test_gap_retires(): + """A particle missing longer than max_gap is not re-caught.""" + frames = [ + np.array([[0.0, 0, 0]]), + np.array([[9.0, 9, 9]]), + np.array([[9.0, 9, 9]]), + np.array([[9.0, 9, 9]]), + np.array([[4.0, 0, 0]]), + ] + tr = TwoPhaseTracker(cfg(max_gap=2)) + got = tr.track_frames(frames, leaves_of(frames), project_fn=ident) + assert all(t1 != 4 for _, _, t1, _ in got) # never re-caught + + +def test_cold_start_zero_velocity(): + """First link from standstill links nearest neighbour.""" + frames = [np.array([[0.0, 0, 0]]), np.array([[0.4, 0, 0]])] + tr = TwoPhaseTracker(cfg()) + assert tr.track_frames(frames, leaves_of(frames), + project_fn=ident) == [(0, 0, 1, 0)] + + +def test_cost_3d_without_project_fn(): + """No project_fn -> 3D costs, velocity still crosses.""" + frames = [ + np.array([[0.0, 0, 0], [2.0, 0, 0]]), + np.array([[0.9, 0, 0], [1.1, 0, 0]]), + np.array([[2.0, 0, 0], [0.0, 0, 0]]), + ] + tr = TwoPhaseTracker(cfg(cost_mode="3d")) + got = tr.track_frames(frames, leaves_of(frames), project_fn=None) + assert (1, 0, 2, 0) in got and (1, 1, 2, 1) in got + + +def test_cascade_merge_additive(): + """Merge extends free ends, never steals, accepts gap links.""" + import importlib.util + from pathlib import Path as _Path + + spec = importlib.util.spec_from_file_location( + "cascade_track", + _Path(__file__).resolve().parent.parent.parent + / "scripts" / "cascade_track.py", + ) + mod = importlib.util.module_from_spec(spec) + spec.loader.exec_module(mod) + merge_links = mod.merge_links + corr_prev = [[-1, -1], [0, -1]] # frame1: row0<-row0, row1 head + corr_next = [[0, -2], [-1, -1]] # row0->row0; row1 tail (-2 = dropped) + nrows = [2, 2] + # (0,1)->(1,1): both ends free -> accepted (extends tail+head) + # (0,0)->(1,1): target taken after first -> rejected (no steal) + # (0,1)->(1,0): source free but target taken by base -> rejected + # gap link (0,1)->(1,1) counted once + tp_links = [(0, 1, 1, 1), (0, 0, 1, 1), (0, 1, 1, 0)] + mp, mn, n, added = merge_links(corr_prev, corr_next, tp_links, nrows) + assert n == 1 and added == [(0, 1, 1, 1)] + assert list(mn[0]) == [0, 1] and list(mp[1]) == [0, 1] + # base links untouched + assert mn[0][0] == 0 and mp[1][0] == 0 + + +def test_bidirectional_tracking(): + """Bidirectional tracking resolves crossings and returns clean 1-to-1 chains.""" + frames = [ + np.array([[0.0, 0, 0], [2.0, 0, 0]]), + np.array([[0.9, 0, 0], [1.1, 0, 0]]), + np.array([[2.0, 0, 0], [0.0, 0, 0]]), + ] + tr = TwoPhaseTracker(cfg(bidirectional=True)) + got = tr.track_frames(frames, leaves_of(frames), project_fn=ident) + assert (1, 0, 2, 0) in got and (1, 1, 2, 1) in got + + # Test return_chains=True with bidirectional=True + links, chains = tr.track_frames( + frames, leaves_of(frames), project_fn=ident, return_chains=True + ) + assert len(links) == len(got) + assert len(chains) == 2 + assert all(len(c["frames"]) == 3 for c in chains)