mirror of
https://github.com/ruvnet/RuView
synced 2026-06-28 13:23:19 +00:00
fix(sensing-server): emit real field-derived person position/motion to /ws/sensing (#1050)
The Observatory 3D figure never animated because the sensing_update WS frame carried no per-person position/motion_score/pose — only image-space keypoints. The FigurePool/PoseSystem (and demo-data.js's own contract) animate each figure from persons[i].position (room-world), .motion_score (0..100), and .pose; none were on the live stream. Honest scope (Case 2): the pipeline has no calibrated per-person room localizer or per-person skeletal pose. New field_localize module extracts the strongest peak(s) from the real signal_field grid (subcarrier variances x motion-band power) and maps the peak cell to Observatory world coords with the exact _buildSignalField transform. motion_score is the measured motion_band_power passed through; pose is set only from a real aggregate posture estimate, else None (never a fabricated skeleton). Empty/below-threshold field -> persons: [] (no phantom); present person with no resolvable peak keeps position [0,0,0], not invented coords. attach_field_positions runs after the tracker step at all five broadcast sites. New position/motion_score/pose fields added to both PersonDetection structs. No UI change needed — the Observatory already reads these fields. Tests: field_localize peak/coordinate/empty/separation units + observatory_persons_field_position_tests (known-peak -> emitted position, empty-room -> no phantom, pose real-or-None, below-threshold honesty). sensing-server bin 441->451, 0 failed. Co-Authored-By: claude-flow <ruv@ruv.net>
This commit is contained in:
ruv
@@ -0,0 +1,241 @@
|
||||
//! Field-peak localization for the Observatory 3D view (issue #1050).
|
||||
//!
|
||||
//! ## What this is (and is not)
|
||||
//!
|
||||
//! The `/ws/sensing` `sensing_update` frame already carries a real `signal_field`
|
||||
//! — a 20×20 grid built by `generate_signal_field()` from **measured subcarrier
|
||||
//! variances** weighted by the **measured motion-band power**. The grid's hot
|
||||
//! cells are the strongest scatterers in that field representation; as the CSI
|
||||
//! changes (a person moving through the link), the peak cell moves with it.
|
||||
//!
|
||||
//! This module reads the **strongest peak(s)** out of that real field and maps
|
||||
//! the peak cell to the Observatory room's world coordinates. That gives the
|
||||
//! 3D figure a position + motion magnitude that are **derived from real signal
|
||||
//! data**, so the figure now tracks where the field energy concentrates.
|
||||
//!
|
||||
//! ### Honesty caveat (do not over-claim)
|
||||
//!
|
||||
//! The field's subcarrier→angle mapping in `generate_signal_field()` is a
|
||||
//! *representation*, not calibrated multistatic triangulation in metric room
|
||||
//! coordinates. A single ESP32 link cannot resolve a true (x, z) room position.
|
||||
//! So the emitted `position` is **"strongest field peak in the room model"**,
|
||||
//! not survey-grade localization. It is real (a function of live CSI), it moves
|
||||
//! with real motion, and it is honest about its source — but it is NOT a
|
||||
//! calibrated person fix. Per-person skeletal `pose` keypoints in room
|
||||
//! coordinates remain gated on the pose model + paired ground-truth data
|
||||
//! (ADR-079), so `pose` here is only ever set from a real aggregate posture
|
||||
//! estimate when one exists, and is `None` otherwise (never fabricated).
|
||||
//!
|
||||
//! ## Coordinate mapping
|
||||
//!
|
||||
//! The Observatory builds its field point cloud (see `ui/observatory/js/main.js`
|
||||
//! `_buildSignalField`) as, for grid cell `(ix, iz)` of a `20×20` grid:
|
||||
//!
|
||||
//! ```text
|
||||
//! world_x = (ix - gridSize/2) * 0.6
|
||||
//! world_z = (iz - gridSize/2) * 0.5
|
||||
//! world_y = 0 (floor)
|
||||
//! ```
|
||||
//!
|
||||
//! and indexes the field as `idx = iz * gridSize + ix` — identical to the
|
||||
//! server's `generate_signal_field()` layout (`values[z * grid + x]`). We map
|
||||
//! the peak cell with the **same** transform so the figure lands exactly on the
|
||||
//! field hotspot it is standing on.
|
||||
|
||||
/// World-space scale factor for the X (width) axis, matching the Observatory's
|
||||
/// `_buildSignalField`: `world_x = (ix - nx/2) * X_SCALE`.
|
||||
pub const X_SCALE: f64 = 0.6;
|
||||
/// World-space scale factor for the Z (depth) axis, matching the Observatory's
|
||||
/// `_buildSignalField`: `world_z = (iz - nz/2) * Z_SCALE`.
|
||||
pub const Z_SCALE: f64 = 0.5;
|
||||
|
||||
/// Minimum normalized field value (`signal_field.values` are normalized to
|
||||
/// `[0, 1]`) for a cell to be considered a real peak rather than background
|
||||
/// attenuation. Below this we treat the field as having no localizable hotspot.
|
||||
pub const PEAK_THRESHOLD: f64 = 0.35;
|
||||
|
||||
/// A localized field peak in Observatory world coordinates.
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub struct FieldPeak {
|
||||
/// World position `[x, y, z]` in Observatory scene units (meters). `y` is
|
||||
/// always `0.0` — the field is a floor-plane grid with no height info.
|
||||
pub position: [f64; 3],
|
||||
/// Normalized field intensity at the peak cell, in `[0, 1]`.
|
||||
pub intensity: f64,
|
||||
/// Source grid cell `(ix, iz)` the peak was read from (for tests/debug).
|
||||
pub cell: (usize, usize),
|
||||
}
|
||||
|
||||
/// Map a grid cell `(ix, iz)` of an `nx × nz` field to Observatory world
|
||||
/// coordinates, matching `ui/observatory/js/main.js::_buildSignalField`.
|
||||
#[must_use]
|
||||
pub fn cell_to_world(ix: usize, iz: usize, nx: usize, nz: usize) -> [f64; 3] {
|
||||
let wx = (ix as f64 - nx as f64 / 2.0) * X_SCALE;
|
||||
let wz = (iz as f64 - nz as f64 / 2.0) * Z_SCALE;
|
||||
[wx, 0.0, wz]
|
||||
}
|
||||
|
||||
/// Extract up to `max_peaks` strongest, spatially-separated peaks from a
|
||||
/// `signal_field` grid.
|
||||
///
|
||||
/// * `values` — row-major field grid, `values[iz * nx + ix]`, normalized to
|
||||
/// `[0, 1]` (as produced by `generate_signal_field`).
|
||||
/// * `nx`, `nz` — grid dimensions (the field's `grid_size` is `[nx, 1, nz]`).
|
||||
/// * `max_peaks` — how many person positions to extract (≥ 1).
|
||||
///
|
||||
/// Returns peaks sorted strongest-first. Each successive peak is forced to be
|
||||
/// at least `min_separation_cells` away from all previously selected peaks so
|
||||
/// two persons don't collapse onto the same hotspot. Returns an **empty**
|
||||
/// vector when no cell exceeds [`PEAK_THRESHOLD`] — an empty / no-presence
|
||||
/// field yields no phantom person.
|
||||
#[must_use]
|
||||
pub fn extract_peaks(
|
||||
values: &[f64],
|
||||
nx: usize,
|
||||
nz: usize,
|
||||
max_peaks: usize,
|
||||
min_separation_cells: f64,
|
||||
) -> Vec<FieldPeak> {
|
||||
if nx == 0 || nz == 0 || values.len() < nx * nz || max_peaks == 0 {
|
||||
return Vec::new();
|
||||
}
|
||||
|
||||
// Collect all cells above threshold, strongest first.
|
||||
let mut candidates: Vec<(usize, usize, f64)> = Vec::new();
|
||||
for iz in 0..nz {
|
||||
for ix in 0..nx {
|
||||
let v = values[iz * nx + ix];
|
||||
if v >= PEAK_THRESHOLD {
|
||||
candidates.push((ix, iz, v));
|
||||
}
|
||||
}
|
||||
}
|
||||
candidates.sort_by(|a, b| b.2.total_cmp(&a.2));
|
||||
|
||||
let mut peaks: Vec<FieldPeak> = Vec::new();
|
||||
for (ix, iz, v) in candidates {
|
||||
if peaks.len() >= max_peaks {
|
||||
break;
|
||||
}
|
||||
// Enforce spatial separation from already-chosen peaks (in cell units).
|
||||
let too_close = peaks.iter().any(|p| {
|
||||
let dx = p.cell.0 as f64 - ix as f64;
|
||||
let dz = p.cell.1 as f64 - iz as f64;
|
||||
(dx * dx + dz * dz).sqrt() < min_separation_cells
|
||||
});
|
||||
if too_close {
|
||||
continue;
|
||||
}
|
||||
peaks.push(FieldPeak {
|
||||
position: cell_to_world(ix, iz, nx, nz),
|
||||
intensity: v,
|
||||
cell: (ix, iz),
|
||||
});
|
||||
}
|
||||
peaks
|
||||
}
|
||||
|
||||
/// Convert measured `motion_band_power` to the `motion_score` scale the
|
||||
/// Observatory UI expects.
|
||||
///
|
||||
/// The UI compares `motion_score > 50` to switch between calm and energetic
|
||||
/// emission (see `_updateDotMatrixMist` / `_updateParticleTrail`). The raw
|
||||
/// `motion_band_power` is already in roughly that band for live ESP32 data
|
||||
/// (the issue reports `motion_band_power: 63.3` while moving), so we pass it
|
||||
/// through directly, clamped to a sane `[0, 100]` display range. This keeps the
|
||||
/// emitted value a **direct, real** function of measured motion energy rather
|
||||
/// than a re-scaled invention.
|
||||
#[must_use]
|
||||
pub fn motion_score_from_power(motion_band_power: f64) -> f64 {
|
||||
motion_band_power.clamp(0.0, 100.0)
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn cell_to_world_matches_observatory_layout() {
|
||||
// Center cell of a 20×20 grid maps near origin.
|
||||
let c = cell_to_world(10, 10, 20, 20);
|
||||
assert!((c[0] - 0.0).abs() < 1e-9);
|
||||
assert_eq!(c[1], 0.0);
|
||||
assert!((c[2] - 0.0).abs() < 1e-9);
|
||||
|
||||
// Corner cell (0,0) maps to the room's near-left corner.
|
||||
let corner = cell_to_world(0, 0, 20, 20);
|
||||
assert!((corner[0] - (-6.0)).abs() < 1e-9); // (0-10)*0.6
|
||||
assert!((corner[2] - (-5.0)).abs() < 1e-9); // (0-10)*0.5
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn extract_peaks_finds_known_hotspot() {
|
||||
// 20×20 field, all background, single strong peak at cell (15, 4).
|
||||
let nx = 20;
|
||||
let nz = 20;
|
||||
let mut values = vec![0.05; nx * nz];
|
||||
let peak_ix = 15;
|
||||
let peak_iz = 4;
|
||||
values[peak_iz * nx + peak_ix] = 1.0;
|
||||
|
||||
let peaks = extract_peaks(&values, nx, nz, 1, 3.0);
|
||||
assert_eq!(peaks.len(), 1);
|
||||
assert_eq!(peaks[0].cell, (peak_ix, peak_iz));
|
||||
|
||||
// Position must match the Observatory cell→world transform within tol.
|
||||
let expected = cell_to_world(peak_ix, peak_iz, nx, nz);
|
||||
assert!((peaks[0].position[0] - expected[0]).abs() < 1e-9);
|
||||
assert!((peaks[0].position[2] - expected[2]).abs() < 1e-9);
|
||||
// Sanity: (15-10)*0.6 = 3.0, (4-10)*0.5 = -3.0
|
||||
assert!((peaks[0].position[0] - 3.0).abs() < 1e-9);
|
||||
assert!((peaks[0].position[2] - (-3.0)).abs() < 1e-9);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn empty_field_yields_no_peaks() {
|
||||
let nx = 20;
|
||||
let nz = 20;
|
||||
// All cells below PEAK_THRESHOLD — no presence.
|
||||
let values = vec![0.10; nx * nz];
|
||||
let peaks = extract_peaks(&values, nx, nz, 3, 3.0);
|
||||
assert!(
|
||||
peaks.is_empty(),
|
||||
"below-threshold field must not produce a phantom peak"
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn two_separated_peaks_do_not_collapse() {
|
||||
let nx = 20;
|
||||
let nz = 20;
|
||||
let mut values = vec![0.05; nx * nz];
|
||||
values[2 * nx + 3] = 0.95; // peak A at (3, 2)
|
||||
values[15 * nx + 17] = 0.90; // peak B at (17, 15)
|
||||
|
||||
let peaks = extract_peaks(&values, nx, nz, 2, 3.0);
|
||||
assert_eq!(peaks.len(), 2);
|
||||
// Strongest first.
|
||||
assert_eq!(peaks[0].cell, (3, 2));
|
||||
assert_eq!(peaks[1].cell, (17, 15));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn nearby_secondary_peak_is_suppressed() {
|
||||
let nx = 20;
|
||||
let nz = 20;
|
||||
let mut values = vec![0.05; nx * nz];
|
||||
values[10 * nx + 10] = 1.00; // primary
|
||||
values[10 * nx + 11] = 0.99; // adjacent — should be suppressed (sep 3.0)
|
||||
|
||||
let peaks = extract_peaks(&values, nx, nz, 2, 3.0);
|
||||
assert_eq!(peaks.len(), 1, "adjacent cell must not become a 2nd person");
|
||||
assert_eq!(peaks[0].cell, (10, 10));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn motion_score_passthrough_and_clamp() {
|
||||
assert!((motion_score_from_power(63.3) - 63.3).abs() < 1e-9);
|
||||
assert_eq!(motion_score_from_power(-5.0), 0.0);
|
||||
assert_eq!(motion_score_from_power(250.0), 100.0);
|
||||
}
|
||||
}
|
||||
@@ -14,6 +14,7 @@ pub mod cli;
|
||||
pub mod csi;
|
||||
mod engine_bridge;
|
||||
mod field_bridge;
|
||||
mod field_localize;
|
||||
mod model_format;
|
||||
mod multistatic_bridge;
|
||||
pub mod pose;
|
||||
@@ -406,6 +407,24 @@ struct PersonDetection {
|
||||
keypoints: Vec<PoseKeypoint>,
|
||||
bbox: BoundingBox,
|
||||
zone: String,
|
||||
/// Room-world position `[x, y, z]` (Observatory scene units / meters),
|
||||
/// derived from the strongest `signal_field` peak this person sits on
|
||||
/// (issue #1050). `y` is `0.0` — the field is a floor-plane grid. This is
|
||||
/// a real field-peak readout, not calibrated triangulation; see
|
||||
/// `field_localize` for the honesty caveat. Defaults to `[0,0,0]` until
|
||||
/// field positions are attached by `attach_field_positions`.
|
||||
#[serde(default)]
|
||||
position: [f64; 3],
|
||||
/// Motion magnitude on the Observatory's `0..100` scale, passed through
|
||||
/// from the measured `motion_band_power` (issue #1050).
|
||||
#[serde(default)]
|
||||
motion_score: f64,
|
||||
/// Coarse posture label (`"standing"`/`"lying"`/…) when a **real** aggregate
|
||||
/// posture estimate exists, else `None`. Never fabricated — per-person
|
||||
/// skeletal pose in room coordinates remains gated on the pose model
|
||||
/// (ADR-079). The Observatory defaults to `'standing'` when this is absent.
|
||||
#[serde(skip_serializing_if = "Option::is_none")]
|
||||
pose: Option<String>,
|
||||
}
|
||||
|
||||
#[derive(Debug, Clone, Serialize, Deserialize)]
|
||||
@@ -2572,6 +2591,8 @@ async fn windows_wifi_task(state: SharedState, tick_ms: u64) {
|
||||
if !tracked.is_empty() {
|
||||
update.persons = Some(tracked);
|
||||
}
|
||||
// #1050: attach real signal_field-peak positions to each person.
|
||||
attach_field_positions(&mut update);
|
||||
|
||||
if let Ok(json) = serde_json::to_string(&update) {
|
||||
let _ = s.tx.send(json);
|
||||
@@ -2725,6 +2746,8 @@ async fn windows_wifi_fallback_tick(state: &SharedState, seq: u32) {
|
||||
if !tracked.is_empty() {
|
||||
update.persons = Some(tracked);
|
||||
}
|
||||
// #1050: attach real signal_field-peak positions to each person.
|
||||
attach_field_positions(&mut update);
|
||||
|
||||
if let Ok(json) = serde_json::to_string(&update) {
|
||||
let _ = s.tx.send(json);
|
||||
@@ -3163,12 +3186,21 @@ async fn handle_ws_pose_client(mut socket: WebSocket, state: SharedState) {
|
||||
x: kp[0], y: kp[1], z: kp[2], confidence: kp[3],
|
||||
})
|
||||
.collect();
|
||||
let [nx, _ny, nz] = sensing.signal_field.grid_size;
|
||||
let peak = field_localize::extract_peaks(
|
||||
&sensing.signal_field.values, nx, nz, 1, 3.0,
|
||||
).into_iter().next();
|
||||
vec![PersonDetection {
|
||||
id: 1,
|
||||
confidence: sensing.classification.confidence,
|
||||
bbox: BoundingBox { x: 260.0, y: 150.0, width: 120.0, height: 220.0 },
|
||||
keypoints,
|
||||
zone: "zone_1".into(),
|
||||
position: peak.map_or([0.0, 0.0, 0.0], |p| p.position),
|
||||
motion_score: field_localize::motion_score_from_power(
|
||||
sensing.features.motion_band_power,
|
||||
),
|
||||
pose: sensing.posture.clone(),
|
||||
}]
|
||||
}).unwrap_or_else(|| {
|
||||
// Prefer tracked persons from broadcast if available
|
||||
@@ -3947,6 +3979,53 @@ fn derive_single_person_pose(
|
||||
height: (max_y - min_y).max(160.0),
|
||||
},
|
||||
zone: format!("zone_{}", person_idx + 1),
|
||||
// Position/motion_score/pose are attached from the real signal_field
|
||||
// peaks by `attach_field_positions` after the tracker step (#1050);
|
||||
// default here so the synthetic-skeleton geometry stays unchanged.
|
||||
position: [0.0, 0.0, 0.0],
|
||||
motion_score: 0.0,
|
||||
pose: None,
|
||||
}
|
||||
}
|
||||
|
||||
/// Attach real, field-derived per-person world positions to a `SensingUpdate`'s
|
||||
/// `persons` (issue #1050).
|
||||
///
|
||||
/// For each detected person we read a strongest-peak position out of the frame's
|
||||
/// real `signal_field` (the same grid the Observatory already renders) and map
|
||||
/// it to room-world coordinates via `field_localize::cell_to_world`. `motion_score`
|
||||
/// is passed through from the measured `motion_band_power`; `pose` is taken from
|
||||
/// the real aggregate `posture` estimate when present, else left `None` (never
|
||||
/// fabricated). Persons beyond the number of resolvable field peaks fall back to
|
||||
/// the strongest peak so they remain co-located with real energy rather than at
|
||||
/// a fake origin; if the field has no peak above threshold the position stays at
|
||||
/// `[0,0,0]` and `motion_score` still reflects real motion power.
|
||||
fn attach_field_positions(update: &mut SensingUpdate) {
|
||||
let Some(persons) = update.persons.as_mut() else {
|
||||
return;
|
||||
};
|
||||
if persons.is_empty() {
|
||||
return;
|
||||
}
|
||||
|
||||
let [nx, _ny, nz] = update.signal_field.grid_size;
|
||||
let peaks = field_localize::extract_peaks(
|
||||
&update.signal_field.values,
|
||||
nx,
|
||||
nz,
|
||||
persons.len().max(1),
|
||||
3.0,
|
||||
);
|
||||
|
||||
let motion_score = field_localize::motion_score_from_power(update.features.motion_band_power);
|
||||
let pose_label = update.posture.clone();
|
||||
|
||||
for (i, person) in persons.iter_mut().enumerate() {
|
||||
if let Some(peak) = peaks.get(i).or_else(|| peaks.first()) {
|
||||
person.position = peak.position;
|
||||
}
|
||||
person.motion_score = motion_score;
|
||||
person.pose = pose_label.clone();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -5473,6 +5552,8 @@ async fn udp_receiver_task(state: SharedState, udp_port: u16) {
|
||||
if !tracked.is_empty() {
|
||||
update.persons = Some(tracked);
|
||||
}
|
||||
// #1050: attach real signal_field-peak positions to each person.
|
||||
attach_field_positions(&mut update);
|
||||
|
||||
if let Ok(json) = serde_json::to_string(&update) {
|
||||
let _ = s.tx.send(json);
|
||||
@@ -5903,6 +5984,8 @@ async fn udp_receiver_task(state: SharedState, udp_port: u16) {
|
||||
if !tracked.is_empty() {
|
||||
update.persons = Some(tracked);
|
||||
}
|
||||
// #1050: attach real signal_field-peak positions to each person.
|
||||
attach_field_positions(&mut update);
|
||||
|
||||
if let Ok(json) = serde_json::to_string(&update) {
|
||||
let _ = s.tx.send(json);
|
||||
@@ -6076,6 +6159,8 @@ async fn simulated_data_task(state: SharedState, tick_ms: u64) {
|
||||
if !tracked.is_empty() {
|
||||
update.persons = Some(tracked);
|
||||
}
|
||||
// #1050: attach real signal_field-peak positions to each person.
|
||||
attach_field_positions(&mut update);
|
||||
|
||||
if update.classification.presence {
|
||||
s.total_detections += 1;
|
||||
@@ -8220,3 +8305,171 @@ mod export_rvf_mode_tests {
|
||||
assert!(!export_emits_placeholder_demo(false, true, false));
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod observatory_persons_field_position_tests {
|
||||
//! Issue #1050 — the Observatory 3D figure animates from per-person
|
||||
//! `position` / `motion_score` / `pose` carried on `sensing_update.persons`.
|
||||
//!
|
||||
//! These tests pin the public WS contract: a frame that detects a person on
|
||||
//! a known signal_field peak must emit a `persons` array whose first entry
|
||||
//! carries a `position` derived from that peak (matching the Observatory's
|
||||
//! cell→world transform), a real `motion_score`, and a serialized frame
|
||||
//! that round-trips. An empty / no-presence field must emit `persons: []`
|
||||
//! (or no person), never a phantom person at a fabricated origin.
|
||||
|
||||
use super::*;
|
||||
|
||||
/// Build a 20×20 signal_field that is background everywhere except a single
|
||||
/// strong normalized peak at grid cell `(ix, iz)`.
|
||||
fn field_with_peak(ix: usize, iz: usize) -> SignalField {
|
||||
let nx = 20usize;
|
||||
let nz = 20usize;
|
||||
let mut values = vec![0.05f64; nx * nz];
|
||||
values[iz * nx + ix] = 1.0;
|
||||
SignalField {
|
||||
grid_size: [nx, 1, nz],
|
||||
values,
|
||||
}
|
||||
}
|
||||
|
||||
/// Build an all-background (below-threshold) 20×20 field — no localizable
|
||||
/// hotspot, modelling an empty / no-presence room.
|
||||
fn empty_field() -> SignalField {
|
||||
SignalField {
|
||||
grid_size: [20, 1, 20],
|
||||
values: vec![0.05f64; 20 * 20],
|
||||
}
|
||||
}
|
||||
|
||||
fn base_update(signal_field: SignalField, presence: bool, motion_band_power: f64) -> SensingUpdate {
|
||||
SensingUpdate {
|
||||
msg_type: "sensing_update".to_string(),
|
||||
timestamp: 1.0,
|
||||
source: "test".to_string(),
|
||||
tick: 1,
|
||||
nodes: vec![],
|
||||
features: FeatureInfo {
|
||||
mean_rssi: -60.0,
|
||||
variance: 48.6,
|
||||
motion_band_power,
|
||||
breathing_band_power: 0.0,
|
||||
dominant_freq_hz: 1.0,
|
||||
change_points: 0,
|
||||
spectral_power: 0.0,
|
||||
},
|
||||
classification: ClassificationInfo {
|
||||
motion_level: if presence { "present_moving".to_string() } else { "absent".to_string() },
|
||||
presence,
|
||||
confidence: 0.8,
|
||||
},
|
||||
signal_field,
|
||||
vital_signs: None,
|
||||
enhanced_motion: None,
|
||||
enhanced_breathing: None,
|
||||
posture: None,
|
||||
signal_quality_score: None,
|
||||
quality_verdict: None,
|
||||
bssid_count: None,
|
||||
pose_keypoints: None,
|
||||
model_status: None,
|
||||
persons: None,
|
||||
estimated_persons: Some(1),
|
||||
node_features: None,
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn sensing_update_emits_persons_with_field_derived_position() {
|
||||
// Person present, motion energy 63.3, a hotspot at cell (15, 4).
|
||||
let peak_ix = 15;
|
||||
let peak_iz = 4;
|
||||
let mut update = base_update(field_with_peak(peak_ix, peak_iz), true, 63.3);
|
||||
|
||||
// Pipeline order: derive raw skeleton, then attach real field positions.
|
||||
update.persons = Some(derive_pose_from_sensing(&update));
|
||||
attach_field_positions(&mut update);
|
||||
|
||||
let persons = update.persons.as_ref().expect("persons should be Some");
|
||||
assert!(!persons.is_empty(), "a present person must be emitted");
|
||||
|
||||
// Position must match the Observatory cell→world transform for (15, 4):
|
||||
// x = (15-10)*0.6 = 3.0 ; z = (4-10)*0.5 = -3.0 ; y = 0.
|
||||
let p0 = &persons[0];
|
||||
assert!((p0.position[0] - 3.0).abs() < 1e-6, "x={}", p0.position[0]);
|
||||
assert!((p0.position[1] - 0.0).abs() < 1e-9);
|
||||
assert!((p0.position[2] - (-3.0)).abs() < 1e-6, "z={}", p0.position[2]);
|
||||
|
||||
// motion_score is the measured motion_band_power passed through (≤100).
|
||||
assert!((p0.motion_score - 63.3).abs() < 1e-6, "motion_score={}", p0.motion_score);
|
||||
|
||||
// The serialized WS frame must carry the new fields by their exact
|
||||
// contract names the Observatory UI reads.
|
||||
let v = serde_json::to_value(&update).unwrap();
|
||||
let arr = v["persons"].as_array().expect("persons must be a JSON array");
|
||||
assert_eq!(arr.len(), persons.len());
|
||||
let pj = &arr[0];
|
||||
assert!(pj.get("position").is_some(), "person.position missing from WS frame");
|
||||
assert!(pj.get("motion_score").is_some(), "person.motion_score missing from WS frame");
|
||||
assert!((pj["position"][0].as_f64().unwrap() - 3.0).abs() < 1e-6);
|
||||
assert!((pj["position"][2].as_f64().unwrap() - (-3.0)).abs() < 1e-6);
|
||||
assert!((pj["motion_score"].as_f64().unwrap() - 63.3).abs() < 1e-6);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn pose_is_real_when_posture_present_and_absent_otherwise() {
|
||||
// No aggregate posture estimate → pose is None (never fabricated).
|
||||
let mut no_posture = base_update(field_with_peak(10, 10), true, 40.0);
|
||||
no_posture.persons = Some(derive_pose_from_sensing(&no_posture));
|
||||
attach_field_positions(&mut no_posture);
|
||||
let p = &no_posture.persons.as_ref().unwrap()[0];
|
||||
assert!(p.pose.is_none(), "pose must stay None when no real posture exists");
|
||||
// skip_serializing_if drops the key entirely (UI defaults to 'standing').
|
||||
let v = serde_json::to_value(&no_posture).unwrap();
|
||||
assert!(v["persons"][0].get("pose").is_none());
|
||||
|
||||
// Real aggregate posture present → pose is carried through verbatim.
|
||||
let mut with_posture = base_update(field_with_peak(10, 10), true, 40.0);
|
||||
with_posture.posture = Some("lying".to_string());
|
||||
with_posture.persons = Some(derive_pose_from_sensing(&with_posture));
|
||||
attach_field_positions(&mut with_posture);
|
||||
let p2 = &with_posture.persons.as_ref().unwrap()[0];
|
||||
assert_eq!(p2.pose.as_deref(), Some("lying"));
|
||||
let v2 = serde_json::to_value(&with_posture).unwrap();
|
||||
assert_eq!(v2["persons"][0]["pose"], "lying");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn empty_room_yields_no_phantom_person() {
|
||||
// No presence → derive_pose_from_sensing returns no persons at all.
|
||||
let mut update = base_update(empty_field(), false, 2.0);
|
||||
update.persons = Some(derive_pose_from_sensing(&update));
|
||||
attach_field_positions(&mut update);
|
||||
|
||||
let persons = update.persons.as_ref().unwrap();
|
||||
assert!(
|
||||
persons.is_empty(),
|
||||
"no-presence frame must not emit a phantom person, got {} persons",
|
||||
persons.len()
|
||||
);
|
||||
|
||||
// And in the serialized frame the array is empty (no fake origin person).
|
||||
let v = serde_json::to_value(&update).unwrap();
|
||||
assert_eq!(v["persons"].as_array().unwrap().len(), 0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn present_but_below_threshold_field_keeps_position_at_origin_not_fabricated() {
|
||||
// Presence is true but the field has no peak above PEAK_THRESHOLD — we
|
||||
// must NOT invent a position; it stays at the [0,0,0] default while
|
||||
// motion_score still reflects the real measured motion power. This is
|
||||
// the honest degenerate case (no localizable hotspot to report).
|
||||
let mut update = base_update(empty_field(), true, 55.0);
|
||||
update.persons = Some(derive_pose_from_sensing(&update));
|
||||
attach_field_positions(&mut update);
|
||||
|
||||
let p = &update.persons.as_ref().unwrap()[0];
|
||||
assert_eq!(p.position, [0.0, 0.0, 0.0], "no peak → default origin, not fabricated coords");
|
||||
assert!((p.motion_score - 55.0).abs() < 1e-6, "motion_score stays real");
|
||||
}
|
||||
}
|
||||
|
||||
@@ -192,6 +192,11 @@ pub fn derive_single_person_pose(
|
||||
height: (max_y - min_y).max(160.0),
|
||||
},
|
||||
zone: format!("zone_{}", person_idx + 1),
|
||||
// Field-derived fields (#1050) — defaulted here; the live `/ws/sensing`
|
||||
// path attaches real positions via `attach_field_positions`.
|
||||
position: [0.0, 0.0, 0.0],
|
||||
motion_score: 0.0,
|
||||
pose: None,
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -176,6 +176,13 @@ pub fn tracker_to_person_detections(tracker: &PoseTracker) -> Vec<PersonDetectio
|
||||
keypoints,
|
||||
bbox,
|
||||
zone: "tracked".to_string(),
|
||||
// Field-derived position/motion_score/pose are (re)attached from
|
||||
// the live signal_field by `attach_field_positions` after this
|
||||
// tracker step (#1050); the Kalman tracker smooths keypoints only,
|
||||
// so we default here and let the field readout fill them in.
|
||||
position: [0.0, 0.0, 0.0],
|
||||
motion_score: 0.0,
|
||||
pose: None,
|
||||
}
|
||||
})
|
||||
.collect()
|
||||
@@ -329,6 +336,9 @@ mod tests {
|
||||
height: 1.0,
|
||||
},
|
||||
zone: "test".to_string(),
|
||||
position: [0.0, 0.0, 0.0],
|
||||
motion_score: 0.0,
|
||||
pose: None,
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -203,6 +203,21 @@ pub struct PersonDetection {
|
||||
pub keypoints: Vec<PoseKeypoint>,
|
||||
pub bbox: BoundingBox,
|
||||
pub zone: String,
|
||||
/// Room-world position `[x, y, z]` (Observatory scene units / meters),
|
||||
/// derived from the strongest `signal_field` peak (issue #1050). `y` is
|
||||
/// `0.0` — the field is a floor-plane grid. Real field-peak readout, not
|
||||
/// calibrated triangulation. Defaults to `[0,0,0]`.
|
||||
#[serde(default)]
|
||||
pub position: [f64; 3],
|
||||
/// Motion magnitude on the Observatory's `0..100` scale, passed through
|
||||
/// from the measured `motion_band_power` (issue #1050).
|
||||
#[serde(default)]
|
||||
pub motion_score: f64,
|
||||
/// Coarse posture label when a real aggregate posture estimate exists,
|
||||
/// else `None`. Never fabricated; per-person skeletal pose remains gated
|
||||
/// on the pose model (ADR-079).
|
||||
#[serde(skip_serializing_if = "Option::is_none")]
|
||||
pub pose: Option<String>,
|
||||
}
|
||||
|
||||
#[derive(Debug, Clone, Serialize, Deserialize)]
|
||||
|
||||
Reference in New Issue
Block a user