135 lines
3.8 KiB
Python
135 lines
3.8 KiB
Python
import numpy as np
|
|
|
|
|
|
import numpy as np
|
|
|
|
def angle_between(pkt1, pkt2, pkt3):
|
|
"""
|
|
Oblicza kąt między trzema punktami w stopniach z zachowaniem znaku.
|
|
pkt2 jest wierzchołkiem kąta.
|
|
|
|
Parameters:
|
|
pkt1, pkt2, pkt3 : array-like (x, y) lub (x, y, z)
|
|
|
|
Returns:
|
|
Kąt w stopniach (ujemny lub dodatni)
|
|
"""
|
|
|
|
pkt1 = np.array(pkt1[:2].cpu().numpy())
|
|
pkt2 = np.array(pkt2[:2].cpu().numpy())
|
|
pkt3 = np.array(pkt3[:2].cpu().numpy())
|
|
|
|
# wektory względem pkt2
|
|
a = pkt1 - pkt2
|
|
b = pkt3 - pkt2
|
|
|
|
# iloczyn skalarny i cosinus kąta
|
|
dot = np.dot(a, b)
|
|
norm = np.linalg.norm(a) * np.linalg.norm(b)
|
|
cos_theta = dot / norm
|
|
cos_theta = np.clip(cos_theta, -1.0, 1.0)
|
|
|
|
# kąt bez znaku
|
|
angle = np.degrees(np.arccos(cos_theta))
|
|
|
|
# znak z iloczynu wektorowego (w 2D to skalar = z-component)
|
|
cross = a[0]*b[1] - a[1]*b[0]
|
|
|
|
if cross < 0:
|
|
angle = -angle
|
|
|
|
return angle
|
|
|
|
def compare_poses(f1, f2):
|
|
# Odległość euklidesowa
|
|
l2_dist = np.linalg.norm(f1 - f2)
|
|
|
|
# Cosine similarity
|
|
cos_sim = np.dot(f1, f2) / (np.linalg.norm(f1) * np.linalg.norm(f2) + 1e-6)
|
|
|
|
return l2_dist, cos_sim
|
|
|
|
def compare_poses_boolean(f1, f2):
|
|
l2, cos_sim = compare_poses(f1, f2)
|
|
|
|
return l2 < 0.7 and cos_sim > 0.90
|
|
|
|
def center(keypoints):
|
|
mid_hip = (keypoints[11] + keypoints[12]) / 2 # left_hip=11, right_hip=12
|
|
keypoints = keypoints - mid_hip
|
|
|
|
return keypoints
|
|
|
|
def normalize_pose(keypoints):
|
|
"""
|
|
keypoints: np.array shape (17, 2) [x,y] dla COCO
|
|
Zwraca wektor cech odporny na skalę i przesunięcie
|
|
"""
|
|
|
|
# 1. translacja -> środek bioder jako początek układu
|
|
mid_hip = (keypoints[11] + keypoints[12]) / 2 # left_hip=11, right_hip=12
|
|
keypoints = keypoints - mid_hip
|
|
|
|
# 2. normalizacja skali -> odległość między barkami
|
|
shoulder_dist = np.linalg.norm(keypoints[5] - keypoints[6]) # left_shoulder=5, right_shoulder=6
|
|
if shoulder_dist > 0:
|
|
keypoints = keypoints / shoulder_dist
|
|
|
|
# 3. definicja segmentów (przykład: łokieć-ramię, nadgarstek-łokieć)
|
|
limbs = [
|
|
(5, 7), # ramię L
|
|
(7, 9), # przedramię L
|
|
(6, 8), # ramię P
|
|
(8, 10), # przedramię P
|
|
(11, 13), # udo L
|
|
(13, 15), # goleń L
|
|
(12, 14), # udo P
|
|
(14, 16), # goleń P
|
|
]
|
|
|
|
# 4. oblicz kąty
|
|
angles = []
|
|
for (a, b), (c, d) in zip(limbs[::2], limbs[1::2]): # np. (ramię, przedramię)
|
|
v1 = keypoints[b] - keypoints[a]
|
|
v2 = keypoints[d] - keypoints[c]
|
|
cos_angle = np.dot(v1, v2) / (np.linalg.norm(v1) * np.linalg.norm(v2) + 1e-6)
|
|
angle = np.arccos(np.clip(cos_angle, -1, 1))
|
|
angles.append(angle)
|
|
|
|
# 5. opcjonalnie: dodać wektory kończyn (znormalizowane)
|
|
vectors = []
|
|
for (a, b) in limbs:
|
|
v = keypoints[b] - keypoints[a]
|
|
v_norm = v / (np.linalg.norm(v) + 1e-6)
|
|
vectors.extend(v_norm)
|
|
|
|
# finalny wektor cech = kąty + wektory
|
|
feature_vector = np.concatenate([angles, vectors])
|
|
|
|
return feature_vector
|
|
|
|
|
|
def denormalize_pose(feature_vector):
|
|
"""
|
|
feature_vector: wynik normalize_pose
|
|
Zwraca przybliżone współrzędne keypoints (w układzie znormalizowanym)
|
|
"""
|
|
# 1. oddziel kąty i wektory
|
|
angles = feature_vector[:4]
|
|
vectors_flat = feature_vector[4:]
|
|
vectors = vectors_flat.reshape(-1, 2)
|
|
|
|
# 2. inicjalizacja keypoints
|
|
keypoints = np.zeros((17, 2))
|
|
|
|
# 3. przybliżona rekonstrukcja kończyn
|
|
limbs = [
|
|
(5, 7), (7, 9), (6, 8), (8, 10),
|
|
(11, 13), (13, 15), (12, 14), (14, 16)
|
|
]
|
|
|
|
for (a, b), v in zip(limbs, vectors):
|
|
keypoints[b] = keypoints[a] + v # przybliżona rekonstrukcja
|
|
|
|
# 4. punkt startowy (biodra) = (0,0), skalowanie w oryginale trzeba by przywrócić osobno
|
|
return keypoints |