working

calculate.py

@@ -0,0 +1,135 @@
+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