Skip to main content

qualia_vision/biosense/liveness/
rigid_head_pose.rs

1//! Rigid head pose from a stable landmark subset (PnP-class geometry).
2//!
3//! Maps nose tip, chin, and outer eye corners to a generic 3D canonical face
4//! under weak-perspective alignment, then extracts pitch / yaw / roll in
5//! degrees. Coordinates are scale-normalized by interocular distance so
6//! camera distance does not look like structural change.
7//!
8//! Pure Rust — no OpenCV product ABI. Full EPnP is not required for PAD
9//! thresholds; this solve is the geometric lock for challenge validation.
10
11use super::landmark_types::{Landmark2, LandmarkFrame, PadLandmarkId};
12
13/// Generic 3D face model (mm-scale), camera looking −Z, Y down.
14const MODEL_NOSE: [f32; 3] = [0.0, 0.0, 0.0];
15const MODEL_CHIN: [f32; 3] = [0.0, 63.0, -12.0];
16const MODEL_LEFT_EYE: [f32; 3] = [-35.0, -32.0, -26.0];
17const MODEL_RIGHT_EYE: [f32; 3] = [35.0, -32.0, -26.0];
18
19#[derive(Debug, Clone, Copy, Default, PartialEq)]
20pub struct HeadPose {
21    pub pitch_deg: f32,
22    pub yaw_deg: f32,
23    pub roll_deg: f32,
24    /// Interocular distance used for scale (pixels or same units as landmarks).
25    pub scale: f32,
26}
27
28/// Estimate rigid head pose from pose-core landmarks.
29///
30/// Returns `None` if the pose core is incomplete or degenerate.
31pub fn estimate_head_pose(frame: &LandmarkFrame) -> Option<HeadPose> {
32    let nose = frame.get(PadLandmarkId::NoseTip)?;
33    let chin = frame.get(PadLandmarkId::Chin)?;
34    let le = frame.get(PadLandmarkId::LeftEyeOuter)?;
35    let re = frame.get(PadLandmarkId::RightEyeOuter)?;
36    let iod = le.dist(re);
37    if iod < 1e-4 {
38        return None;
39    }
40
41    // Weak-perspective: center 2D/3D, scale 2D by IOD and 3D by model IOD.
42    let eye_c = le.midpoint(re);
43    let model_eye_c = [
44        0.5 * (MODEL_LEFT_EYE[0] + MODEL_RIGHT_EYE[0]),
45        0.5 * (MODEL_LEFT_EYE[1] + MODEL_RIGHT_EYE[1]),
46        0.5 * (MODEL_LEFT_EYE[2] + MODEL_RIGHT_EYE[2]),
47    ];
48    let model_iod = {
49        let dx = MODEL_LEFT_EYE[0] - MODEL_RIGHT_EYE[0];
50        let dy = MODEL_LEFT_EYE[1] - MODEL_RIGHT_EYE[1];
51        let dz = MODEL_LEFT_EYE[2] - MODEL_RIGHT_EYE[2];
52        (dx * dx + dy * dy + dz * dz).sqrt()
53    };
54    if model_iod < 1e-4 {
55        return None;
56    }
57
58    // Geometric euler proxies (normalized, distance-invariant).
59    // Yaw: horizontal nose offset from eye midline relative to IOD.
60    // Positive yaw = subject looking left (nose shifts toward image-right when facing camera).
61    let yaw_n = (nose.x - eye_c.x) / iod;
62    // Pitch: nose vs eye-chin vertical structure.
63    let face_h = (chin.y - eye_c.y).abs().max(1e-4);
64    let pitch_n = ((nose.y - eye_c.y) / face_h) - 0.35; // frontal bias ≈ 0
65                                                        // Roll: eye-line angle.
66    let roll_rad = (re.y - le.y).atan2(re.x - le.x);
67
68    // Scale to degrees with stable gains (production-calibrate in MANIFEST).
69    let mut pose = HeadPose {
70        pitch_deg: (pitch_n * 55.0).clamp(-90.0, 90.0),
71        yaw_deg: (yaw_n * 70.0).clamp(-90.0, 90.0),
72        roll_deg: roll_rad.to_degrees().clamp(-90.0, 90.0),
73        scale: iod,
74    };
75
76    // Refine with weak-perspective residual on canonical model (depth cue).
77    if let Some(refined) = refine_with_model(nose, chin, le, re, iod, model_eye_c, model_iod) {
78        // Blend geometric (stable) with model residual (non-rigid sensitive).
79        pose.yaw_deg = 0.65 * pose.yaw_deg + 0.35 * refined.yaw_deg;
80        pose.pitch_deg = 0.65 * pose.pitch_deg + 0.35 * refined.pitch_deg;
81    }
82
83    Some(pose)
84}
85
86fn refine_with_model(
87    nose: Landmark2,
88    chin: Landmark2,
89    le: Landmark2,
90    re: Landmark2,
91    iod: f32,
92    model_eye_c: [f32; 3],
93    model_iod: f32,
94) -> Option<HeadPose> {
95    // Project model XY after centering; residual of nose vs expected frontal
96    // projects to yaw/pitch corrections.
97    let s = iod / model_iod;
98    let eye_c = le.midpoint(re);
99    let pred_nose = Landmark2 {
100        x: eye_c.x + s * (MODEL_NOSE[0] - model_eye_c[0]),
101        y: eye_c.y + s * (MODEL_NOSE[1] - model_eye_c[1]),
102    };
103    let pred_chin = Landmark2 {
104        x: eye_c.x + s * (MODEL_CHIN[0] - model_eye_c[0]),
105        y: eye_c.y + s * (MODEL_CHIN[1] - model_eye_c[1]),
106    };
107    let dn = Landmark2 {
108        x: (nose.x - pred_nose.x) / iod,
109        y: (nose.y - pred_nose.y) / iod,
110    };
111    let dc = Landmark2 {
112        x: (chin.x - pred_chin.x) / iod,
113        y: (chin.y - pred_chin.y) / iod,
114    };
115    // Average residual → angle proxies.
116    Some(HeadPose {
117        yaw_deg: ((dn.x + 0.5 * dc.x) * 80.0).clamp(-90.0, 90.0),
118        pitch_deg: ((dn.y + 0.5 * dc.y) * 70.0).clamp(-90.0, 90.0),
119        roll_deg: 0.0,
120        scale: iod,
121    })
122}
123
124/// Normalize pose deltas so absolute camera distance does not dominate.
125pub fn pose_delta(a: HeadPose, b: HeadPose) -> HeadPose {
126    HeadPose {
127        pitch_deg: b.pitch_deg - a.pitch_deg,
128        yaw_deg: b.yaw_deg - a.yaw_deg,
129        roll_deg: b.roll_deg - a.roll_deg,
130        scale: b.scale,
131    }
132}
133
134#[cfg(test)]
135mod tests {
136    use super::*;
137    use crate::biosense::liveness::landmark_types::{Landmark2, LandmarkFrame, PadLandmarkId};
138
139    fn frontal() -> LandmarkFrame {
140        let mut f = LandmarkFrame::empty(0);
141        f.set(PadLandmarkId::LeftEyeOuter, Landmark2::new(100.0, 120.0));
142        f.set(PadLandmarkId::RightEyeOuter, Landmark2::new(180.0, 120.0));
143        f.set(PadLandmarkId::NoseTip, Landmark2::new(140.0, 150.0));
144        f.set(PadLandmarkId::Chin, Landmark2::new(140.0, 210.0));
145        f
146    }
147
148    #[test]
149    fn frontal_near_zero_yaw() {
150        let p = estimate_head_pose(&frontal()).unwrap();
151        assert!(p.yaw_deg.abs() < 8.0, "yaw={}", p.yaw_deg);
152        assert!(p.scale > 0.0);
153    }
154
155    #[test]
156    fn left_yaw_positive() {
157        let mut f = frontal();
158        // Subject looks left → nose toward image-right (facing camera).
159        f.set(PadLandmarkId::NoseTip, Landmark2::new(170.0, 150.0));
160        let p = estimate_head_pose(&f).unwrap();
161        assert!(
162            p.yaw_deg > 5.0,
163            "expected positive left yaw, got {}",
164            p.yaw_deg
165        );
166    }
167}