import numpy as np from biohead.define import HeadJoints, FaceMap import math from scipy.optimize import fsolve from biohead.define import HeadJoints, FaceMap class LowPassFilter: def __init__(self, alpha=0.2): self.alpha = alpha self.last_val = None def __call__(self, val): if self.last_val is None: self.last_val = val else: self.last_val = self.alpha * val + (1 - self.alpha) * self.last_val return self.last_val lip_z_lpf_left = LowPassFilter(alpha=0.5) lip_z_lpf_right = LowPassFilter(alpha=0.5) def calc_eyebrow(uv, result: HeadJoints): try: # LEFT left_outer = np.array(uv[FaceMap.left_eyelid_outside]) left_inner = np.array(uv[FaceMap.left_eyelid_inside]) eye_width_left = np.linalg.norm(left_inner - left_outer) + 1e-6 dir_eye_left = (left_inner - left_outer) / eye_width_left perp_left = np.cross(np.array([0, 0, 1]), dir_eye_left) perp_left /= np.linalg.norm(perp_left) + 1e-6 mid_left = (left_outer + left_inner) / 2 delta_out_left = np.array(uv[FaceMap.left_eyebrow_outside]) - mid_left delta_in_left = np.array(uv[FaceMap.left_eyebrow_inside]) - mid_left result.left_eyebrow_outside_y = abs(np.dot(delta_out_left, perp_left)) / eye_width_left result.left_eyebrow_inside_y = abs(np.dot(delta_in_left, perp_left)) / eye_width_left # RIGHT right_outer = np.array(uv[FaceMap.right_eyelid_outside]) right_inner = np.array(uv[FaceMap.right_eyelid_inside]) eye_width_right = np.linalg.norm(right_inner - right_outer) + 1e-6 dir_eye_right = (right_inner - right_outer) / eye_width_right perp_right = np.cross(np.array([0, 0, 1]), dir_eye_right) perp_right /= np.linalg.norm(perp_right) + 1e-6 mid_right = (right_outer + right_inner) / 2 delta_out_right = np.array(uv[FaceMap.right_eyebrow_outside]) - mid_right delta_in_right = np.array(uv[FaceMap.right_eyebrow_inside]) - mid_right result.right_eyebrow_outside_y = abs(np.dot(delta_out_right, perp_right)) / eye_width_right result.right_eyebrow_inside_y = abs(np.dot(delta_in_right, perp_right)) / eye_width_right return result except KeyError as e: print("Eyebrow landmarks missing:", e) def calc_eyelid(uv, result: HeadJoints): try: # LEFT left_outer = np.array(uv[FaceMap.left_eyelid_outside]) left_inner = np.array(uv[FaceMap.left_eyelid_inside]) eye_width_left = np.linalg.norm(left_inner - left_outer) + 1e-6 dir_eye_left = (left_inner - left_outer) / eye_width_left perp_left = np.cross(np.array([0, 0, 1]), dir_eye_left) perp_left /= np.linalg.norm(perp_left) + 1e-6 mid_left = (left_outer + left_inner) / 2 delta_upper_left = np.array(uv[FaceMap.left_eyelid_upper]) - mid_left delta_lower_left = np.array(uv[FaceMap.left_eyelid_lower]) - mid_left result.left_eye_upper_lid_y = abs(np.dot(delta_upper_left, perp_left)) / eye_width_left result.left_eye_lower_lid_y = abs(np.dot(delta_lower_left, perp_left)) / eye_width_left # RIGHT right_outer = np.array(uv[FaceMap.right_eyelid_outside]) right_inner = np.array(uv[FaceMap.right_eyelid_inside]) eye_width_right = np.linalg.norm(right_inner - right_outer) + 1e-6 dir_eye_right = (right_inner - right_outer) / eye_width_right perp_right = np.cross(np.array([0, 0, 1]), dir_eye_right) perp_right /= np.linalg.norm(perp_right) + 1e-6 mid_right = (right_outer + right_inner) / 2 delta_upper_right = np.array(uv[FaceMap.right_eyelid_upper]) - mid_right delta_lower_right = np.array(uv[FaceMap.right_eyelid_lower]) - mid_right result.right_eye_upper_lid_y = abs(np.dot(delta_upper_right, perp_right)) / eye_width_right result.right_eye_lower_lid_y = abs(np.dot(delta_lower_right, perp_right)) / eye_width_right return result except KeyError as e: print("Eyelid landmarks missing:", e) def calc_eyeball(uv, result: HeadJoints): try: # LEFT left_outer = np.array(uv[FaceMap.left_eyelid_outside]) left_inner = np.array(uv[FaceMap.left_eyelid_inside]) eye_width_left = np.linalg.norm(left_inner - left_outer) + 1e-6 dir_eye_left = (left_inner - left_outer) / eye_width_left perp_left = np.cross(np.array([0, 0, 1]), dir_eye_left) perp_left /= np.linalg.norm(perp_left) + 1e-6 mid_left = (left_outer + left_inner) / 2 eyeball_center_left = np.array(uv[FaceMap.left_eyeball_center]) delta_left = eyeball_center_left - mid_left result.left_eye_ball_x = np.dot(delta_left, dir_eye_left) / eye_width_left result.left_eye_ball_y = np.dot(delta_left, perp_left) / eye_width_left # RIGHT right_outer = np.array(uv[FaceMap.right_eyelid_outside]) right_inner = np.array(uv[FaceMap.right_eyelid_inside]) eye_width_right = np.linalg.norm(right_inner - right_outer) + 1e-6 dir_eye_right = (right_inner - right_outer) / eye_width_right perp_right = np.cross(np.array([0, 0, 1]), dir_eye_right) perp_right /= np.linalg.norm(perp_right) + 1e-6 mid_right = (right_outer + right_inner) / 2 eyeball_center_right = np.array(uv[FaceMap.right_eyeball_center]) delta_right = eyeball_center_right - mid_right result.right_eye_ball_x = np.dot(delta_right, dir_eye_right) / eye_width_right result.right_eye_ball_y = np.dot(delta_right, perp_right) / eye_width_right return result except KeyError as e: print("Eyeball landmarks missing:", e) def _jaw_ik_eqs_3d(thetas, y_d, z_d, L1, L2): theta1, theta2 = thetas t1, t2 = math.radians(theta1), math.radians(theta2) y = L1 * math.cos(t1) + L2 * math.cos(t2) z = L1 * math.sin(t1) + L2 * math.sin(t2) return [ y - y_d, z - z_d ] def _solve_jaw_servo_angles(y_d, z_d, L1, L2, init_guess=(0.0, 0.0)): sol = fsolve(_jaw_ik_eqs_3d, init_guess, args=(y_d, z_d, L1, L2)) return sol[0], sol[1] def _normalize_angle(angle, min_angle=-100, max_angle=100): return max(0.0, min(1.0, (angle - min_angle) / (max_angle - min_angle))) def calc_mouth(uv, result: HeadJoints): try: left_uv = uv[FaceMap.left_head] right_uv = uv[FaceMap.right_head] top_uv = uv[FaceMap.top_head] bottom_uv = uv[FaceMap.bottom_head] head_width = abs(right_uv[0] - left_uv[0]) + 1e-6 head_height = abs(top_uv[1] - bottom_uv[1]) + 1e-6 # === 新增：计算头部平均深度用于 z 归一化 === head_depth = ( abs(left_uv[2]) + abs(right_uv[2]) + abs(top_uv[2]) + abs(bottom_uv[2]) + abs(uv[FaceMap.mid_up_lip][2]) ) / 5 + 1e-6 upper_center_u = (left_uv[0] + right_uv[0] + top_uv[0]) / 3 upper_center_v = (left_uv[1] + right_uv[1] + top_uv[1]) / 3 bottom_center_u = bottom_uv[0] bottom_center_v = bottom_uv[1] upper_mouth_points = { "upper_right_lip": FaceMap.right_upper_lip, "upper_left_lip": FaceMap.left_upper_lip, "right_corner_lip": FaceMap.right_mouth_corner, "left_corner_lip": FaceMap.left_mouth_corner, "upper_lip": FaceMap.mid_up_lip } jaw_y_accum = 0.0 jaw_z_accum = 0.0 jaw_count = 0 for name, idx in upper_mouth_points.items(): u, v, z = uv[idx] delta_u = u - upper_center_u delta_v = v - upper_center_v x_val = delta_u / head_width y_val = delta_v / head_height # === 滤波并归一化 z 值 === if name == "left_corner_lip": z = lip_z_lpf_left(z) elif name == "right_corner_lip": z = lip_z_lpf_right(z) z_val = z / head_depth setattr(result, f"{name}_x", x_val) setattr(result, f"{name}_y", y_val) setattr(result, f"{name}_z", z_val) if name in ("left_corner_lip", "right_corner_lip"): jaw_y_accum += y_val * head_height jaw_z_accum += z_val * head_height jaw_count += 1 # === IK 解算（仅嘴角） === if jaw_count > 0: jaw_displacement_y = jaw_y_accum / jaw_count jaw_displacement_z = jaw_z_accum / jaw_count else: jaw_displacement_y = 0.0 jaw_displacement_z = 0.0 L1, L2 = 145, 177 theta1, theta2 = _solve_jaw_servo_angles( y_d=jaw_displacement_y, z_d=jaw_displacement_z, L1=L1, L2=L2, init_guess=(0.0, 0.0) ) norm_theta1 = _normalize_angle(theta1) norm_theta2 = _normalize_angle(theta2) print("norm_theta1", norm_theta1) print("norm_theta2", norm_theta2) result.upper_left_lip_y = norm_theta1 result.upper_right_lip_y = norm_theta1 result.left_corner_lip_y = norm_theta2 result.right_corner_lip_y = norm_theta2 # === 下唇位置记录（不参与 IK） === lower_mouth_points = { "lower_right_lip": FaceMap.right_lower_lip, "lower_left_lip": FaceMap.left_lower_lip, "lower_lip": FaceMap.mid_down_lip } for name, idx in lower_mouth_points.items(): u, v, z = uv[idx] delta_u = u - bottom_center_u delta_v = v - bottom_center_v x_val = delta_u / head_width y_val = delta_v / head_height z_val = z / head_depth setattr(result, f"{name}_x", x_val) setattr(result, f"{name}_y", y_val) setattr(result, f"{name}_z", z_val) return result except KeyError as e: print("Mouth landmarks missing:", e) return result def calc_jaw(uv, result: HeadJoints): try: # === 获取头部关键点 === upper_indices = [ FaceMap.left_eyebrow_inside, FaceMap.right_eyebrow_inside, FaceMap.left_eyelid_inside, FaceMap.right_eyelid_inside, FaceMap.top_head ] lower_indices = [ FaceMap.mid_down_lip, FaceMap.left_mouth_corner, FaceMap.right_mouth_corner, FaceMap.bottom_head ] upper_points = np.array([uv[idx] for idx in upper_indices]) lower_points = np.array([uv[idx] for idx in lower_indices]) upper_center = np.mean(upper_points, axis=0) lower_center = np.mean(lower_points, axis=0) # === 获取头高进行归一化 === top_uv = np.array(uv[FaceMap.top_head]) bottom_uv = np.array(uv[FaceMap.bottom_head]) head_height = abs(top_uv[1] - bottom_uv[1]) + 1e-6 head_width = abs(uv[FaceMap.left_head][0] - uv[FaceMap.right_head][0]) + 1e-6 # === 张嘴判断：中心点垂直位移归一化 === vertical_offset = abs(lower_center[1] - upper_center[1]) mouth_open_ratio = vertical_offset / head_height if mouth_open_ratio > 0.035: result.jaw_y = mouth_open_ratio else: result.jaw_y = 0.0 # === 左右偏移保留 === upper_lip = np.array(uv[FaceMap.mid_up_lip]) lower_lip = np.array(uv[FaceMap.mid_down_lip]) result.jaw_x = abs(lower_lip[0] - upper_lip[0]) / head_width return result except KeyError as e: print("Jaw landmarks missing:", e) return result