300 lines
12 KiB
Python
300 lines
12 KiB
Python
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
|