242 lines
9.4 KiB
Rust
242 lines
9.4 KiB
Rust
//! 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);
|
||
}
|
||
}
|