195 lines
7.9 KiB
Rust
195 lines
7.9 KiB
Rust
//! Skeleton derivation, pose estimation, and temporal smoothing.
|
|
|
|
use crate::types::*;
|
|
|
|
/// Expected bone lengths in pixel-space for the COCO-17 skeleton.
|
|
pub const POSE_BONE_PAIRS: &[(usize, usize)] = &[
|
|
(5, 7), (7, 9), (6, 8), (8, 10),
|
|
(5, 11), (6, 12),
|
|
(11, 13), (13, 15), (12, 14), (14, 16),
|
|
(5, 6), (11, 12),
|
|
];
|
|
|
|
const TORSO_KP: [usize; 4] = [5, 6, 11, 12];
|
|
const EXTREMITY_KP: [usize; 4] = [9, 10, 15, 16];
|
|
|
|
pub fn derive_single_person_pose(
|
|
update: &SensingUpdate, person_idx: usize, total_persons: usize,
|
|
) -> PersonDetection {
|
|
let cls = &update.classification;
|
|
let feat = &update.features;
|
|
|
|
let phase_offset = person_idx as f64 * 2.094;
|
|
let half = (total_persons as f64 - 1.0) / 2.0;
|
|
let person_x_offset = (person_idx as f64 - half) * 120.0;
|
|
let conf_decay = 1.0 - person_idx as f64 * 0.15;
|
|
|
|
let motion_score = (feat.motion_band_power / 15.0).clamp(0.0, 1.0);
|
|
let is_walking = motion_score > 0.55;
|
|
let breath_amp = (feat.breathing_band_power * 4.0).clamp(0.0, 12.0);
|
|
|
|
let breath_phase = if let Some(ref vs) = update.vital_signs {
|
|
let bpm = vs.breathing_rate_bpm.unwrap_or(15.0);
|
|
let freq = (bpm / 60.0).clamp(0.1, 0.5);
|
|
(update.tick as f64 * freq * 0.02 * std::f64::consts::TAU + phase_offset).sin()
|
|
} else {
|
|
(update.tick as f64 * 0.02 + phase_offset).sin()
|
|
};
|
|
|
|
let lean_x = (feat.dominant_freq_hz / 5.0 - 1.0).clamp(-1.0, 1.0) * 18.0;
|
|
let stride_x = if is_walking {
|
|
let stride_phase = (feat.motion_band_power * 0.7 + update.tick as f64 * 0.06 + phase_offset).sin();
|
|
stride_phase * 20.0 * motion_score
|
|
} else { 0.0 };
|
|
|
|
let burst = (feat.change_points as f64 / 20.0).clamp(0.0, 0.3);
|
|
let noise_seed = person_idx as f64 * 97.1;
|
|
let noise_val = (noise_seed.sin() * 43758.545).fract();
|
|
let snr_factor = ((feat.variance - 0.5) / 10.0).clamp(0.0, 1.0);
|
|
let base_confidence = cls.confidence * (0.6 + 0.4 * snr_factor) * conf_decay;
|
|
|
|
let base_x = 320.0 + stride_x + lean_x * 0.5 + person_x_offset;
|
|
let base_y = 240.0 - motion_score * 8.0;
|
|
|
|
let kp_names = [
|
|
"nose", "left_eye", "right_eye", "left_ear", "right_ear",
|
|
"left_shoulder", "right_shoulder", "left_elbow", "right_elbow",
|
|
"left_wrist", "right_wrist", "left_hip", "right_hip",
|
|
"left_knee", "right_knee", "left_ankle", "right_ankle",
|
|
];
|
|
|
|
let kp_offsets: [(f64, f64); 17] = [
|
|
(0.0, -80.0), (-8.0, -88.0), (8.0, -88.0), (-16.0, -82.0), (16.0, -82.0),
|
|
(-30.0, -50.0), (30.0, -50.0), (-45.0, -15.0), (45.0, -15.0),
|
|
(-50.0, 20.0), (50.0, 20.0), (-20.0, 20.0), (20.0, 20.0),
|
|
(-22.0, 70.0), (22.0, 70.0), (-24.0, 120.0), (24.0, 120.0),
|
|
];
|
|
|
|
let keypoints: Vec<PoseKeypoint> = kp_names.iter().zip(kp_offsets.iter())
|
|
.enumerate()
|
|
.map(|(i, (name, (dx, dy)))| {
|
|
let breath_dx = if TORSO_KP.contains(&i) {
|
|
let sign = if *dx < 0.0 { -1.0 } else { 1.0 };
|
|
sign * breath_amp * breath_phase * 0.5
|
|
} else { 0.0 };
|
|
let breath_dy = if TORSO_KP.contains(&i) {
|
|
let sign = if *dy < 0.0 { -1.0 } else { 1.0 };
|
|
sign * breath_amp * breath_phase * 0.3
|
|
} else { 0.0 };
|
|
|
|
let extremity_jitter = if EXTREMITY_KP.contains(&i) {
|
|
let phase = noise_seed + i as f64 * 2.399;
|
|
(phase.sin() * burst * motion_score * 4.0, (phase * 1.31).cos() * burst * motion_score * 3.0)
|
|
} else { (0.0, 0.0) };
|
|
|
|
let kp_noise_x = ((noise_seed + i as f64 * 1.618).sin() * 43758.545).fract()
|
|
* feat.variance.sqrt().clamp(0.0, 3.0) * motion_score;
|
|
let kp_noise_y = ((noise_seed + i as f64 * 2.718).cos() * 31415.926).fract()
|
|
* feat.variance.sqrt().clamp(0.0, 3.0) * motion_score * 0.6;
|
|
|
|
let swing_dy = if is_walking {
|
|
let stride_phase = (feat.motion_band_power * 0.7 + update.tick as f64 * 0.12 + phase_offset).sin();
|
|
match i {
|
|
7 | 9 => -stride_phase * 20.0 * motion_score,
|
|
8 | 10 => stride_phase * 20.0 * motion_score,
|
|
13 | 15 => stride_phase * 25.0 * motion_score,
|
|
14 | 16 => -stride_phase * 25.0 * motion_score,
|
|
_ => 0.0,
|
|
}
|
|
} else { 0.0 };
|
|
|
|
let final_x = base_x + dx + breath_dx + extremity_jitter.0 + kp_noise_x;
|
|
let final_y = base_y + dy + breath_dy + extremity_jitter.1 + kp_noise_y + swing_dy;
|
|
|
|
let kp_conf = if EXTREMITY_KP.contains(&i) {
|
|
base_confidence * (0.7 + 0.3 * snr_factor) * (0.85 + 0.15 * noise_val)
|
|
} else {
|
|
base_confidence * (0.88 + 0.12 * ((i as f64 * 0.7 + noise_seed).cos()))
|
|
};
|
|
|
|
PoseKeypoint { name: name.to_string(), x: final_x, y: final_y, z: lean_x * 0.02, confidence: kp_conf.clamp(0.1, 1.0) }
|
|
})
|
|
.collect();
|
|
|
|
let xs: Vec<f64> = keypoints.iter().map(|k| k.x).collect();
|
|
let ys: Vec<f64> = keypoints.iter().map(|k| k.y).collect();
|
|
let min_x = xs.iter().cloned().fold(f64::MAX, f64::min) - 10.0;
|
|
let min_y = ys.iter().cloned().fold(f64::MAX, f64::min) - 10.0;
|
|
let max_x = xs.iter().cloned().fold(f64::MIN, f64::max) + 10.0;
|
|
let max_y = ys.iter().cloned().fold(f64::MIN, f64::max) + 10.0;
|
|
|
|
PersonDetection {
|
|
id: (person_idx + 1) as u32,
|
|
confidence: cls.confidence * conf_decay,
|
|
keypoints,
|
|
bbox: BoundingBox { x: min_x, y: min_y, width: (max_x - min_x).max(80.0), height: (max_y - min_y).max(160.0) },
|
|
zone: format!("zone_{}", person_idx + 1),
|
|
}
|
|
}
|
|
|
|
pub fn derive_pose_from_sensing(update: &SensingUpdate) -> Vec<PersonDetection> {
|
|
let cls = &update.classification;
|
|
if !cls.presence { return vec![]; }
|
|
let person_count = update.estimated_persons.unwrap_or(1).max(1);
|
|
(0..person_count).map(|idx| derive_single_person_pose(update, idx, person_count)).collect()
|
|
}
|
|
|
|
/// Apply temporal EMA smoothing and bone-length clamping to person detections.
|
|
pub fn apply_temporal_smoothing(persons: &mut [PersonDetection], ns: &mut NodeState) {
|
|
if persons.is_empty() { return; }
|
|
|
|
let alpha = ns.ema_alpha();
|
|
let person = &mut persons[0];
|
|
|
|
let current_kps: Vec<[f64; 3]> = person.keypoints.iter()
|
|
.map(|kp| [kp.x, kp.y, kp.z]).collect();
|
|
|
|
let smoothed = if let Some(ref prev) = ns.prev_keypoints {
|
|
let mut out = Vec::with_capacity(current_kps.len());
|
|
for (cur, prv) in current_kps.iter().zip(prev.iter()) {
|
|
out.push([
|
|
alpha * cur[0] + (1.0 - alpha) * prv[0],
|
|
alpha * cur[1] + (1.0 - alpha) * prv[1],
|
|
alpha * cur[2] + (1.0 - alpha) * prv[2],
|
|
]);
|
|
}
|
|
clamp_bone_lengths_f64(&mut out, prev);
|
|
out
|
|
} else {
|
|
current_kps.clone()
|
|
};
|
|
|
|
for (kp, s) in person.keypoints.iter_mut().zip(smoothed.iter()) {
|
|
kp.x = s[0]; kp.y = s[1]; kp.z = s[2];
|
|
}
|
|
ns.prev_keypoints = Some(smoothed);
|
|
}
|
|
|
|
fn clamp_bone_lengths_f64(pose: &mut Vec<[f64; 3]>, prev: &[[f64; 3]]) {
|
|
for &(p, c) in POSE_BONE_PAIRS {
|
|
if p >= pose.len() || c >= pose.len() { continue; }
|
|
let prev_len = dist_f64(&prev[p], &prev[c]);
|
|
if prev_len < 1e-6 { continue; }
|
|
let cur_len = dist_f64(&pose[p], &pose[c]);
|
|
if cur_len < 1e-6 { continue; }
|
|
let ratio = cur_len / prev_len;
|
|
let lo = 1.0 - MAX_BONE_CHANGE_RATIO;
|
|
let hi = 1.0 + MAX_BONE_CHANGE_RATIO;
|
|
if ratio < lo || ratio > hi {
|
|
let target = prev_len * ratio.clamp(lo, hi);
|
|
let scale = target / cur_len;
|
|
for dim in 0..3 {
|
|
let diff = pose[c][dim] - pose[p][dim];
|
|
pose[c][dim] = pose[p][dim] + diff * scale;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
fn dist_f64(a: &[f64; 3], b: &[f64; 3]) -> f64 {
|
|
let dx = b[0] - a[0];
|
|
let dy = b[1] - a[1];
|
|
let dz = b[2] - a[2];
|
|
(dx * dx + dy * dy + dz * dz).sqrt()
|
|
}
|