From 04028acd5493d038836ed5a09dcb7e84eadded9b Mon Sep 17 00:00:00 2001 From: rUv Date: Wed, 15 Jul 2026 09:53:38 -0400 Subject: [PATCH] feat: add multi-actor PoseCode scene crate --- CHANGELOG.md | 3 + CLAUDE.md | 2 + README.md | 3 +- .../ADR-266-multi-actor-posecode-scenes.md | 141 ++++++ docs/adr/README.md | 3 +- v2/Cargo.lock | 11 + v2/Cargo.toml | 4 + v2/crates/wifi-densepose-posecode/Cargo.toml | 28 ++ .../benches/posecode_bench.rs | 37 ++ .../wifi-densepose-posecode/src/adapter.rs | 443 ++++++++++++++++++ .../wifi-densepose-posecode/src/error.rs | 24 + v2/crates/wifi-densepose-posecode/src/lib.rs | 29 ++ .../wifi-densepose-posecode/src/model.rs | 203 ++++++++ .../wifi-densepose-posecode/src/parser.rs | 375 +++++++++++++++ .../wifi-densepose-posecode/src/segmenter.rs | 212 +++++++++ .../wifi-densepose-posecode/src/serialize.rs | 131 ++++++ .../wifi-densepose-posecode/src/validate.rs | 202 ++++++++ 17 files changed, 1849 insertions(+), 2 deletions(-) create mode 100644 docs/adr/ADR-266-multi-actor-posecode-scenes.md create mode 100644 v2/crates/wifi-densepose-posecode/Cargo.toml create mode 100644 v2/crates/wifi-densepose-posecode/benches/posecode_bench.rs create mode 100644 v2/crates/wifi-densepose-posecode/src/adapter.rs create mode 100644 v2/crates/wifi-densepose-posecode/src/error.rs create mode 100644 v2/crates/wifi-densepose-posecode/src/lib.rs create mode 100644 v2/crates/wifi-densepose-posecode/src/model.rs create mode 100644 v2/crates/wifi-densepose-posecode/src/parser.rs create mode 100644 v2/crates/wifi-densepose-posecode/src/segmenter.rs create mode 100644 v2/crates/wifi-densepose-posecode/src/serialize.rs create mode 100644 v2/crates/wifi-densepose-posecode/src/validate.rs diff --git a/CHANGELOG.md b/CHANGELOG.md index 9fd3bffa..6083056b 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -7,6 +7,9 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ## [Unreleased] +### Added +- **Multi-actor PoseCode scene crate (`wifi-densepose-posecode`, ADR-266).** Adds a deterministic Rust scene model for multiple persistent RuView actors, synchronized poses, inter-person contacts, confidence and observation provenance. Includes a bounded line parser, canonical serializer, direct `PoseTrack` adapter, confidence-aware phase segmentation, provenance-sensitive range validation, 14 feature-enabled unit tests and Criterion parser/serializer benchmarks. Raw 20 Hz observations remain separate from semantic phases; observed range anomalies are warnings rather than medical safety claims. MEASURED on the implementation container: two-actor parse 3.40 µs, serialization 3.29 µs. + ### Changed - **`@ruvnet/rvagent` startup optimization — stdio time-to-first-response ~242 ms → ~189 ms (−22%; MEASURED, median of repeated `initialize` round-trips against `dist/index.js`, this container, reproduce with a piped-stdin timer).** Two changes: (1) `./http-transport.js` is now imported **lazily** inside the `RVAGENT_HTTP_PORT` branch — it chain-loads the MCP SDK's `streamableHttp` module (~48 ms MEASURED via per-module `import()` timing), which the default stdio path never uses; (2) the advertised JSON Schemas generated from the Zod sources are memoized per tool instead of re-walking the Zod tree on every `tools/list` (matters under the session-per-server HTTP model where each session lists tools). No behavior change: 99/99 jest tests, HTTP session flow re-smoke-tested through the lazy path. The `@ruvnet/ruview` harness CLI was profiled too and left alone — 50 ms vs the ~29 ms bare `node -e ''` floor on the same box (MEASURED), i.e. already near the interpreter floor with zero dependencies. diff --git a/CLAUDE.md b/CLAUDE.md index 262884dc..bb52e820 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -14,6 +14,7 @@ Dual codebase: Python v1 (`v1/`) and Rust port (`v2/`). | `wifi-densepose-mat` | Mass Casualty Assessment Tool — disaster survivor detection | | `wifi-densepose-hardware` | ESP32 aggregator, TDM protocol, channel hopping firmware; `ieee80211bf/` 802.11bf forward-compat protocol model (ADR-153) | | `wifi-densepose-ruvector` | RuVector v2.0.4 integration + cross-viewpoint fusion (5 modules) | +| `wifi-densepose-posecode` | ADR-266 multi-actor semantic motion scenes, PoseTrack adapter, bounded parser, confidence-aware phase segmentation | | `wifi-densepose-wasm` | WebAssembly bindings for browser deployment | | `wifi-densepose-cli` | CLI tool (`wifi-densepose` binary) — `calibrate`/`calibrate-serve`/`enroll`/`train-room`/`room-watch` + MAT (MAT gated behind the `mat` feature; build `--no-default-features` for the aarch64/appliance calibration binary) | | `wifi-densepose-calibration` | ADR-151 per-room calibration & specialist training — `baseline → enroll → extract → train` → bank of small specialists (presence/posture/breathing/heartbeat/restlessness/anomaly) + multistatic fusion; pure Rust, edge-deployable | @@ -81,6 +82,7 @@ All 5 ruvector crates integrated in workspace: - ADR-263: `@ruvnet/ruview` npm harness deep review + optimization strategy (Proposed) - ADR-264: `@ruvnet/rvagent` MCP server + `@ruv/ruview-cli` deep review + optimization strategy (Proposed) - ADR-265: RuView npm distribution strategy — CI gate, provenance, version single-sourcing (Proposed) +- ADR-266: Multi-actor PoseCode scenes from persistent RuView tracks (Accepted) ### Supported Hardware diff --git a/README.md b/README.md index 7b8c77e4..f4a05033 100644 --- a/README.md +++ b/README.md @@ -617,7 +617,8 @@ Verify the plugin structure: `bash plugins/ruview/scripts/smoke.sh`. Full detail | [Semantic Primitives — Precision/Recall](docs/integrations/semantic-primitives-metrics.md) | Per-primitive F1 on the held-out paired-capture set: someone-sleeping, possible-distress, room-active, elderly-inactivity-anomaly, meeting, bathroom, fall-risk, bed-exit, no-movement, multi-room. | | [Claude Code / Codex Plugin](plugins/ruview/README.md) | The `ruview` plugin + marketplace — skills, `/ruview-*` commands, agents, and the Codex prompt mirror | | [Portable harness — `npx @ruvnet/ruview`](harness/ruview/README.md) | MetaHarness-minted, host-portable RuView operator harness — `ruview.*` MCP tools + the MEASURED-vs-CLAIMED honesty guardrail enforced in code ([ADR-182](docs/adr/ADR-182-npx-ruview-harness-via-metaharness.md)). A lighter, multi-host companion to the in-repo plugin. | -| [Architecture Decisions](docs/adr/README.md) | 182 ADRs — why each technical choice was made, organized by domain (hardware, signal processing, ML, platform, infrastructure) | +| [Architecture Decisions](docs/adr/README.md) | 183 ADRs — why each technical choice was made, organized by domain (hardware, signal processing, ML, platform, infrastructure) | +| [Multi-actor PoseCode](v2/crates/wifi-densepose-posecode/) | Rust scene protocol converting persistent RuView pose tracks into confidence-scored actors, synchronized motion phases, contacts and deterministic text. [ADR-266](docs/adr/ADR-266-multi-actor-posecode-scenes.md). | | [Domain Models](docs/ddd/README.md) | 8 DDD models (RuvSense, Signal Processing, Training Pipeline, Hardware Platform, Sensing Server, WiFi-Mat, CHCI, rvCSI) — bounded contexts, aggregates, domain events, and ubiquitous language | | [rvCSI — edge RF sensing runtime](https://github.com/ruvnet/rvcsi) | Rust-first / TypeScript-accessible / hardware-abstracted CSI runtime: multi-source ingestion (incl. real nexmon_csi `.pcap` from a **Raspberry Pi 5** / Pi 4 / Pi 3B+ — CYW43455 / BCM43455c0) → validation → DSP → typed events → RuVector RF memory ([ADR-095](docs/adr/ADR-095-rvcsi-edge-rf-sensing-platform.md), [ADR-096](docs/adr/ADR-096-rvcsi-ffi-crate-layout.md), [domain model](docs/ddd/rvcsi-domain-model.md)). Now its own repo — [`ruvnet/rvcsi`](https://github.com/ruvnet/rvcsi) — vendored here under `vendor/rvcsi`; 9 `rvcsi-*` crates on crates.io, `@ruv/rvcsi` on npm, plus a Claude Code plugin. | | [Desktop App](v2/crates/wifi-densepose-desktop/README.md) | **WIP** — Tauri v2 desktop app for node management, OTA updates, WASM deployment, and mesh visualization | diff --git a/docs/adr/ADR-266-multi-actor-posecode-scenes.md b/docs/adr/ADR-266-multi-actor-posecode-scenes.md new file mode 100644 index 00000000..538a1632 --- /dev/null +++ b/docs/adr/ADR-266-multi-actor-posecode-scenes.md @@ -0,0 +1,141 @@ +# ADR-266: Multi-Actor PoseCode Scenes + +| Field | Value | +|-------|-------| +| Status | Accepted, crate implementation complete | +| Date | 2026-07-15 | +| Deciders | ruv | +| Crate | `wifi-densepose-posecode` | +| Related | ADR-029, ADR-037, ADR-082, ADR-101, ADR-170 | + +## Context + +RuView already maintains multiple persistent `PoseTrack` aggregates. Each track +contains 17 COCO keypoints, a lifecycle, an AETHER identity embedding and a +stable `TrackId`. The missing layer is a compact, agent-readable representation +of what those tracked people do together over time. + +PoseCode 0.1 provides a useful vocabulary for one actor, sequential movement +phases, semantic joint actions, contacts and browser rendering. Its current +protocol does not represent multiple actors, shared world coordinates, +inter-person contact, observation confidence or track provenance. + +Encoding every 20 Hz RuView frame as text would also be incorrect. It would +produce 1,200 steps per person per minute, discard observation uncertainty and +couple the sensing hot path to a presentation language. + +## Decision + +Create the leaf crate `wifi-densepose-posecode` and define a backwards-minded +0.2 scene extension with these invariants: + +1. A `Scene` owns named actors and synchronized phases. +2. Each actor can carry the originating RuView `TrackId` and confidence. +3. Every joint target and inter-person contact carries confidence. +4. Actor positions and travel use one room-calibrated three-dimensional frame. +5. Raw `SceneFrame` observations remain separate from compact semantic phases. +6. `PhaseSegmenter` emits a phase on actor membership change, motion settling + or a bounded maximum duration. +7. Serialization is canonical and deterministic. +8. The parser has hard document, line, actor, phase and target limits. +9. General range-of-motion violations are errors for authored scenes but only + warnings for observed WiFi scenes. RF pose estimates are not medical joint + measurements and must not create false safety claims. + +The text form is: + +```posecode +posecode scene "Assisted squat" +source observed_wifi_csi +actor patient: + rig humanoid + pose start = standing + track 7 + confidence 0.82 + position 0 0 0 +actor therapist: + rig humanoid + pose start = standing + track 12 + confidence 0.76 + position 1.2 0 0 +step "Lower" 1.5s flow: + patient.knee_left: flex 95 0.8 + therapist.shoulder_left: flex 30 0.7 + contact therapist.hand_left patient.shoulder_right 0.7 +repeat 1 +``` + +## Architecture + +`RuViewAdapter` consumes references to existing `PoseTrack` values. It does not +duplicate assignment or identity tracking. It calculates elbows and knees from +three-point interior angles, estimates hip and shoulder sagittal movement in a +calibrated coordinate frame and maps the hip midpoint to actor position. +The adapter is exposed by the crate's `ruview` feature so the protocol, parser +and segmenter remain usable without pulling the full signal and RuVector graph. + +The adapter uses keypoint confidence when available. Current trackers that have +not populated that field receive an explicit configurable fallback confidence, +degraded by track staleness. Non-finite coordinates and terminated tracks fail +closed. + +`PhaseSegmenter` consumes ordered frames. High-rate frames remain available for +live rendering and evidence storage while the resulting scene records only +meaningful synchronized targets. This preserves both fidelity and readability. + +## Security and Resource Bounds + +The public parser accepts at most 1 MiB per document and 4,096 bytes per line. +The scene validator defaults to 32 actors, 10,000 phases and 64 joint targets +per actor per phase. All coordinates, angles and confidence values must be +finite. References to undeclared actors are rejected. Durations, repeats and +timeline addition are bounded. + +No network, filesystem, model or renderer capability is present in the crate. +It is a deterministic transformation leaf over owned data. + +## Consequences + +RuView can now produce privacy-preserving multi-person replays, structured fall +and interaction evidence, exercise comparison inputs and agent-readable motion +records without video. + +The crate does not solve RF source separation. Its multi-person accuracy is +bounded by the upstream pose model and `PoseTracker`. Contact inference is also +not fabricated; contacts must be observed by a future calibrated classifier or +authored explicitly. + +The current COCO skeleton has no toe keypoints, so ankle rotation and precise +foot contact cannot be inferred honestly. Those targets are omitted rather +than guessed. + +## Acceptance Criteria + +1. A scene containing two actors, simultaneous targets and a cross-actor + contact parses and serializes deterministically. +2. Unknown actors, non-finite values, reversed timestamps and unbounded inputs + fail closed. +3. One RuView track converts to finite semantic targets with the same TrackId. +4. Observed out-of-range angles produce warnings, not medical errors. +5. Phase count is smaller than input frame count for a normal motion sequence. +6. Focused crate tests and the complete workspace test suite pass before merge. + +## Measured Results + +On the implementation container with Rust 1.88, Criterion measured the canonical +two-actor scene parser at 3.40 microseconds and serializer at 3.29 microseconds +per operation. The protocol-only dependency surface is three runtime crates: +`serde`, `serde_json` and `thiserror`. + +The protocol-only configuration passes 13 unit tests, documentation tests, +Rustfmt and Clippy with warnings denied. The `ruview` feature passes 14 unit +tests, including native `PoseTrack` conversion, under the repository's RuVector +AVX512-capable build configuration. + +## References + +1. PoseCode repository and protocol, MIT licensed: + +2. RuView multi-person pose decision: ADR-037. +3. RuView persistent pose tracking: ADR-029 and `pose_tracker.rs`. diff --git a/docs/adr/README.md b/docs/adr/README.md index 94c0f218..4520be44 100644 --- a/docs/adr/README.md +++ b/docs/adr/README.md @@ -1,6 +1,6 @@ # Architecture Decision Records -This folder contains 182 Architecture Decision Records (ADRs) that document every significant technical choice in the RuView / WiFi-DensePose project. (The index tables below list a curated subset per domain; see the directory listing for the full set.) +This folder contains 183 Architecture Decision Records (ADRs) that document every significant technical choice in the RuView / WiFi-DensePose project. (The index tables below list a curated subset per domain; see the directory listing for the full set.) ## Why ADRs? @@ -123,6 +123,7 @@ Statuses: **Proposed** (under discussion), **Accepted** (approved and/or impleme | [ADR-263](ADR-263-ruview-npm-harness-deep-review.md) | `@ruvnet/ruview` npm harness — deep review + optimization strategy | Proposed | | [ADR-264](ADR-264-rvagent-mcp-and-cli-npm-deep-review.md) | `@ruvnet/rvagent` MCP server + `@ruv/ruview-cli` — deep review + optimization strategy | Proposed | | [ADR-265](ADR-265-ruview-npm-distribution-strategy.md) | RuView npm distribution strategy — CI gate, provenance, version single-sourcing, namespace | Proposed | +| [ADR-266](ADR-266-multi-actor-posecode-scenes.md) | Multi-actor PoseCode scenes from persistent RuView tracks | Accepted | --- diff --git a/v2/Cargo.lock b/v2/Cargo.lock index fd73e14a..ee2b37cd 100644 --- a/v2/Cargo.lock +++ b/v2/Cargo.lock @@ -11091,6 +11091,17 @@ dependencies = [ "tower-http", ] +[[package]] +name = "wifi-densepose-posecode" +version = "0.3.0" +dependencies = [ + "criterion", + "serde", + "serde_json", + "thiserror 2.0.18", + "wifi-densepose-signal", +] + [[package]] name = "wifi-densepose-rufield" version = "0.3.0" diff --git a/v2/Cargo.toml b/v2/Cargo.toml index a85f5de8..b3497d25 100644 --- a/v2/Cargo.toml +++ b/v2/Cargo.toml @@ -46,6 +46,9 @@ members = [ # PR #491 slot heuristic with a Candle network + Stoer-Wagner fusion. # Motivated by #499 ghost-skeleton reports. "crates/cog-person-count", + # ADR-266: Multi-actor PoseCode scene protocol. Converts persistent + # RuvSense PoseTracks into confidence-scored, deterministic motion scenes. + "crates/wifi-densepose-posecode", # ADR-116: Home Assistant + Matter Cognitum Seed cog. Wraps the # ADR-115 MQTT publisher as a Seed-installable artifact with # mDNS, embedded broker, RuVector thresholds, Ed25519 witness. @@ -215,6 +218,7 @@ wifi-densepose-hardware = { version = "0.3.0", path = "crates/wifi-densepose-har wifi-densepose-wasm = { version = "0.3.0", path = "crates/wifi-densepose-wasm" } wifi-densepose-mat = { version = "0.3.0", path = "crates/wifi-densepose-mat" } wifi-densepose-ruvector = { version = "0.3.0", path = "crates/wifi-densepose-ruvector" } +wifi-densepose-posecode = { version = "0.3.0", path = "crates/wifi-densepose-posecode" } wifi-densepose-worldmodel = { version = "0.3.0", path = "crates/worldgraph/wifi-densepose-worldmodel" } [profile.release] diff --git a/v2/crates/wifi-densepose-posecode/Cargo.toml b/v2/crates/wifi-densepose-posecode/Cargo.toml new file mode 100644 index 00000000..3c28774d --- /dev/null +++ b/v2/crates/wifi-densepose-posecode/Cargo.toml @@ -0,0 +1,28 @@ +[package] +name = "wifi-densepose-posecode" +version.workspace = true +edition.workspace = true +authors.workspace = true +license.workspace = true +repository.workspace = true +description = "Multi-actor semantic motion scenes for RuView PoseTrack streams" +documentation = "https://docs.rs/wifi-densepose-posecode" +keywords = ["wifi", "pose", "motion", "posecode", "multi-person"] +categories = ["science", "computer-vision", "parser-implementations"] + +[dependencies] +serde.workspace = true +serde_json.workspace = true +thiserror.workspace = true +wifi-densepose-signal = { version = "0.3.5", path = "../wifi-densepose-signal", default-features = false, optional = true } + +[dev-dependencies] +criterion.workspace = true + +[features] +default = [] +ruview = ["dep:wifi-densepose-signal"] + +[[bench]] +name = "posecode_bench" +harness = false diff --git a/v2/crates/wifi-densepose-posecode/benches/posecode_bench.rs b/v2/crates/wifi-densepose-posecode/benches/posecode_bench.rs new file mode 100644 index 00000000..6c59582d --- /dev/null +++ b/v2/crates/wifi-densepose-posecode/benches/posecode_bench.rs @@ -0,0 +1,37 @@ +use criterion::{black_box, criterion_group, criterion_main, Criterion}; +use wifi_densepose_posecode::{parse_posecode, to_posecode}; + +const TWO_ACTORS: &str = r#"posecode scene "crossing" +source observed_wifi_csi +actor person_1: + rig humanoid + pose start = standing + track 1 + confidence 0.8 + position 0 0 0 +actor person_2: + rig humanoid + pose start = standing + track 2 + confidence 0.8 + position 1 0 0 +step "Move" 0.05s linear: + person_1.knee_left: flex 30 0.8 + person_1.hip_left: flex 20 0.8 + person_2.knee_right: flex 35 0.8 + person_2.hip_right: flex 25 0.8 +repeat 1 +"#; + +fn benchmarks(c: &mut Criterion) { + c.bench_function("parse_two_actor_scene", |b| { + b.iter(|| parse_posecode(black_box(TWO_ACTORS)).unwrap()) + }); + let scene = parse_posecode(TWO_ACTORS).unwrap(); + c.bench_function("serialize_two_actor_scene", |b| { + b.iter(|| to_posecode(black_box(&scene))) + }); +} + +criterion_group!(benches, benchmarks); +criterion_main!(benches); diff --git a/v2/crates/wifi-densepose-posecode/src/adapter.rs b/v2/crates/wifi-densepose-posecode/src/adapter.rs new file mode 100644 index 00000000..ffeac957 --- /dev/null +++ b/v2/crates/wifi-densepose-posecode/src/adapter.rs @@ -0,0 +1,443 @@ +//! Conversion from tracked COCO-17 skeletons to semantic joint observations. + +use crate::error::{Error, Result}; +use crate::model::{ActorObservation, JointTarget, SceneFrame, Vec3}; + +const NUM_KEYPOINTS: usize = 17; +const LEFT_SHOULDER: usize = 5; +const RIGHT_SHOULDER: usize = 6; +const LEFT_ELBOW: usize = 7; +const RIGHT_ELBOW: usize = 8; +const LEFT_WRIST: usize = 9; +const RIGHT_WRIST: usize = 10; +const LEFT_HIP: usize = 11; +const RIGHT_HIP: usize = 12; +const LEFT_KNEE: usize = 13; +const RIGHT_KNEE: usize = 14; +const LEFT_ANKLE: usize = 15; +const RIGHT_ANKLE: usize = 16; + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum TrackState { + Tentative, + Active, + Lost, + Terminated, +} + +#[derive(Debug, Clone, Copy, Default, PartialEq)] +pub struct KeypointObservation { + pub position: Vec3, + pub confidence: f32, +} + +#[derive(Debug, Clone, PartialEq)] +pub struct TrackObservation { + pub id: u64, + pub state: TrackState, + pub keypoints: [KeypointObservation; NUM_KEYPOINTS], + pub time_since_update: u64, +} + +#[cfg(feature = "ruview")] +impl From<&wifi_densepose_signal::ruvsense::PoseTrack> for TrackObservation { + fn from(track: &wifi_densepose_signal::ruvsense::PoseTrack) -> Self { + use wifi_densepose_signal::ruvsense::TrackLifecycleState; + Self { + id: track.id.0, + state: match track.lifecycle { + TrackLifecycleState::Tentative => TrackState::Tentative, + TrackLifecycleState::Active => TrackState::Active, + TrackLifecycleState::Lost => TrackState::Lost, + TrackLifecycleState::Terminated => TrackState::Terminated, + }, + keypoints: std::array::from_fn(|i| { + let point = track.keypoints[i].position(); + KeypointObservation { + position: Vec3::new(point[0], point[1], point[2]), + confidence: track.keypoints[i].confidence, + } + }), + time_since_update: track.time_since_update, + } + } +} + +#[derive(Debug, Clone)] +pub struct AdapterConfig { + /// Coordinate system up vector after room calibration. + pub world_up: Vec3, + /// Coordinate system forward vector after room calibration. + pub world_forward: Vec3, + /// Ignore tracks below this aggregate confidence. + pub min_confidence: f32, + /// Confidence used until an inference backend populates keypoint confidence. + pub unscored_confidence: f32, + /// Emit lost tracks as predicted observations. + pub include_lost: bool, +} + +impl Default for AdapterConfig { + fn default() -> Self { + Self { + world_up: Vec3::new(0.0, 1.0, 0.0), + world_forward: Vec3::new(0.0, 0.0, 1.0), + min_confidence: 0.2, + unscored_confidence: 0.5, + include_lost: false, + } + } +} + +#[derive(Debug, Clone)] +pub struct RuViewAdapter { + config: AdapterConfig, +} + +impl RuViewAdapter { + pub fn new(config: AdapterConfig) -> Result { + if !config.world_up.is_finite() || !config.world_forward.is_finite() { + return Err(Error::Observation("coordinate axes must be finite".into())); + } + if !(0.0..=1.0).contains(&config.min_confidence) + || !(0.0..=1.0).contains(&config.unscored_confidence) + { + return Err(Error::Observation("confidence must be in [0, 1]".into())); + } + if norm(config.world_up) < 1e-6 || norm(config.world_forward) < 1e-6 { + return Err(Error::Observation( + "coordinate axes must be non-zero".into(), + )); + } + let alignment = dot(normalize(config.world_up), normalize(config.world_forward)).abs(); + if alignment > 0.98 { + return Err(Error::Observation( + "world up and forward axes must not be parallel".into(), + )); + } + Ok(Self { config }) + } + + /// Convert all visible tracks into one synchronized observation frame. + pub fn frame(&self, timestamp_ms: u64, tracks: &[TrackObservation]) -> SceneFrame { + let actors = tracks + .iter() + .filter_map(|track| self.actor(track)) + .collect(); + SceneFrame { + timestamp_ms, + actors, + } + } + + /// Direct bridge for the RuView tracker aggregate. The feature is optional + /// so protocol-only consumers do not pull the signal and RuVector graph. + #[cfg(feature = "ruview")] + pub fn frame_from_pose_tracks( + &self, + timestamp_ms: u64, + tracks: &[&wifi_densepose_signal::ruvsense::PoseTrack], + ) -> SceneFrame { + let observations: Vec = + tracks.iter().map(|track| (*track).into()).collect(); + self.frame(timestamp_ms, &observations) + } + + pub fn actor(&self, track: &TrackObservation) -> Option { + if track.state == TrackState::Terminated + || (!self.config.include_lost && track.state == TrackState::Lost) + { + return None; + } + + let confidence = track_confidence(track, self.config.unscored_confidence); + if confidence < self.config.min_confidence { + return None; + } + + let p = |index: usize| track.keypoints[index].position; + if track.keypoints.iter().any(|kp| !kp.position.is_finite()) { + return None; + } + + let left_hip = p(LEFT_HIP); + let right_hip = p(RIGHT_HIP); + let position = midpoint(left_hip, right_hip); + let up = normalize(self.config.world_up); + let forward = normalize(sub( + self.config.world_forward, + scale(up, dot(self.config.world_forward, up)), + )); + + let mut joints = Vec::with_capacity(10); + add_flexion( + &mut joints, + "elbow_left", + p(LEFT_SHOULDER), + p(LEFT_ELBOW), + p(LEFT_WRIST), + joint_confidence(track, &[LEFT_SHOULDER, LEFT_ELBOW, LEFT_WRIST], confidence), + ); + add_flexion( + &mut joints, + "elbow_right", + p(RIGHT_SHOULDER), + p(RIGHT_ELBOW), + p(RIGHT_WRIST), + joint_confidence( + track, + &[RIGHT_SHOULDER, RIGHT_ELBOW, RIGHT_WRIST], + confidence, + ), + ); + add_flexion( + &mut joints, + "knee_left", + left_hip, + p(LEFT_KNEE), + p(LEFT_ANKLE), + joint_confidence(track, &[LEFT_HIP, LEFT_KNEE, LEFT_ANKLE], confidence), + ); + add_flexion( + &mut joints, + "knee_right", + right_hip, + p(RIGHT_KNEE), + p(RIGHT_ANKLE), + joint_confidence(track, &[RIGHT_HIP, RIGHT_KNEE, RIGHT_ANKLE], confidence), + ); + + add_signed_flex( + &mut joints, + "hip_left", + sub(p(LEFT_KNEE), left_hip), + up, + forward, + joint_confidence(track, &[LEFT_HIP, LEFT_KNEE], confidence), + ); + add_signed_flex( + &mut joints, + "hip_right", + sub(p(RIGHT_KNEE), right_hip), + up, + forward, + joint_confidence(track, &[RIGHT_HIP, RIGHT_KNEE], confidence), + ); + add_signed_flex( + &mut joints, + "shoulder_left", + sub(p(LEFT_ELBOW), p(LEFT_SHOULDER)), + up, + forward, + joint_confidence(track, &[LEFT_SHOULDER, LEFT_ELBOW], confidence), + ); + add_signed_flex( + &mut joints, + "shoulder_right", + sub(p(RIGHT_ELBOW), p(RIGHT_SHOULDER)), + up, + forward, + joint_confidence(track, &[RIGHT_SHOULDER, RIGHT_ELBOW], confidence), + ); + + let shoulder_mid = midpoint(p(LEFT_SHOULDER), p(RIGHT_SHOULDER)); + let torso = sub(shoulder_mid, position); + let hinge = angle_deg(torso, up).clamp(0.0, 180.0); + joints.push(JointTarget { + joint: "pelvis".into(), + action: "hinge".into(), + degrees: hinge, + confidence: joint_confidence( + track, + &[LEFT_SHOULDER, RIGHT_SHOULDER, LEFT_HIP, RIGHT_HIP], + confidence, + ), + }); + + Some(ActorObservation { + actor_id: format!("person_{}", track.id), + track_id: track.id, + confidence, + position, + joints, + }) + } +} + +fn track_confidence(track: &TrackObservation, fallback: f32) -> f32 { + let scored: Vec = track + .keypoints + .iter() + .map(|kp| kp.confidence) + .filter(|v| v.is_finite() && *v > 0.0) + .collect(); + let measured = if scored.is_empty() { + fallback + } else { + scored.iter().sum::() / scored.len() as f32 + }; + let freshness = 1.0 / (1.0 + track.time_since_update as f32 * 0.2); + (measured * freshness).clamp(0.0, 1.0) +} + +fn joint_confidence(track: &TrackObservation, indices: &[usize], fallback: f32) -> f32 { + indices.iter().fold(1.0_f32, |confidence, &index| { + let measured = track.keypoints[index].confidence; + confidence.min(if measured.is_finite() && measured > 0.0 { + measured + } else { + fallback + }) + }) +} + +fn add_flexion(out: &mut Vec, name: &str, a: Vec3, b: Vec3, c: Vec3, confidence: f32) { + let inner = angle_deg(sub(a, b), sub(c, b)); + if inner.is_finite() { + out.push(JointTarget { + joint: name.into(), + action: "flex".into(), + degrees: (180.0 - inner).clamp(0.0, 180.0), + confidence, + }); + } +} + +fn add_signed_flex( + out: &mut Vec, + name: &str, + limb: Vec3, + up: Vec3, + forward: Vec3, + confidence: f32, +) { + let down = scale(up, -1.0); + let degrees = dot(limb, forward).atan2(dot(limb, down)).to_degrees(); + if degrees.is_finite() { + out.push(JointTarget { + joint: name.into(), + action: if degrees >= 0.0 { "flex" } else { "extend" }.into(), + degrees: degrees.abs().clamp(0.0, 180.0), + confidence, + }); + } +} + +fn midpoint(a: Vec3, b: Vec3) -> Vec3 { + scale(add(a, b), 0.5) +} +fn add(a: Vec3, b: Vec3) -> Vec3 { + Vec3::new(a.x + b.x, a.y + b.y, a.z + b.z) +} +fn sub(a: Vec3, b: Vec3) -> Vec3 { + Vec3::new(a.x - b.x, a.y - b.y, a.z - b.z) +} +fn scale(v: Vec3, s: f32) -> Vec3 { + Vec3::new(v.x * s, v.y * s, v.z * s) +} +fn dot(a: Vec3, b: Vec3) -> f32 { + a.x * b.x + a.y * b.y + a.z * b.z +} +fn norm(v: Vec3) -> f32 { + dot(v, v).sqrt() +} +fn normalize(v: Vec3) -> Vec3 { + scale(v, 1.0 / norm(v).max(1e-9)) +} +fn angle_deg(a: Vec3, b: Vec3) -> f32 { + let denom = norm(a) * norm(b); + if denom < 1e-9 { + return 0.0; + } + (dot(a, b) / denom).clamp(-1.0, 1.0).acos().to_degrees() +} + +#[cfg(test)] +mod tests { + use super::*; + + fn standing_track() -> TrackObservation { + let mut keypoints = [KeypointObservation::default(); NUM_KEYPOINTS]; + let mut set = |index, x, y, z| { + keypoints[index] = KeypointObservation { + position: Vec3::new(x, y, z), + confidence: 0.9, + }; + }; + set(LEFT_SHOULDER, -0.2, 1.5, 0.0); + set(RIGHT_SHOULDER, 0.2, 1.5, 0.0); + set(LEFT_ELBOW, -0.25, 1.1, 0.0); + set(RIGHT_ELBOW, 0.25, 1.1, 0.0); + set(LEFT_WRIST, -0.25, 0.75, 0.0); + set(RIGHT_WRIST, 0.25, 0.75, 0.0); + set(LEFT_HIP, -0.12, 0.9, 0.0); + set(RIGHT_HIP, 0.12, 0.9, 0.0); + set(LEFT_KNEE, -0.12, 0.5, 0.0); + set(RIGHT_KNEE, 0.12, 0.5, 0.0); + set(LEFT_ANKLE, -0.12, 0.05, 0.0); + set(RIGHT_ANKLE, 0.12, 0.05, 0.0); + TrackObservation { + id: 7, + state: TrackState::Active, + keypoints, + time_since_update: 0, + } + } + + #[test] + fn converts_track_to_named_actor() { + let adapter = RuViewAdapter::new(AdapterConfig::default()).unwrap(); + let actor = adapter.actor(&standing_track()).unwrap(); + assert_eq!(actor.actor_id, "person_7"); + assert_eq!(actor.track_id, 7); + assert!(actor.joints.len() >= 9); + assert!(actor.joints.iter().all(|j| j.degrees.is_finite())); + } + + #[test] + fn rejects_invalid_axes() { + let config = AdapterConfig { + world_up: Vec3::default(), + ..Default::default() + }; + assert!(RuViewAdapter::new(config).is_err()); + } + + #[test] + fn rejects_parallel_coordinate_axes() { + let config = AdapterConfig { + world_forward: Vec3::new(0.0, 2.0, 0.0), + ..Default::default() + }; + assert!(RuViewAdapter::new(config).is_err()); + } + + #[test] + fn filters_stale_low_confidence_tracks() { + let mut track = standing_track(); + track.time_since_update = 100; + let adapter = RuViewAdapter::new(AdapterConfig::default()).unwrap(); + assert!(adapter.actor(&track).is_none()); + } + + #[cfg(feature = "ruview")] + #[test] + fn converts_native_ruview_pose_track() { + use wifi_densepose_signal::ruvsense::{PoseTrack, TrackId, TrackLifecycleState}; + + let source = standing_track(); + let positions = std::array::from_fn(|i| { + let p = source.keypoints[i].position; + [p.x, p.y, p.z] + }); + let mut native = PoseTrack::new(TrackId(23), &positions, 0, 128); + native.lifecycle = TrackLifecycleState::Active; + let observed = TrackObservation::from(&native); + assert_eq!(observed.id, 23); + assert_eq!(observed.state, TrackState::Active); + assert_eq!( + observed.keypoints[LEFT_HIP].position, + source.keypoints[LEFT_HIP].position + ); + } +} diff --git a/v2/crates/wifi-densepose-posecode/src/error.rs b/v2/crates/wifi-densepose-posecode/src/error.rs new file mode 100644 index 00000000..5dbae44c --- /dev/null +++ b/v2/crates/wifi-densepose-posecode/src/error.rs @@ -0,0 +1,24 @@ +//! Error types for parser and adapter boundaries. + +/// Crate result alias. +pub type Result = std::result::Result; + +/// Errors are structured and line anchored where input text is involved. +#[derive(Debug, thiserror::Error, PartialEq)] +pub enum Error { + #[error("line {line}: {message}")] + Parse { line: usize, message: String }, + #[error("invalid scene: {0}")] + Validation(String), + #[error("invalid observation: {0}")] + Observation(String), +} + +impl Error { + pub(crate) fn parse(line: usize, message: impl Into) -> Self { + Self::Parse { + line, + message: message.into(), + } + } +} diff --git a/v2/crates/wifi-densepose-posecode/src/lib.rs b/v2/crates/wifi-densepose-posecode/src/lib.rs new file mode 100644 index 00000000..cab5207a --- /dev/null +++ b/v2/crates/wifi-densepose-posecode/src/lib.rs @@ -0,0 +1,29 @@ +//! Multi-actor semantic motion scenes for RuView. +//! +//! This crate uses the PoseCode movement vocabulary and implements the +//! RuView 0.2 multi-actor extension from ADR-266. It deliberately separates +//! the high-rate observed pose stream from the compact semantic scene: +//! [`adapter::RuViewAdapter`] converts persistent RuView tracks into observed +//! frames, [`segmenter::PhaseSegmenter`] reduces those frames to synchronized +//! phases, and [`serialize::to_posecode`] emits deterministic text. + +pub mod adapter; +pub mod error; +pub mod model; +pub mod parser; +pub mod segmenter; +pub mod serialize; +pub mod validate; + +pub use adapter::{ + AdapterConfig, KeypointObservation, RuViewAdapter, TrackObservation, TrackState, +}; +pub use error::{Error, Result}; +pub use model::*; +pub use parser::parse_posecode; +pub use segmenter::{PhaseSegmenter, SegmenterConfig}; +pub use serialize::to_posecode; +pub use validate::{validate_scene, ValidationConfig, ValidationIssue, ValidationSeverity}; + +/// Protocol version implemented by this crate. +pub const PROTOCOL_VERSION: &str = "0.2"; diff --git a/v2/crates/wifi-densepose-posecode/src/model.rs b/v2/crates/wifi-densepose-posecode/src/model.rs new file mode 100644 index 00000000..fbfac4db --- /dev/null +++ b/v2/crates/wifi-densepose-posecode/src/model.rs @@ -0,0 +1,203 @@ +//! Renderer independent scene model. + +use serde::{Deserialize, Serialize}; + +/// Hard limits keep untrusted scene documents bounded in memory and time. +pub const MAX_ACTORS: usize = 32; +pub const MAX_PHASES: usize = 10_000; +pub const MAX_TARGETS_PER_ACTOR: usize = 64; +pub const MAX_NAME_BYTES: usize = 128; + +#[derive(Debug, Clone, Copy, Default, PartialEq, Serialize, Deserialize)] +pub struct Vec3 { + pub x: f32, + pub y: f32, + pub z: f32, +} + +impl Vec3 { + pub fn new(x: f32, y: f32, z: f32) -> Self { + Self { x, y, z } + } + + pub fn is_finite(self) -> bool { + self.x.is_finite() && self.y.is_finite() && self.z.is_finite() + } + + pub fn distance(self, other: Self) -> f32 { + let dx = self.x - other.x; + let dy = self.y - other.y; + let dz = self.z - other.z; + (dx * dx + dy * dy + dz * dz).sqrt() + } +} + +#[derive(Debug, Clone, Copy, Default, PartialEq, Serialize, Deserialize)] +pub struct EulerDeg { + pub x: f32, + pub y: f32, + pub z: f32, +} + +impl EulerDeg { + pub fn is_finite(self) -> bool { + self.x.is_finite() && self.y.is_finite() && self.z.is_finite() + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)] +#[serde(rename_all = "snake_case")] +pub enum SceneSource { + Authored, + ObservedWifiCsi, + Imported, +} + +impl Default for SceneSource { + fn default() -> Self { + Self::Authored + } +} + +#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)] +pub struct Actor { + pub id: String, + pub rig: String, + pub start_pose: String, + pub track_id: Option, + pub confidence: f32, + pub position: Vec3, +} + +impl Actor { + pub fn humanoid(id: impl Into) -> Self { + Self { + id: id.into(), + rig: "humanoid".into(), + start_pose: "standing".into(), + track_id: None, + confidence: 1.0, + position: Vec3::default(), + } + } +} + +#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)] +pub struct JointTarget { + pub joint: String, + pub action: String, + pub degrees: f32, + pub confidence: f32, +} + +#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)] +pub struct ActorTargets { + pub actor_id: String, + pub joints: Vec, + pub ground_lock: Vec, + pub travel: Option, + pub confidence: f32, +} + +impl ActorTargets { + pub fn new(actor_id: impl Into) -> Self { + Self { + actor_id: actor_id.into(), + joints: Vec::new(), + ground_lock: Vec::new(), + travel: None, + confidence: 1.0, + } + } +} + +#[derive(Debug, Clone, PartialEq, Eq, Serialize, Deserialize)] +pub struct EffectorRef { + pub actor_id: String, + pub effector: String, +} + +#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)] +pub struct Contact { + pub from: EffectorRef, + pub to: EffectorRef, + pub confidence: f32, +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)] +#[serde(rename_all = "kebab-case")] +pub enum Timing { + Flow, + Settle, + Drive, + Snap, + Linear, + EaseIn, + EaseOut, + EaseInOut, +} + +impl Timing { + pub fn as_str(self) -> &'static str { + match self { + Self::Flow => "flow", + Self::Settle => "settle", + Self::Drive => "drive", + Self::Snap => "snap", + Self::Linear => "linear", + Self::EaseIn => "ease-in", + Self::EaseOut => "ease-out", + Self::EaseInOut => "ease-in-out", + } + } +} + +#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)] +pub struct Phase { + pub name: String, + pub start_ms: u64, + pub duration_ms: u64, + pub timing: Timing, + pub actors: Vec, + pub contacts: Vec, +} + +#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)] +pub struct Scene { + pub version: String, + pub name: String, + pub source: SceneSource, + pub actors: Vec, + pub phases: Vec, + pub repeat: u32, +} + +impl Scene { + pub fn new(name: impl Into) -> Self { + Self { + version: crate::PROTOCOL_VERSION.into(), + name: name.into(), + source: SceneSource::Authored, + actors: Vec::new(), + phases: Vec::new(), + repeat: 1, + } + } +} + +/// One actor at one observed instant. Raw observations remain separate from +/// semantic phases so 20 Hz input does not become unreadable scene text. +#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)] +pub struct ActorObservation { + pub actor_id: String, + pub track_id: u64, + pub confidence: f32, + pub position: Vec3, + pub joints: Vec, +} + +#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)] +pub struct SceneFrame { + pub timestamp_ms: u64, + pub actors: Vec, +} diff --git a/v2/crates/wifi-densepose-posecode/src/parser.rs b/v2/crates/wifi-densepose-posecode/src/parser.rs new file mode 100644 index 00000000..7e887659 --- /dev/null +++ b/v2/crates/wifi-densepose-posecode/src/parser.rs @@ -0,0 +1,375 @@ +//! Bounded line parser for the PoseCode 0.2 multi-actor extension. + +use crate::error::{Error, Result}; +use crate::model::*; +use crate::validate::{validate_scene, ValidationConfig, ValidationSeverity}; + +const MAX_INPUT_BYTES: usize = 1_048_576; +const MAX_LINE_BYTES: usize = 4096; + +pub fn parse_posecode(source: &str) -> Result { + if source.len() > MAX_INPUT_BYTES { + return Err(Error::parse(0, "document exceeds 1 MiB")); + } + let mut scene: Option = None; + let mut current_actor: Option = None; + let mut current_phase: Option = None; + let mut elapsed_ms = 0_u64; + + for (index, raw) in source.lines().enumerate() { + let line_no = index + 1; + if raw.len() > MAX_LINE_BYTES { + return Err(Error::parse(line_no, "line exceeds 4096 bytes")); + } + let line = strip_comment(raw).trim(); + if line.is_empty() { + continue; + } + + if scene.is_none() { + let rest = line + .strip_prefix("posecode scene ") + .ok_or_else(|| Error::parse(line_no, "expected posecode scene header"))?; + scene = Some(Scene::new(parse_quoted(rest, line_no)?)); + continue; + } + let doc = scene.as_mut().expect("initialized above"); + + if let Some(value) = line.strip_prefix("source ") { + doc.source = match value { + "authored" => SceneSource::Authored, + "observed_wifi_csi" => SceneSource::ObservedWifiCsi, + "imported" => SceneSource::Imported, + _ => return Err(Error::parse(line_no, "unknown source")), + }; + continue; + } + if let Some(value) = line.strip_prefix("actor ") { + let id = value + .strip_suffix(':') + .ok_or_else(|| Error::parse(line_no, "actor declaration must end with ':'"))?; + doc.actors.push(Actor::humanoid(id.trim())); + current_actor = Some(doc.actors.len() - 1); + current_phase = None; + continue; + } + if let Some(value) = line.strip_prefix("step ") { + let (name, tail) = take_quoted(value, line_no)?; + let fields: Vec<&str> = tail.trim_end_matches(':').split_whitespace().collect(); + if fields.len() != 2 { + return Err(Error::parse(line_no, "step requires duration and timing")); + } + let duration_ms = parse_duration(fields[0], line_no)?; + let timing = parse_timing(fields[1], line_no)?; + doc.phases.push(Phase { + name, + start_ms: elapsed_ms, + duration_ms, + timing, + actors: Vec::new(), + contacts: Vec::new(), + }); + elapsed_ms = elapsed_ms + .checked_add(duration_ms) + .ok_or_else(|| Error::parse(line_no, "timeline overflow"))?; + current_phase = Some(doc.phases.len() - 1); + current_actor = None; + continue; + } + if let Some(value) = line.strip_prefix("repeat ") { + doc.repeat = value + .parse() + .map_err(|_| Error::parse(line_no, "repeat must be an integer"))?; + if doc.repeat == 0 || doc.repeat > 10_000 { + return Err(Error::parse(line_no, "repeat must be 1 to 10000")); + } + continue; + } + + if let Some(ai) = current_actor { + parse_actor_field(&mut doc.actors[ai], line, line_no)?; + continue; + } + if let Some(pi) = current_phase { + parse_phase_field(&mut doc.phases[pi], line, line_no)?; + continue; + } + return Err(Error::parse(line_no, "directive is outside actor or step")); + } + + let scene = scene.ok_or_else(|| Error::parse(0, "empty document"))?; + if let Some(issue) = validate_scene(&scene, &ValidationConfig::default()) + .into_iter() + .find(|i| i.severity == ValidationSeverity::Error) + { + return Err(Error::Validation(format!( + "{}: {}", + issue.path, issue.message + ))); + } + Ok(scene) +} + +fn parse_actor_field(actor: &mut Actor, line: &str, line_no: usize) -> Result<()> { + if let Some(value) = line.strip_prefix("rig ") { + actor.rig = value.trim().into(); + return Ok(()); + } + if let Some(value) = line.strip_prefix("pose start = ") { + actor.start_pose = value.trim().into(); + return Ok(()); + } + if let Some(value) = line.strip_prefix("track ") { + actor.track_id = Some( + value + .parse() + .map_err(|_| Error::parse(line_no, "track must be an integer"))?, + ); + return Ok(()); + } + if let Some(value) = line.strip_prefix("confidence ") { + actor.confidence = parse_confidence(value, line_no)?; + return Ok(()); + } + if let Some(value) = line.strip_prefix("position ") { + actor.position = parse_vec3(value, line_no)?; + return Ok(()); + } + Err(Error::parse(line_no, "unknown actor directive")) +} + +fn parse_phase_field(phase: &mut Phase, line: &str, line_no: usize) -> Result<()> { + if let Some(value) = line.strip_prefix("contact ") { + let fields: Vec<&str> = value.split_whitespace().collect(); + if !(fields.len() == 2 || fields.len() == 3) { + return Err(Error::parse( + line_no, + "contact requires two effectors and optional confidence", + )); + } + phase.contacts.push(Contact { + from: parse_effector(fields[0], line_no)?, + to: parse_effector(fields[1], line_no)?, + confidence: if fields.len() == 3 { + parse_confidence(fields[2], line_no)? + } else { + 1.0 + }, + }); + return Ok(()); + } + let (lhs, rhs) = line + .split_once(':') + .ok_or_else(|| Error::parse(line_no, "phase directive requires ':'"))?; + let (actor_id, target) = lhs + .trim() + .split_once('.') + .ok_or_else(|| Error::parse(line_no, "target must be actor qualified"))?; + let actor = phase_actor_mut(phase, actor_id); + match target { + "ground-lock" => { + actor.ground_lock = rhs + .split(',') + .map(|s| s.trim().to_string()) + .filter(|s| !s.is_empty()) + .collect(); + } + "travel" => { + actor.travel = Some(parse_vec3_or_xz(rhs, line_no)?); + } + joint => { + let fields: Vec<&str> = rhs.split_whitespace().collect(); + if !(fields.len() == 2 || fields.len() == 3) { + return Err(Error::parse( + line_no, + "joint target requires action, degrees and optional confidence", + )); + } + actor.joints.push(JointTarget { + joint: joint.into(), + action: fields[0].into(), + degrees: fields[1] + .parse() + .map_err(|_| Error::parse(line_no, "degrees must be numeric"))?, + confidence: if fields.len() == 3 { + parse_confidence(fields[2], line_no)? + } else { + 1.0 + }, + }); + } + } + Ok(()) +} + +fn phase_actor_mut<'a>(phase: &'a mut Phase, id: &str) -> &'a mut ActorTargets { + if let Some(index) = phase.actors.iter().position(|a| a.actor_id == id) { + return &mut phase.actors[index]; + } + phase.actors.push(ActorTargets::new(id)); + phase.actors.last_mut().expect("just pushed") +} + +fn parse_effector(value: &str, line: usize) -> Result { + let (actor_id, effector) = value + .split_once('.') + .ok_or_else(|| Error::parse(line, "effector must be actor qualified"))?; + Ok(EffectorRef { + actor_id: actor_id.into(), + effector: effector.into(), + }) +} + +fn parse_vec3(value: &str, line: usize) -> Result { + let f: Vec<&str> = value.split_whitespace().collect(); + if f.len() != 3 { + return Err(Error::parse(line, "position requires x y z")); + } + Ok(Vec3::new( + parse_f32(f[0], line)?, + parse_f32(f[1], line)?, + parse_f32(f[2], line)?, + )) +} + +fn parse_vec3_or_xz(value: &str, line: usize) -> Result { + let f: Vec<&str> = value.split_whitespace().collect(); + match f.len() { + 2 => Ok(Vec3::new( + parse_f32(f[0], line)?, + 0.0, + parse_f32(f[1], line)?, + )), + 3 => parse_vec3(value, line), + _ => Err(Error::parse(line, "travel requires x z or x y z")), + } +} + +fn parse_f32(value: &str, line: usize) -> Result { + let v: f32 = value + .parse() + .map_err(|_| Error::parse(line, "expected finite number"))?; + if !v.is_finite() { + return Err(Error::parse(line, "expected finite number")); + } + Ok(v) +} + +fn parse_confidence(value: &str, line: usize) -> Result { + let v = parse_f32(value, line)?; + if !(0.0..=1.0).contains(&v) { + return Err(Error::parse(line, "confidence must be in [0, 1]")); + } + Ok(v) +} + +fn parse_duration(value: &str, line: usize) -> Result { + let seconds = value + .strip_suffix('s') + .ok_or_else(|| Error::parse(line, "duration must end in s"))?; + let seconds = parse_f32(seconds, line)?; + if seconds <= 0.0 || seconds > 300.0 { + return Err(Error::parse(line, "duration must be in (0, 300] seconds")); + } + Ok((seconds * 1000.0).round() as u64) +} + +fn parse_timing(value: &str, line: usize) -> Result { + match value { + "flow" => Ok(Timing::Flow), + "settle" => Ok(Timing::Settle), + "drive" => Ok(Timing::Drive), + "snap" => Ok(Timing::Snap), + "linear" => Ok(Timing::Linear), + "ease-in" => Ok(Timing::EaseIn), + "ease-out" => Ok(Timing::EaseOut), + "ease-in-out" => Ok(Timing::EaseInOut), + _ => Err(Error::parse(line, "unknown timing")), + } +} + +fn parse_quoted(value: &str, line: usize) -> Result { + let (quoted, tail) = take_quoted(value, line)?; + if !tail.trim().is_empty() { + return Err(Error::parse(line, "unexpected text after quoted name")); + } + Ok(quoted) +} + +fn take_quoted(value: &str, line: usize) -> Result<(String, &str)> { + let rest = value + .strip_prefix('"') + .ok_or_else(|| Error::parse(line, "expected quoted name"))?; + let end = rest + .find('"') + .ok_or_else(|| Error::parse(line, "unterminated quoted name"))?; + Ok((rest[..end].to_string(), &rest[end + 1..])) +} + +fn strip_comment(line: &str) -> &str { + let hash = line.find('#'); + let slash = line.find("//"); + match (hash, slash) { + (Some(a), Some(b)) => &line[..a.min(b)], + (Some(a), None) | (None, Some(a)) => &line[..a], + (None, None) => line, + } +} + +#[cfg(test)] +mod tests { + use super::*; + + const SCENE: &str = r#"posecode scene "Assisted squat" +source observed_wifi_csi +actor patient: + rig humanoid + pose start = standing + track 7 + confidence 0.82 + position 0 0 0 +actor therapist: + rig humanoid + pose start = standing + position 1.2 0 0 +step "Lower" 1.5s flow: + patient.knee_left: flex 95 0.8 + patient.ground-lock: feet + therapist.shoulder_left: flex 30 + contact therapist.hand_left patient.shoulder_right 0.7 +repeat 2 +"#; + + #[test] + fn parses_multiple_actors_and_contacts() { + let scene = parse_posecode(SCENE).unwrap(); + assert_eq!(scene.actors.len(), 2); + assert_eq!(scene.phases[0].actors.len(), 2); + assert_eq!(scene.phases[0].contacts.len(), 1); + assert_eq!(scene.repeat, 2); + } + + #[test] + fn rejects_unknown_actor_reference() { + let bad = "posecode scene \"x\"\nactor p1:\n rig humanoid\nstep \"x\" 1s flow:\n ghost.knee_left: flex 20"; + assert!(parse_posecode(bad).is_err()); + } + + #[test] + fn rejects_non_finite_number() { + let bad = "posecode scene \"x\"\nactor p1:\n position NaN 0 0"; + assert!(parse_posecode(bad).is_err()); + } + + #[test] + fn rejects_duplicate_actors() { + let bad = "posecode scene \"x\"\nactor p1:\n rig humanoid\nactor p1:\n rig humanoid"; + assert!(parse_posecode(bad).is_err()); + } + + #[test] + fn rejects_oversized_document_before_parsing() { + let bad = "x".repeat(MAX_INPUT_BYTES + 1); + assert!(parse_posecode(&bad).is_err()); + } +} diff --git a/v2/crates/wifi-densepose-posecode/src/segmenter.rs b/v2/crates/wifi-densepose-posecode/src/segmenter.rs new file mode 100644 index 00000000..c146f451 --- /dev/null +++ b/v2/crates/wifi-densepose-posecode/src/segmenter.rs @@ -0,0 +1,212 @@ +//! Confidence preserving reduction of high-rate observations into phases. + +use std::collections::{BTreeMap, BTreeSet}; + +use crate::error::{Error, Result}; +use crate::model::*; + +#[derive(Debug, Clone)] +pub struct SegmenterConfig { + pub minimum_phase_ms: u64, + pub maximum_phase_ms: u64, + pub stable_velocity_deg_s: f32, +} + +impl Default for SegmenterConfig { + fn default() -> Self { + Self { + minimum_phase_ms: 200, + maximum_phase_ms: 3_000, + stable_velocity_deg_s: 8.0, + } + } +} + +#[derive(Debug, Clone)] +pub struct PhaseSegmenter { + config: SegmenterConfig, +} + +impl PhaseSegmenter { + pub fn new(config: SegmenterConfig) -> Result { + if config.minimum_phase_ms == 0 + || config.maximum_phase_ms < config.minimum_phase_ms + || !config.stable_velocity_deg_s.is_finite() + || config.stable_velocity_deg_s < 0.0 + { + return Err(Error::Observation("invalid segmenter configuration".into())); + } + Ok(Self { config }) + } + + pub fn segment(&self, name: impl Into, frames: &[SceneFrame]) -> Result { + if frames.is_empty() { + return Err(Error::Observation("at least one frame is required".into())); + } + for pair in frames.windows(2) { + if pair[1].timestamp_ms <= pair[0].timestamp_ms { + return Err(Error::Observation( + "timestamps must be strictly increasing".into(), + )); + } + } + let mut scene = Scene::new(name); + scene.source = SceneSource::ObservedWifiCsi; + scene.actors = actors_from_frames(frames); + + if frames.len() == 1 { + return Ok(scene); + } + let mut start = 0_usize; + let mut was_stable = true; + for i in 1..frames.len() { + let dt = frames[i].timestamp_ms - frames[i - 1].timestamp_ms; + let velocity = angular_velocity(&frames[i - 1], &frames[i], dt); + let stable = velocity <= self.config.stable_velocity_deg_s; + let elapsed = frames[i].timestamp_ms - frames[start].timestamp_ms; + let membership_changed = actor_ids(&frames[i - 1]) != actor_ids(&frames[i]); + let boundary = membership_changed + || elapsed >= self.config.maximum_phase_ms + || (stable && !was_stable && elapsed >= self.config.minimum_phase_ms); + if boundary { + scene + .phases + .push(make_phase(scene.phases.len(), &frames[start], &frames[i])); + start = i; + } + was_stable = stable; + } + let last = frames.len() - 1; + if start < last { + scene.phases.push(make_phase( + scene.phases.len(), + &frames[start], + &frames[last], + )); + } + Ok(scene) + } +} + +fn actors_from_frames(frames: &[SceneFrame]) -> Vec { + let mut found = BTreeMap::::new(); + for frame in frames { + for observed in &frame.actors { + found + .entry(observed.track_id) + .and_modify(|actor| { + actor.confidence = actor.confidence.max(observed.confidence); + actor.position = observed.position; + }) + .or_insert_with(|| { + let mut actor = Actor::humanoid(observed.actor_id.clone()); + actor.track_id = Some(observed.track_id); + actor.confidence = observed.confidence; + actor.position = observed.position; + actor + }); + } + } + found.into_values().collect() +} + +fn make_phase(index: usize, start: &SceneFrame, end: &SceneFrame) -> Phase { + let actors = end + .actors + .iter() + .map(|observed| ActorTargets { + actor_id: observed.actor_id.clone(), + joints: observed.joints.clone(), + ground_lock: Vec::new(), + travel: Some(observed.position), + confidence: observed.confidence, + }) + .collect(); + Phase { + name: format!("Observed {}", index + 1), + start_ms: start.timestamp_ms, + duration_ms: (end.timestamp_ms - start.timestamp_ms).max(1), + timing: Timing::Linear, + actors, + contacts: Vec::new(), + } +} + +fn actor_ids(frame: &SceneFrame) -> BTreeSet { + frame.actors.iter().map(|a| a.track_id).collect() +} + +fn angular_velocity(previous: &SceneFrame, current: &SceneFrame, dt_ms: u64) -> f32 { + let mut sum = 0.0; + let mut count = 0_u32; + for a in ¤t.actors { + let Some(before) = previous.actors.iter().find(|p| p.track_id == a.track_id) else { + return f32::INFINITY; + }; + for joint in &a.joints { + if let Some(old) = before + .joints + .iter() + .find(|j| j.joint == joint.joint && j.action == joint.action) + { + sum += (joint.degrees - old.degrees).abs() * joint.confidence.min(old.confidence); + count += 1; + } + } + } + if count == 0 { + return 0.0; + } + (sum / count as f32) / (dt_ms as f32 / 1000.0).max(0.001) +} + +#[cfg(test)] +mod tests { + use super::*; + + fn frame(ms: u64, angle: f32) -> SceneFrame { + SceneFrame { + timestamp_ms: ms, + actors: vec![ActorObservation { + actor_id: "p1".into(), + track_id: 1, + confidence: 0.9, + position: Vec3::default(), + joints: vec![JointTarget { + joint: "knee_left".into(), + action: "flex".into(), + degrees: angle, + confidence: 0.9, + }], + }], + } + } + + #[test] + fn creates_compact_observed_scene() { + let frames = vec![ + frame(0, 0.0), + frame(100, 20.0), + frame(200, 40.0), + frame(300, 40.1), + frame(500, 40.1), + ]; + let scene = PhaseSegmenter::new(SegmenterConfig::default()) + .unwrap() + .segment("squat", &frames) + .unwrap(); + assert_eq!(scene.actors.len(), 1); + assert!(!scene.phases.is_empty()); + assert!(scene.phases.len() < frames.len()); + assert_eq!(scene.source, SceneSource::ObservedWifiCsi); + } + + #[test] + fn rejects_reversed_time() { + let frames = vec![frame(100, 0.0), frame(50, 1.0)]; + assert!(PhaseSegmenter::new(SegmenterConfig::default()) + .unwrap() + .segment("bad", &frames) + .is_err()); + } +} diff --git a/v2/crates/wifi-densepose-posecode/src/serialize.rs b/v2/crates/wifi-densepose-posecode/src/serialize.rs new file mode 100644 index 00000000..647981d6 --- /dev/null +++ b/v2/crates/wifi-densepose-posecode/src/serialize.rs @@ -0,0 +1,131 @@ +//! Canonical deterministic PoseCode text serialization. + +use std::fmt::Write; + +use crate::model::*; + +pub fn to_posecode(scene: &Scene) -> String { + let mut out = String::new(); + writeln!(out, "posecode scene \"{}\"", safe_text(&scene.name)).unwrap(); + writeln!(out, "source {}", source_name(scene.source)).unwrap(); + for actor in &scene.actors { + writeln!(out, "actor {}:", actor.id).unwrap(); + writeln!(out, " rig {}", actor.rig).unwrap(); + writeln!(out, " pose start = {}", actor.start_pose).unwrap(); + if let Some(track) = actor.track_id { + writeln!(out, " track {track}").unwrap(); + } + writeln!(out, " confidence {}", number(actor.confidence)).unwrap(); + writeln!( + out, + " position {} {} {}", + number(actor.position.x), + number(actor.position.y), + number(actor.position.z) + ) + .unwrap(); + } + for phase in &scene.phases { + writeln!( + out, + "step \"{}\" {}s {}:", + safe_text(&phase.name), + number(phase.duration_ms as f32 / 1000.0), + phase.timing.as_str() + ) + .unwrap(); + for actor in &phase.actors { + for joint in &actor.joints { + writeln!( + out, + " {}.{}: {} {} {}", + actor.actor_id, + joint.joint, + joint.action, + number(joint.degrees), + number(joint.confidence) + ) + .unwrap(); + } + if !actor.ground_lock.is_empty() { + writeln!( + out, + " {}.ground-lock: {}", + actor.actor_id, + actor.ground_lock.join(", ") + ) + .unwrap(); + } + if let Some(p) = actor.travel { + writeln!( + out, + " {}.travel: {} {} {}", + actor.actor_id, + number(p.x), + number(p.y), + number(p.z) + ) + .unwrap(); + } + } + for contact in &phase.contacts { + writeln!( + out, + " contact {}.{} {}.{} {}", + contact.from.actor_id, + contact.from.effector, + contact.to.actor_id, + contact.to.effector, + number(contact.confidence) + ) + .unwrap(); + } + } + writeln!(out, "repeat {}", scene.repeat).unwrap(); + out +} + +fn source_name(source: SceneSource) -> &'static str { + match source { + SceneSource::Authored => "authored", + SceneSource::ObservedWifiCsi => "observed_wifi_csi", + SceneSource::Imported => "imported", + } +} + +fn safe_text(value: &str) -> String { + value + .chars() + .filter(|c| !c.is_control() && *c != '"') + .collect() +} + +fn number(value: f32) -> String { + if value == 0.0 { + return "0".into(); + } + let mut text = format!("{value:.4}"); + while text.ends_with('0') { + text.pop(); + } + if text.ends_with('.') { + text.pop(); + } + text +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::parse_posecode; + + #[test] + fn canonical_text_round_trips() { + let mut scene = Scene::new("two people"); + scene.actors.push(Actor::humanoid("p1")); + scene.actors.push(Actor::humanoid("p2")); + let text = to_posecode(&scene); + assert_eq!(parse_posecode(&text).unwrap(), scene); + assert_eq!(to_posecode(&parse_posecode(&text).unwrap()), text); + } +} diff --git a/v2/crates/wifi-densepose-posecode/src/validate.rs b/v2/crates/wifi-densepose-posecode/src/validate.rs new file mode 100644 index 00000000..b68a73c1 --- /dev/null +++ b/v2/crates/wifi-densepose-posecode/src/validate.rs @@ -0,0 +1,202 @@ +//! Deterministic scene validation with provenance-aware range policy. + +use std::collections::HashSet; + +use crate::model::*; + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum ValidationSeverity { + Warning, + Error, +} + +#[derive(Debug, Clone, PartialEq)] +pub struct ValidationIssue { + pub severity: ValidationSeverity, + pub path: String, + pub message: String, +} + +#[derive(Debug, Clone)] +pub struct ValidationConfig { + pub max_actors: usize, + pub max_phases: usize, + /// Authored values outside general ROM are errors. Observed values are + /// warnings because RF estimates are not medical measurements. + pub authored_rom_is_error: bool, +} + +impl Default for ValidationConfig { + fn default() -> Self { + Self { + max_actors: MAX_ACTORS, + max_phases: MAX_PHASES, + authored_rom_is_error: true, + } + } +} + +pub fn validate_scene(scene: &Scene, config: &ValidationConfig) -> Vec { + let mut issues = Vec::new(); + if scene.name.is_empty() || scene.name.len() > MAX_NAME_BYTES { + error( + &mut issues, + "scene.name", + "name must contain 1 to 128 bytes", + ); + } + if scene.actors.len() > config.max_actors { + error(&mut issues, "scene.actors", "actor limit exceeded"); + } + if scene.phases.len() > config.max_phases { + error(&mut issues, "scene.phases", "phase limit exceeded"); + } + + let mut ids = HashSet::new(); + for (i, actor) in scene.actors.iter().enumerate() { + let path = format!("actors[{i}]"); + if !valid_id(&actor.id) { + error(&mut issues, &path, "invalid actor identifier"); + } + if !ids.insert(actor.id.as_str()) { + error(&mut issues, &path, "duplicate actor identifier"); + } + check_confidence(&mut issues, &format!("{path}.confidence"), actor.confidence); + if !actor.position.is_finite() { + error( + &mut issues, + &format!("{path}.position"), + "position must be finite", + ); + } + } + + let actor_ids: HashSet<&str> = scene.actors.iter().map(|a| a.id.as_str()).collect(); + for (pi, phase) in scene.phases.iter().enumerate() { + let path = format!("phases[{pi}]"); + if phase.duration_ms == 0 || phase.duration_ms > 300_000 { + error( + &mut issues, + &format!("{path}.duration_ms"), + "duration must be 1 to 300000 ms", + ); + } + let mut phase_actors = HashSet::new(); + for targets in &phase.actors { + if !actor_ids.contains(targets.actor_id.as_str()) { + error(&mut issues, &path, "phase references unknown actor"); + } + if !phase_actors.insert(targets.actor_id.as_str()) { + error(&mut issues, &path, "actor appears more than once in phase"); + } + if targets.joints.len() > MAX_TARGETS_PER_ACTOR { + error(&mut issues, &path, "joint target limit exceeded"); + } + check_confidence(&mut issues, &path, targets.confidence); + for joint in &targets.joints { + check_confidence(&mut issues, &path, joint.confidence); + if !joint.degrees.is_finite() { + error(&mut issues, &path, "joint angle must be finite"); + continue; + } + if let Some((min, max)) = range(&joint.joint, &joint.action) { + if joint.degrees < min || joint.degrees > max { + let severity = if scene.source == SceneSource::Authored + && config.authored_rom_is_error + { + ValidationSeverity::Error + } else { + ValidationSeverity::Warning + }; + issues.push(ValidationIssue { + severity, + path: path.clone(), + message: format!( + "{}.{} angle {} outside general range [{}, {}]", + joint.joint, joint.action, joint.degrees, min, max + ), + }); + } + } + } + } + for contact in &phase.contacts { + if !actor_ids.contains(contact.from.actor_id.as_str()) + || !actor_ids.contains(contact.to.actor_id.as_str()) + { + error(&mut issues, &path, "contact references unknown actor"); + } + check_confidence(&mut issues, &path, contact.confidence); + } + } + issues +} + +fn valid_id(id: &str) -> bool { + !id.is_empty() + && id.len() <= 64 + && id + .bytes() + .enumerate() + .all(|(i, b)| b.is_ascii_alphanumeric() || b == b'_' || (i > 0 && b == b'-')) +} + +fn check_confidence(issues: &mut Vec, path: &str, value: f32) { + if !value.is_finite() || !(0.0..=1.0).contains(&value) { + error(issues, path, "confidence must be finite and in [0, 1]"); + } +} + +fn error(issues: &mut Vec, path: &str, message: &str) { + issues.push(ValidationIssue { + severity: ValidationSeverity::Error, + path: path.into(), + message: message.into(), + }); +} + +fn range(joint: &str, action: &str) -> Option<(f32, f32)> { + match ( + joint.trim_end_matches("_left").trim_end_matches("_right"), + action, + ) { + ("shoulder", "flex") => Some((0.0, 180.0)), + ("shoulder", "extend") => Some((0.0, 60.0)), + ("elbow", "flex") => Some((0.0, 154.0)), + ("hip", "flex") => Some((0.0, 135.0)), + ("hip", "extend") => Some((0.0, 20.0)), + ("knee", "flex") => Some((0.0, 144.0)), + ("pelvis", "hinge") => Some((0.0, 120.0)), + _ => None, + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn observed_rom_is_warning_not_medical_error() { + let mut scene = Scene::new("observed"); + scene.source = SceneSource::ObservedWifiCsi; + scene.actors.push(Actor::humanoid("p1")); + let mut targets = ActorTargets::new("p1"); + targets.joints.push(JointTarget { + joint: "knee_left".into(), + action: "flex".into(), + degrees: 170.0, + confidence: 0.5, + }); + scene.phases.push(Phase { + name: "frame".into(), + start_ms: 0, + duration_ms: 50, + timing: Timing::Linear, + actors: vec![targets], + contacts: vec![], + }); + let issues = validate_scene(&scene, &ValidationConfig::default()); + assert_eq!(issues.len(), 1); + assert_eq!(issues[0].severity, ValidationSeverity::Warning); + } +}