qualia_vision/biosense/liveness/
rigid_head_pose.rs1use super::landmark_types::{Landmark2, LandmarkFrame, PadLandmarkId};
12
13const 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 pub scale: f32,
26}
27
28pub 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 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 let yaw_n = (nose.x - eye_c.x) / iod;
62 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; let roll_rad = (re.y - le.y).atan2(re.x - le.x);
67
68 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 if let Some(refined) = refine_with_model(nose, chin, le, re, iod, model_eye_c, model_iod) {
78 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 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 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
124pub 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 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}