Skip to main content

qualia_vision/biosense/pose/
pack_pose_landmarks.rs

1//! Pack MediaPipe Pose Landmarker xy (ignore z) into fixed landmark slots.
2
3use crate::cv::error::CvError;
4
5/// MediaPipe pose landmarker lite: 33 landmarks.
6pub const MAX_POSE_LANDMARKS: usize = 33;
7
8#[derive(Debug, Clone, Copy, Default, PartialEq)]
9pub struct PoseLandmark {
10    pub x: f32,
11    pub y: f32,
12    pub visibility: f32,
13}
14
15/// `xy` length ≥ count*2 (or *3 if has_z — z discarded). `vis` optional per-landmark.
16pub fn pack_pose_landmarks_xy(
17    xy: &[f32],
18    has_z: bool,
19    vis: Option<&[f32]>,
20    count: usize,
21    out: &mut [PoseLandmark],
22) -> Result<usize, CvError> {
23    let n = count.min(MAX_POSE_LANDMARKS).min(out.len());
24    let stride = if has_z { 3 } else { 2 };
25    if xy.len() < n * stride {
26        return Err(CvError::BufferTooSmall);
27    }
28    for i in 0..n {
29        let base = i * stride;
30        out[i] = PoseLandmark {
31            x: xy[base],
32            y: xy[base + 1],
33            visibility: vis.map(|v| v.get(i).copied().unwrap_or(1.0)).unwrap_or(1.0),
34        };
35    }
36    Ok(n)
37}
38
39#[cfg(test)]
40mod tests {
41    use super::*;
42
43    #[test]
44    fn packs_xy_ignores_z() {
45        let mut xy = [0.0f32; 33 * 3];
46        xy[0] = 0.5;
47        xy[1] = 0.25;
48        xy[2] = 9.9; // z ignored
49        let mut out = [PoseLandmark::default(); MAX_POSE_LANDMARKS];
50        let n = pack_pose_landmarks_xy(&xy, true, None, 33, &mut out).unwrap();
51        assert_eq!(n, 33);
52        assert!((out[0].x - 0.5).abs() < 1e-6);
53        assert!((out[0].y - 0.25).abs() < 1e-6);
54    }
55}