510 lines
18 KiB
Rust
510 lines
18 KiB
Rust
//! User-teachable gesture recognition via DTW template learning.
|
|
//!
|
|
//! ADR-041 adaptive learning module — Event IDs 730-733.
|
|
//!
|
|
//! Allows users to teach the system new gestures by performing them three times.
|
|
//! The learning protocol:
|
|
//! 1. Enter learning mode: 3 seconds of stillness (motion < threshold)
|
|
//! 2. Perform gesture: record phase trajectory during motion
|
|
//! 3. Return to stillness: trajectory captured
|
|
//! 4. Repeat 3x — if trajectories are similar (DTW distance < learn_threshold),
|
|
//! average them into a template and store it
|
|
//!
|
|
//! Recognition: DTW distance of incoming phase trajectory against all stored
|
|
//! templates. Best match emitted if distance < recognition threshold.
|
|
//!
|
|
//! Budget: H (heavy, < 10 ms) — DTW is O(n*m) but n=m=64, so 4096 ops.
|
|
|
|
use libm::fabsf;
|
|
|
|
/// Maximum phase samples per gesture template.
|
|
const TEMPLATE_LEN: usize = 64;
|
|
|
|
/// Maximum stored gesture templates.
|
|
const MAX_TEMPLATES: usize = 16;
|
|
|
|
/// Number of rehearsals required before a template is committed.
|
|
const REHEARSALS_REQUIRED: usize = 3;
|
|
|
|
/// Stillness threshold (motion energy below this = still).
|
|
const STILLNESS_THRESHOLD: f32 = 0.05;
|
|
|
|
/// Number of consecutive still frames to trigger learning mode (3 s at 20 Hz).
|
|
const STILLNESS_FRAMES: u16 = 60;
|
|
|
|
/// DTW distance threshold for considering two rehearsals "similar".
|
|
const LEARN_DTW_THRESHOLD: f32 = 3.0;
|
|
|
|
/// DTW distance threshold for recognizing a stored gesture.
|
|
const RECOGNIZE_DTW_THRESHOLD: f32 = 2.5;
|
|
|
|
/// Cooldown frames after a gesture match (avoid double-fire, ~2 s at 20 Hz).
|
|
const MATCH_COOLDOWN: u16 = 40;
|
|
|
|
/// Sakoe-Chiba band width to constrain DTW warping.
|
|
const BAND_WIDTH: usize = 8;
|
|
|
|
// ── Event IDs (730-series: Adaptive Learning) ────────────────────────────────
|
|
|
|
pub const EVENT_GESTURE_LEARNED: i32 = 730;
|
|
pub const EVENT_GESTURE_MATCHED: i32 = 731;
|
|
pub const EVENT_MATCH_DISTANCE: i32 = 732;
|
|
pub const EVENT_TEMPLATE_COUNT: i32 = 733;
|
|
|
|
/// Learning state machine phases.
|
|
#[derive(Clone, Copy, Debug, PartialEq)]
|
|
enum LearnPhase {
|
|
/// Idle — waiting for stillness to begin learning.
|
|
Idle,
|
|
/// Counting consecutive stillness frames.
|
|
WaitingStill,
|
|
/// Recording motion trajectory.
|
|
Recording,
|
|
/// Motion ended — trajectory captured, waiting for next rehearsal or commit.
|
|
Captured,
|
|
}
|
|
|
|
/// A single gesture template: a fixed-length phase-delta trajectory.
|
|
#[derive(Clone, Copy)]
|
|
struct Template {
|
|
samples: [f32; TEMPLATE_LEN],
|
|
len: usize,
|
|
/// User-assigned gesture ID (starts at 100 to avoid colliding with built-in IDs).
|
|
id: u8,
|
|
}
|
|
|
|
impl Template {
|
|
const fn empty() -> Self {
|
|
Self {
|
|
samples: [0.0; TEMPLATE_LEN],
|
|
len: 0,
|
|
id: 0,
|
|
}
|
|
}
|
|
}
|
|
|
|
/// User-teachable gesture learner and recognizer.
|
|
pub struct GestureLearner {
|
|
// ── Stored templates ─────────────────────────────────────────────────
|
|
templates: [Template; MAX_TEMPLATES],
|
|
template_count: usize,
|
|
|
|
// ── Learning state ───────────────────────────────────────────────────
|
|
learn_phase: LearnPhase,
|
|
/// Consecutive stillness frame counter.
|
|
still_count: u16,
|
|
/// Rehearsal buffer: up to 3 captured trajectories.
|
|
rehearsals: [[f32; TEMPLATE_LEN]; REHEARSALS_REQUIRED],
|
|
rehearsal_lens: [usize; REHEARSALS_REQUIRED],
|
|
rehearsal_count: usize,
|
|
/// Current recording buffer.
|
|
recording: [f32; TEMPLATE_LEN],
|
|
recording_len: usize,
|
|
|
|
// ── Recognition state ────────────────────────────────────────────────
|
|
/// Phase delta sliding window for recognition.
|
|
window: [f32; TEMPLATE_LEN],
|
|
window_len: usize,
|
|
window_idx: usize,
|
|
prev_phase: f32,
|
|
phase_initialized: bool,
|
|
cooldown: u16,
|
|
|
|
/// Next ID to assign to a learned template.
|
|
next_id: u8,
|
|
}
|
|
|
|
impl GestureLearner {
|
|
pub const fn new() -> Self {
|
|
Self {
|
|
templates: [Template::empty(); MAX_TEMPLATES],
|
|
template_count: 0,
|
|
learn_phase: LearnPhase::Idle,
|
|
still_count: 0,
|
|
rehearsals: [[0.0; TEMPLATE_LEN]; REHEARSALS_REQUIRED],
|
|
rehearsal_lens: [0; REHEARSALS_REQUIRED],
|
|
rehearsal_count: 0,
|
|
recording: [0.0; TEMPLATE_LEN],
|
|
recording_len: 0,
|
|
window: [0.0; TEMPLATE_LEN],
|
|
window_len: 0,
|
|
window_idx: 0,
|
|
prev_phase: 0.0,
|
|
phase_initialized: false,
|
|
cooldown: 0,
|
|
next_id: 100,
|
|
}
|
|
}
|
|
|
|
/// Process one CSI frame.
|
|
///
|
|
/// `phases` — per-subcarrier phase values (uses first subcarrier).
|
|
/// `motion_energy` — aggregate motion metric from host (Tier 2).
|
|
///
|
|
/// Returns events as `(event_id, value)` pairs in a static buffer.
|
|
pub fn process_frame(&mut self, phases: &[f32], motion_energy: f32) -> &[(i32, f32)] {
|
|
static mut EVENTS: [(i32, f32); 4] = [(0, 0.0); 4];
|
|
let mut n_ev = 0usize;
|
|
|
|
if phases.is_empty() {
|
|
return &[];
|
|
}
|
|
|
|
// ── Compute phase delta ──────────────────────────────────────────
|
|
let primary = phases[0];
|
|
if !self.phase_initialized {
|
|
self.prev_phase = primary;
|
|
self.phase_initialized = true;
|
|
return &[];
|
|
}
|
|
let delta = primary - self.prev_phase;
|
|
self.prev_phase = primary;
|
|
|
|
// ── Push into recognition window ─────────────────────────────────
|
|
self.window[self.window_idx] = delta;
|
|
self.window_idx = (self.window_idx + 1) % TEMPLATE_LEN;
|
|
if self.window_len < TEMPLATE_LEN {
|
|
self.window_len += 1;
|
|
}
|
|
|
|
if self.cooldown > 0 {
|
|
self.cooldown -= 1;
|
|
}
|
|
|
|
// ── Learning state machine ───────────────────────────────────────
|
|
let is_still = motion_energy < STILLNESS_THRESHOLD;
|
|
|
|
match self.learn_phase {
|
|
LearnPhase::Idle => {
|
|
if is_still {
|
|
self.still_count += 1;
|
|
if self.still_count >= STILLNESS_FRAMES {
|
|
self.learn_phase = LearnPhase::WaitingStill;
|
|
self.rehearsal_count = 0;
|
|
}
|
|
} else {
|
|
self.still_count = 0;
|
|
}
|
|
}
|
|
LearnPhase::WaitingStill => {
|
|
if !is_still {
|
|
// Motion started — begin recording.
|
|
self.learn_phase = LearnPhase::Recording;
|
|
self.recording_len = 0;
|
|
self.recording[0] = delta;
|
|
self.recording_len = 1;
|
|
}
|
|
}
|
|
LearnPhase::Recording => {
|
|
if self.recording_len < TEMPLATE_LEN {
|
|
self.recording[self.recording_len] = delta;
|
|
self.recording_len += 1;
|
|
}
|
|
if is_still {
|
|
// Motion ended — capture this rehearsal.
|
|
self.learn_phase = LearnPhase::Captured;
|
|
}
|
|
}
|
|
LearnPhase::Captured => {
|
|
// Store captured trajectory as a rehearsal.
|
|
if self.rehearsal_count < REHEARSALS_REQUIRED && self.recording_len >= 4 {
|
|
let idx = self.rehearsal_count;
|
|
let len = self.recording_len;
|
|
self.rehearsal_lens[idx] = len;
|
|
let mut i = 0;
|
|
while i < len {
|
|
self.rehearsals[idx][i] = self.recording[i];
|
|
i += 1;
|
|
}
|
|
// Zero remainder.
|
|
while i < TEMPLATE_LEN {
|
|
self.rehearsals[idx][i] = 0.0;
|
|
i += 1;
|
|
}
|
|
self.rehearsal_count += 1;
|
|
}
|
|
|
|
if self.rehearsal_count >= REHEARSALS_REQUIRED {
|
|
// Check if all 3 rehearsals are mutually similar.
|
|
if self.rehearsals_are_similar() {
|
|
if let Some(id) = self.commit_template() {
|
|
unsafe {
|
|
EVENTS[n_ev] = (EVENT_GESTURE_LEARNED, id as f32);
|
|
n_ev += 1;
|
|
EVENTS[n_ev] = (EVENT_TEMPLATE_COUNT, self.template_count as f32);
|
|
n_ev += 1;
|
|
}
|
|
}
|
|
}
|
|
// Reset learning state regardless.
|
|
self.learn_phase = LearnPhase::Idle;
|
|
self.still_count = 0;
|
|
self.rehearsal_count = 0;
|
|
} else {
|
|
// Wait for next stillness -> motion cycle.
|
|
self.learn_phase = LearnPhase::WaitingStill;
|
|
}
|
|
}
|
|
}
|
|
|
|
// ── Recognition (only when not in active learning) ───────────────
|
|
if self.learn_phase == LearnPhase::Idle && self.cooldown == 0
|
|
&& self.template_count > 0 && self.window_len >= 8
|
|
{
|
|
// Build contiguous observation from ring buffer.
|
|
let mut obs = [0.0f32; TEMPLATE_LEN];
|
|
for i in 0..self.window_len {
|
|
let ri = (self.window_idx + TEMPLATE_LEN - self.window_len + i) % TEMPLATE_LEN;
|
|
obs[i] = self.window[ri];
|
|
}
|
|
|
|
let mut best_dist = RECOGNIZE_DTW_THRESHOLD;
|
|
let mut best_id: Option<u8> = None;
|
|
|
|
for t in 0..self.template_count {
|
|
let tmpl = &self.templates[t];
|
|
if tmpl.len == 0 || self.window_len < tmpl.len {
|
|
continue;
|
|
}
|
|
// Use tail of observation matching template length.
|
|
let start = if self.window_len > tmpl.len + 8 {
|
|
self.window_len - tmpl.len - 8
|
|
} else {
|
|
0
|
|
};
|
|
let dist = dtw_distance(
|
|
&obs[start..self.window_len],
|
|
&tmpl.samples[..tmpl.len],
|
|
);
|
|
if dist < best_dist {
|
|
best_dist = dist;
|
|
best_id = Some(tmpl.id);
|
|
}
|
|
}
|
|
|
|
if let Some(id) = best_id {
|
|
self.cooldown = MATCH_COOLDOWN;
|
|
unsafe {
|
|
EVENTS[n_ev] = (EVENT_GESTURE_MATCHED, id as f32);
|
|
n_ev += 1;
|
|
if n_ev < 4 {
|
|
EVENTS[n_ev] = (EVENT_MATCH_DISTANCE, best_dist);
|
|
n_ev += 1;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
unsafe { &EVENTS[..n_ev] }
|
|
}
|
|
|
|
/// Check if all rehearsals are pairwise similar (DTW distance < threshold).
|
|
fn rehearsals_are_similar(&self) -> bool {
|
|
for i in 0..self.rehearsal_count {
|
|
for j in (i + 1)..self.rehearsal_count {
|
|
let len_i = self.rehearsal_lens[i];
|
|
let len_j = self.rehearsal_lens[j];
|
|
if len_i < 4 || len_j < 4 {
|
|
return false;
|
|
}
|
|
let dist = dtw_distance(
|
|
&self.rehearsals[i][..len_i],
|
|
&self.rehearsals[j][..len_j],
|
|
);
|
|
if dist >= LEARN_DTW_THRESHOLD {
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
true
|
|
}
|
|
|
|
/// Average rehearsals into a new template and store it.
|
|
/// Returns the assigned gesture ID, or None if template slots are full.
|
|
fn commit_template(&mut self) -> Option<u8> {
|
|
if self.template_count >= MAX_TEMPLATES {
|
|
return None;
|
|
}
|
|
|
|
// Find the maximum trajectory length among rehearsals.
|
|
let mut max_len = 0usize;
|
|
for i in 0..self.rehearsal_count {
|
|
if self.rehearsal_lens[i] > max_len {
|
|
max_len = self.rehearsal_lens[i];
|
|
}
|
|
}
|
|
if max_len < 4 {
|
|
return None;
|
|
}
|
|
|
|
// Average the rehearsals sample-by-sample.
|
|
let mut avg = [0.0f32; TEMPLATE_LEN];
|
|
for s in 0..max_len {
|
|
let mut sum = 0.0f32;
|
|
let mut count = 0u8;
|
|
for r in 0..self.rehearsal_count {
|
|
if s < self.rehearsal_lens[r] {
|
|
sum += self.rehearsals[r][s];
|
|
count += 1;
|
|
}
|
|
}
|
|
if count > 0 {
|
|
avg[s] = sum / count as f32;
|
|
}
|
|
}
|
|
|
|
let id = self.next_id;
|
|
self.next_id = self.next_id.wrapping_add(1);
|
|
|
|
self.templates[self.template_count] = Template {
|
|
samples: avg,
|
|
len: max_len,
|
|
id,
|
|
};
|
|
self.template_count += 1;
|
|
|
|
Some(id)
|
|
}
|
|
|
|
/// Number of currently stored templates.
|
|
pub fn template_count(&self) -> usize {
|
|
self.template_count
|
|
}
|
|
}
|
|
|
|
/// Compute constrained DTW distance between two sequences.
|
|
///
|
|
/// Uses Sakoe-Chiba band to limit warping path. Result is normalized
|
|
/// by path length (n + m) to allow comparison across different lengths.
|
|
fn dtw_distance(a: &[f32], b: &[f32]) -> f32 {
|
|
let n = a.len();
|
|
let m = b.len();
|
|
|
|
if n == 0 || m == 0 {
|
|
return f32::MAX;
|
|
}
|
|
|
|
// Stack-allocated cost matrix: max 64x64 = 4096 cells.
|
|
let mut cost = [[f32::MAX; TEMPLATE_LEN]; TEMPLATE_LEN];
|
|
|
|
cost[0][0] = fabsf(a[0] - b[0]);
|
|
|
|
for i in 0..n {
|
|
for j in 0..m {
|
|
let diff = if i > j { i - j } else { j - i };
|
|
if diff > BAND_WIDTH {
|
|
continue;
|
|
}
|
|
|
|
let c = fabsf(a[i] - b[j]);
|
|
|
|
if i == 0 && j == 0 {
|
|
cost[i][j] = c;
|
|
} else {
|
|
let mut min_prev = f32::MAX;
|
|
if i > 0 && cost[i - 1][j] < min_prev {
|
|
min_prev = cost[i - 1][j];
|
|
}
|
|
if j > 0 && cost[i][j - 1] < min_prev {
|
|
min_prev = cost[i][j - 1];
|
|
}
|
|
if i > 0 && j > 0 && cost[i - 1][j - 1] < min_prev {
|
|
min_prev = cost[i - 1][j - 1];
|
|
}
|
|
cost[i][j] = c + min_prev;
|
|
}
|
|
}
|
|
}
|
|
|
|
let path_len = (n + m) as f32;
|
|
cost[n - 1][m - 1] / path_len
|
|
}
|
|
|
|
#[cfg(test)]
|
|
mod tests {
|
|
use super::*;
|
|
|
|
#[test]
|
|
fn test_new_state() {
|
|
let gl = GestureLearner::new();
|
|
assert_eq!(gl.template_count(), 0);
|
|
assert_eq!(gl.learn_phase, LearnPhase::Idle);
|
|
assert_eq!(gl.cooldown, 0);
|
|
}
|
|
|
|
#[test]
|
|
fn test_dtw_identical() {
|
|
let a = [0.1, 0.3, 0.5, 0.7, 0.5, 0.3, 0.1];
|
|
let b = [0.1, 0.3, 0.5, 0.7, 0.5, 0.3, 0.1];
|
|
let d = dtw_distance(&a, &b);
|
|
assert!(d < 0.001, "identical sequences should have near-zero DTW distance");
|
|
}
|
|
|
|
#[test]
|
|
fn test_dtw_different() {
|
|
let a = [0.1, 0.3, 0.5, 0.7, 0.5, 0.3, 0.1];
|
|
let b = [-0.5, -0.8, -1.0, -0.8, -0.5, -0.2, 0.0];
|
|
let d = dtw_distance(&a, &b);
|
|
assert!(d > 0.3, "different sequences should have large DTW distance");
|
|
}
|
|
|
|
#[test]
|
|
fn test_dtw_empty() {
|
|
let a: [f32; 0] = [];
|
|
let b = [1.0, 2.0];
|
|
assert_eq!(dtw_distance(&a, &b), f32::MAX);
|
|
}
|
|
|
|
#[test]
|
|
fn test_learning_protocol() {
|
|
let mut gl = GestureLearner::new();
|
|
let phase_still = [0.0f32; 8];
|
|
|
|
// Phase 1: Stillness for STILLNESS_FRAMES + 1 frames -> enter learning mode.
|
|
// (+1 because the very first call returns early to initialise phase tracking.)
|
|
for _ in 0..=STILLNESS_FRAMES {
|
|
gl.process_frame(&phase_still, 0.01);
|
|
}
|
|
assert_eq!(gl.learn_phase, LearnPhase::WaitingStill);
|
|
|
|
// Phase 2: Perform gesture 3 times (motion -> stillness).
|
|
let gesture_phases: [f32; 8] = [0.5, 0.3, 0.2, 0.1, 0.4, 0.6, 0.7, 0.8];
|
|
|
|
for rehearsal in 0..3 {
|
|
// Motion frames.
|
|
for frame in 0..10 {
|
|
let mut p = [0.0f32; 8];
|
|
p[0] = gesture_phases[frame % gesture_phases.len()] * (rehearsal as f32 + 1.0) * 0.1;
|
|
gl.process_frame(&p, 0.5);
|
|
}
|
|
// Stillness frame to capture.
|
|
let _ = gl.process_frame(&phase_still, 0.01);
|
|
if rehearsal == 2 {
|
|
// After 3rd rehearsal, should either learn (Idle) or
|
|
// still be in Captured if DTW distances were too different.
|
|
assert!(
|
|
gl.learn_phase == LearnPhase::Idle || gl.learn_phase == LearnPhase::Captured,
|
|
"unexpected phase: {:?}", gl.learn_phase
|
|
);
|
|
}
|
|
}
|
|
}
|
|
|
|
#[test]
|
|
fn test_template_capacity() {
|
|
let mut gl = GestureLearner::new();
|
|
// Manually fill templates to max.
|
|
for i in 0..MAX_TEMPLATES {
|
|
gl.templates[i] = Template {
|
|
samples: [0.1; TEMPLATE_LEN],
|
|
len: 10,
|
|
id: i as u8,
|
|
};
|
|
}
|
|
gl.template_count = MAX_TEMPLATES;
|
|
|
|
// Commit should return None when full.
|
|
assert!(gl.commit_template().is_none());
|
|
}
|
|
}
|