504 lines
35 KiB
Python
504 lines
35 KiB
Python
# Copyright (c) OpenMMLab. All rights reserved.
|
|
import logging
|
|
import time
|
|
|
|
import cv2
|
|
from argparse import ArgumentParser
|
|
|
|
from mmengine.logging import print_log
|
|
from mmpose.apis import inference_topdown, init_model
|
|
from mmpose.registry import VISUALIZERS
|
|
from mmpose.structures import merge_data_samples
|
|
import numpy as np
|
|
|
|
from draw import draw_skeleton, print_animation
|
|
from filter import EMAFilter, filter_moves, HybridFilter, MedianFilter
|
|
from threading import Thread
|
|
|
|
class Position:
|
|
def __init__(self, person_kpts):
|
|
if len(person_kpts) == 8:
|
|
self.left_elbow_angle = person_kpts[0]
|
|
self.right_elbow_angle = person_kpts[1]
|
|
self.left_shoulder_angle = person_kpts[2]
|
|
self.right_shoulder_angle = person_kpts[3]
|
|
self.left_leg_angle = person_kpts[4]
|
|
self.right_leg_angle = person_kpts[5]
|
|
self.left_hip_angle = person_kpts[6]
|
|
self.right_hip_angle = person_kpts[7]
|
|
else:
|
|
self.left_hip = person_kpts[11]
|
|
self.left_knee = person_kpts[13]
|
|
self.left_ankle = person_kpts[15]
|
|
|
|
self.right_hip = person_kpts[12]
|
|
self.right_knee = person_kpts[14]
|
|
self.right_ankle = person_kpts[16]
|
|
|
|
self.left_shoulder = person_kpts[5]
|
|
self.left_elbow = person_kpts[7]
|
|
self.left_wrist = person_kpts[9]
|
|
|
|
self.right_shoulder = person_kpts[6]
|
|
self.right_elbow = person_kpts[8]
|
|
self.right_wrist = person_kpts[10]
|
|
|
|
self.left_elbow_angle = angle_between(self.left_shoulder - self.left_elbow, self.left_wrist - self.left_elbow)
|
|
self.right_elbow_angle = angle_between(self.right_shoulder - self.right_elbow, self.right_wrist - self.right_elbow)
|
|
|
|
self.left_shoulder_angle = angle_between(self.right_shoulder - self.left_shoulder, self.left_elbow - self.left_shoulder)
|
|
self.right_shoulder_angle = angle_between(self.left_shoulder - self.right_shoulder, self.right_elbow - self.right_shoulder)
|
|
|
|
self.left_leg_angle = angle_between(self.left_hip - self.left_knee, self.left_ankle - self.left_knee)
|
|
self.right_leg_angle = angle_between(self.right_hip - self.right_knee, self.right_ankle - self.right_knee)
|
|
|
|
self.left_hip_angle = angle_between(self.left_shoulder - self.left_hip, self.left_knee - self.left_hip)
|
|
self.right_hip_angle = angle_between(self.right_shoulder - self.right_hip, self.right_knee - self.right_hip)
|
|
|
|
def to_array(self):
|
|
return [
|
|
self.left_elbow_angle,
|
|
self.right_elbow_angle,
|
|
self.left_shoulder_angle,
|
|
self.right_shoulder_angle,
|
|
0,
|
|
0,
|
|
0,
|
|
0
|
|
# self.left_leg_angle,
|
|
# self.right_leg_angle,
|
|
# self.left_hip_angle,
|
|
# self.left_hip_angle
|
|
]
|
|
|
|
def distance_to(self, position):
|
|
error = 0
|
|
success = True
|
|
|
|
for i, x in enumerate(self.to_array()):
|
|
if i > 3:
|
|
continue
|
|
|
|
y = position.to_array()[i]
|
|
|
|
if abs(y) > 165:
|
|
x = abs(x)
|
|
y = abs(y)
|
|
|
|
dist = abs(y - x)
|
|
|
|
if dist > 30:
|
|
success = False
|
|
# print(f"{i} nie jest ok: moje: {x}, cel: {y}")
|
|
error += abs(y - x)
|
|
else:
|
|
pass
|
|
# print(f"{i} jest ok: moje: {x}, cel: {position.to_array()[i]}")
|
|
|
|
return (success, error)
|
|
|
|
def parse_args():
|
|
parser = ArgumentParser()
|
|
# parser.add_argument('checkpoint', help='Checkpoint file')
|
|
parser.add_argument(
|
|
'--device', default='cuda:0', help='Device used for inference')
|
|
parser.add_argument(
|
|
'--draw-heatmap',
|
|
action='store_true',
|
|
help='Visualize the predicted heatmap')
|
|
parser.add_argument(
|
|
'--show-kpt-idx',
|
|
action='store_true',
|
|
default=False,
|
|
help='Whether to show the index of keypoints')
|
|
parser.add_argument(
|
|
'--skeleton-style',
|
|
default='mmpose',
|
|
type=str,
|
|
choices=['mmpose', 'openpose'],
|
|
help='Skeleton style selection')
|
|
parser.add_argument(
|
|
'--kpt-thr',
|
|
type=float,
|
|
default=0.3,
|
|
help='Visualizing keypoint thresholds')
|
|
parser.add_argument(
|
|
'--radius',
|
|
type=int,
|
|
default=3,
|
|
help='Keypoint radius for visualization')
|
|
parser.add_argument(
|
|
'--thickness',
|
|
type=int,
|
|
default=1,
|
|
help='Link thickness for visualization')
|
|
parser.add_argument(
|
|
'--alpha', type=float, default=0.8, help='The transparency of bboxes')
|
|
parser.add_argument(
|
|
'--camera-id', type=int, default=0, help='Camera device ID')
|
|
args = parser.parse_args()
|
|
return args
|
|
|
|
|
|
def angle_between(v1, v2):
|
|
"""
|
|
Liczy kąt między wektorami v1 i v2 w stopniach.
|
|
Znak kąta zależy od kierunku (przeciwnie do ruchu wskazówek zegara jest dodatni).
|
|
"""
|
|
# kąt w radianach
|
|
angle = np.arccos(np.clip(np.dot(v1, v2) / (np.linalg.norm(v1) * np.linalg.norm(v2)), -1.0, 1.0))
|
|
|
|
# obliczamy znak kąta w 2D: + jeśli v2 jest "po lewej" od v1
|
|
sign = np.sign(v1[0] * v2[1] - v1[1] * v2[0])
|
|
|
|
return np.degrees(angle) * sign
|
|
|
|
def is_visible(kpt, threshold=0.3):
|
|
# kpt = [x, y, score]
|
|
return kpt[2] > threshold
|
|
|
|
def list_cameras(max_cameras=10):
|
|
available = []
|
|
for i in range(max_cameras):
|
|
cap = cv2.VideoCapture(i)
|
|
if cap.isOpened():
|
|
available.append(i)
|
|
cap.release()
|
|
return available
|
|
|
|
ema = EMAFilter()
|
|
|
|
def main():
|
|
args = parse_args()
|
|
|
|
# build the model from a config file and a checkpoint file
|
|
if args.draw_heatmap:
|
|
cfg_options = dict(model=dict(test_cfg=dict(output_heatmaps=True)))
|
|
else:
|
|
cfg_options = None
|
|
|
|
model = init_model(
|
|
"mmpose/configs/body_2d_keypoint/topdown_heatmap/coco/td-hm_hrnet-w48_8xb32-210e_coco-256x192.py",
|
|
"hrnet_w48_coco_256x192-b9e0b3ab_20200708.pth",
|
|
device=args.device,
|
|
cfg_options=cfg_options)
|
|
|
|
# init visualizer
|
|
model.cfg.visualizer.radius = args.radius
|
|
model.cfg.visualizer.alpha = args.alpha
|
|
model.cfg.visualizer.line_width = args.thickness
|
|
|
|
visualizer = VISUALIZERS.build(model.cfg.visualizer)
|
|
visualizer.set_dataset_meta(
|
|
model.dataset_meta, skeleton_style=args.skeleton_style)
|
|
|
|
# start capturing video from camera
|
|
cap = cv2.VideoCapture(args.camera_id)
|
|
if not cap.isOpened():
|
|
print("Error: Cannot open camera")
|
|
return
|
|
|
|
lastDegs = []
|
|
lastMove = 0
|
|
visible = 0
|
|
maha = 0
|
|
start = False
|
|
|
|
oldpos = None
|
|
waiting = 100
|
|
|
|
positions = []
|
|
numPositions = 0
|
|
tPose = Position(
|
|
[
|
|
167.5568,
|
|
-161.67027,
|
|
-166.49443,
|
|
168.22028,
|
|
110.21745,
|
|
166.41733,
|
|
167.57822,
|
|
-176.08066
|
|
]
|
|
)
|
|
|
|
krakowiak = Position(
|
|
[np.float32(-98.226715), np.float32(106.39389), np.float32(-135.05656), np.float32(139.48904), np.float32(-149.9036), np.float32(8.216028), np.float32(174.70923), np.float32(-176.893)]
|
|
)
|
|
|
|
krakowiakRight = Position(
|
|
[np.float32(-110.61421), np.float32(-174.2424), np.float32(-127.05916), np.float32(174.9463),
|
|
np.float32(175.62007), np.float32(166.89127), np.float32(168.8219), np.float32(-178.02744)]
|
|
)
|
|
|
|
deltaTime = None
|
|
moves = [(0, [np.float32(161.84187), np.float32(-172.6993), np.float32(-154.06445), np.float32(150.50404), 0, 0, 0, 0]), (0, [np.float32(161.84187), np.float32(-172.6993), np.float32(-154.06445), np.float32(150.50404), 0, 0, 0, 0]), (1, [np.float32(163.13943), np.float32(-173.1322), np.float32(-158.12773), np.float32(160.24446), 0, 0, 0, 0]), (2, [np.float32(165.41116), np.float32(-175.18993), np.float32(-161.29004), np.float32(167.44011), 0, 0, 0, 0]), (3, [np.float32(164.24295), np.float32(-176.7612), np.float32(-162.28572), np.float32(172.58678), 0, 0, 0, 0]), (4, [np.float32(163.36208), np.float32(-178.36604), np.float32(-163.05962), np.float32(176.6291), 0, 0, 0, 0]), (5, [np.float32(162.68939), np.float32(-178.87714), np.float32(-163.66457), np.float32(179.06758), 0, 0, 0, 0]), (6, [np.float32(162.17062), np.float32(-179.27579), np.float32(-164.13965), np.float32(-179.11417), 0, 0, 0, 0]), (7, [np.float32(164.41534), np.float32(-179.26317), np.float32(-165.92986), np.float32(-177.91197), 0, 0, 0, 0]), (8, [np.float32(164.73833), np.float32(-176.53934), np.float32(-165.93678), np.float32(-177.11679), 0, 0, 0, 0]), (9, [np.float32(164.99178), np.float32(-174.68193), np.float32(-165.94228), np.float32(-174.50232), 0, 0, 0, 0]), (10, [np.float32(-139.94519), np.float32(174.61586), np.float32(-152.44669), np.float32(143.26634), 0, 0, 0, 0]), (11, [np.float32(-130.12726), np.float32(161.89061), np.float32(-149.16766), np.float32(139.76706), 0, 0, 0, 0]), (12, [np.float32(-122.70336), np.float32(152.0069), np.float32(-146.54733), np.float32(136.28027), 0, 0, 0, 0]), (13, [np.float32(-118.37861), np.float32(142.68488), np.float32(-143.3348), np.float32(134.37267), 0, 0, 0, 0]), (14, [np.float32(-115.18997), np.float32(135.4553), np.float32(-140.76765), np.float32(133.1421), 0, 0, 0, 0]), (15, [np.float32(-112.808075), np.float32(129.39622), np.float32(-137.67496), np.float32(134.91853), 0, 0, 0, 0]), (16, [np.float32(-112.3576), np.float32(124.21153), np.float32(-135.20432), np.float32(136.18352), 0, 0, 0, 0]), (17, [np.float32(-109.60173), np.float32(120.263466), np.float32(-134.26578), np.float32(137.10904), 0, 0, 0, 0]), (18, [np.float32(-109.35222), np.float32(116.95526), np.float32(-134.41501), np.float32(136.63664), 0, 0, 0, 0]), (19, [np.float32(-109.15874), np.float32(114.68397), np.float32(-133.48157), np.float32(136.27863), 0, 0, 0, 0]), (20, [np.float32(-109.00771), np.float32(113.77597), np.float32(-132.73865), np.float32(136.00403), 0, 0, 0, 0]), (21, [np.float32(-108.23168), np.float32(111.97663), np.float32(-132.04283), np.float32(136.99962), 0, 0, 0, 0]), (22, [np.float32(-106.717926), np.float32(111.65684), np.float32(-131.2465), np.float32(137.8607), 0, 0, 0, 0]), (23, [np.float32(-105.5196), np.float32(111.40636), np.float32(-130.61122), np.float32(138.5357), 0, 0, 0, 0]), (24, [np.float32(-106.11364), np.float32(110.01783), np.float32(-129.42293), np.float32(138.86), 0, 0, 0, 0]), (25, [np.float32(-106.58496), np.float32(108.92003), np.float32(-128.47336), np.float32(139.116), 0, 0, 0, 0]), (26, [np.float32(-106.58942), np.float32(106.99456), np.float32(-129.7829), np.float32(139.33575), 0, 0, 0, 0]), (27, [np.float32(-106.59296), np.float32(106.640495), np.float32(-130.83006), np.float32(139.51007), 0, 0, 0, 0]), (28, [np.float32(-105.27759), np.float32(104.671364), np.float32(-132.70628), np.float32(139.48753), 0, 0, 0, 0]), (29, [np.float32(-104.22746), np.float32(102.66828), np.float32(-133.16719), np.float32(143.97647), 0, 0, 0, 0]), (30, [np.float32(-104.70626), np.float32(117.20821), np.float32(-134.57655), np.float32(148.39961), 0, 0, 0, 0]), (31, [np.float32(-104.023735), np.float32(136.78886), np.float32(-135.80733), np.float32(150.81862), 0, 0, 0, 0]), (32, [np.float32(-102.60743), np.float32(171.96053), np.float32(-135.98955), np.float32(164.99341), 0, 0, 0, 0]), (33, [np.float32(-103.24832), np.float32(-178.57991), np.float32(-137.24109), np.float32(168.9585), 0, 0, 0, 0]), (34, [np.float32(-103.760475), np.float32(-172.26549), np.float32(-138.2425), np.float32(172.08504), 0, 0, 0, 0]), (35, [np.float32(-104.169785), np.float32(-167.94159), np.float32(-139.04327), np.float32(174.54433), 0, 0, 0, 0]), (36, [np.float32(-103.22697), np.float32(-166.36867), np.float32(-138.60309), np.float32(178.8122), 0, 0, 0, 0]), (37, [np.float32(-103.513466), np.float32(-164.443), np.float32(-138.25127), np.float32(-178.47545), 0, 0, 0, 0]), (38, [np.float32(-102.70224), np.float32(-163.01959), np.float32(-137.76875), np.float32(-176.12628), 0, 0, 0, 0]), (39, [np.float32(-102.053375), np.float32(-159.2099), np.float32(-137.38295), np.float32(-175.76183), 0, 0, 0, 0]), (40, [np.float32(-104.33853), np.float32(-155.71497), np.float32(-136.15556), np.float32(-177.79669), 0, 0, 0, 0]), (41, [np.float32(-103.73066), np.float32(-154.49757), np.float32(-137.0677), np.float32(178.94426), 0, 0, 0, 0]), (42, [np.float32(-105.71198), np.float32(-170.0752), np.float32(-136.75684), np.float32(172.1105), 0, 0, 0, 0]), (43, [np.float32(-105.871155), np.float32(171.0166), np.float32(-135.36615), np.float32(166.45299), 0, 0, 0, 0]), (44, [np.float32(-105.00114), np.float32(153.44203), np.float32(-134.25249), np.float32(160.00745), 0, 0, 0, 0]), (45, [np.float32(-104.30579), np.float32(139.84973), np.float32(-133.36147), np.float32(154.78476), 0, 0, 0, 0]), (46, [np.float32(-103.7501), np.float32(130.25952), np.float32(-132.64893), np.float32(150.62692), 0, 0, 0, 0]), (47, [np.float32(-104.30217), np.float32(123.622536), np.float32(-132.0792), np.float32(147.3452), 0, 0, 0, 0]), (48, [np.float32(-104.74346), np.float32(121.08171), np.float32(-130.58493), np.float32(144.76329), 0, 0, 0, 0]), (49, [np.float32(-99.244865), np.float32(118.03042), np.float32(-133.99681), np.float32(143.91876), 0, 0, 0, 0]), (50, [np.float32(-92.73578), np.float32(116.2656), np.float32(-137.87865), np.float32(141.01414), 0, 0, 0, 0]), (51, [np.float32(-107.468056), np.float32(114.49911), np.float32(-147.1293), np.float32(139.77505), 0, 0, 0, 0]), (52, [np.float32(-165.97617), np.float32(111.14478), np.float32(-168.62744), np.float32(139.6505), 0, 0, 0, 0]), (53, [np.float32(-174.6152), np.float32(110.74168), np.float32(-175.63455), np.float32(138.70796), 0, 0, 0, 0]), (54, [np.float32(-178.59145), np.float32(110.42677), np.float32(177.65903), np.float32(137.95808), 0, 0, 0, 0]), (55, [np.float32(178.7716), np.float32(110.17957), np.float32(172.34877), np.float32(137.361), 0, 0, 0, 0]), (56, [np.float32(174.95648), np.float32(109.98475), np.float32(169.92851), np.float32(136.88521), 0, 0, 0, 0]), (57, [np.float32(169.87465), np.float32(109.83074), np.float32(169.24754), np.float32(136.50583), 0, 0, 0, 0]), (58, [np.float32(166.18379), np.float32(108.65322), np.float32(170.08055), np.float32(137.35068), 0, 0, 0, 0]), (59, [np.float32(163.31392), np.float32(108.60895), np.float32(166.54794), np.float32(136.87854), 0, 0, 0, 0]), (60, [np.float32(179.7892), np.float32(107.68456), np.float32(172.4392), np.float32(137.64746), 0, 0, 0, 0]), (61, [np.float32(-140.90053), np.float32(105.441475), np.float32(-168.52774), np.float32(138.64941), 0, 0, 0, 0]), (62, [np.float32(-128.9079), np.float32(105.40392), np.float32(-158.6232), np.float32(139.06253), 0, 0, 0, 0]), (63, [np.float32(-122.11822), np.float32(105.373985), np.float32(-152.69356), np.float32(138.24881), 0, 0, 0, 0]), (64, [np.float32(-117.600975), np.float32(104.18713), np.float32(-148.40807), np.float32(137.59804), 0, 0, 0, 0]), (65, [np.float32(-116.59493), np.float32(104.401665), np.float32(-144.06621), np.float32(137.07764), 0, 0, 0, 0]), (66, [np.float32(-114.63403), np.float32(102.531395), np.float32(-141.9956), np.float32(137.54247), 0, 0, 0, 0]), (67, [np.float32(-111.56019), np.float32(102.19489), np.float32(-140.43889), np.float32(137.91415), 0, 0, 0, 0]), (68, [np.float32(-109.25588), np.float32(116.599396), np.float32(-139.25383), np.float32(141.66272), 0, 0, 0, 0]), (69, [np.float32(-107.50838), np.float32(134.56642), np.float32(-138.34264), np.float32(147.80385), 0, 0, 0, 0]), (70, [np.float32(-105.799965), np.float32(152.62177), np.float32(-137.91347), np.float32(155.7829), 0, 0, 0, 0]), (71, [np.float32(-104.4792), np.float32(165.90117), np.float32(-137.57875), np.float32(163.71794), 0, 0, 0, 0]), (72, [np.float32(-104.75762), np.float32(176.23598), np.float32(-137.4273), np.float32(170.3419), 0, 0, 0, 0]), (73, [np.float32(-103.65557), np.float32(-177.33661), np.float32(-137.10136), np.float32(176.50806), 0, 0, 0, 0]), (74, [np.float32(-102.78934), np.float32(-174.1429), np.float32(-136.84392), np.float32(-178.5898), 0, 0, 0, 0]), (75, [np.float32(-103.172806), np.float32(-167.56412), np.float32(-136.84467), np.float32(-177.89601), 0, 0, 0, 0]), (76, [np.float32(-104.72311), np.float32(-161.45961), np.float32(-136.84526), np.float32(179.27217), 0, 0, 0, 0]), (77, [np.float32(-103.67907), np.float32(-158.52728), np.float32(-137.86945), np.float32(175.09763), 0, 0, 0, 0]), (78, [np.float32(-105.117325), np.float32(-154.15791), np.float32(-136.42995), np.float32(168.93047), 0, 0, 0, 0]), (79, [np.float32(-105.25614), np.float32(-169.3192), np.float32(-135.28133), np.float32(163.15321), 0, 0, 0, 0]), (80, [np.float32(-104.45899), np.float32(173.06963), np.float32(-135.22464), np.float32(158.3367), 0, 0, 0, 0]), (81, [np.float32(-103.82391), np.float32(156.2318), np.float32(-135.17943), np.float32(154.51944), 0, 0, 0, 0]), (82, [np.float32(-104.36369), np.float32(145.40253), np.float32(-134.10019), np.float32(149.38187), 0, 0, 0, 0]), (83, [np.float32(-104.79425), np.float32(136.95003), np.float32(-133.23784), np.float32(145.28444), 0, 0, 0, 0]), (84, [np.float32(-104.99684), np.float32(130.52034), np.float32(-130.55696), np.float32(144.32228), 0, 0, 0, 0]), (85, [np.float32(-105.158585), np.float32(125.91378), np.float32(-128.40271), np.float32(143.56995), 0, 0, 0, 0]), (86, [np.float32(-102.13468), np.float32(121.03409), np.float32(-129.73077), np.float32(141.80165), 0, 0, 0, 0]), (87, [np.float32(-99.699646), np.float32(117.398964), np.float32(-130.7912), np.float32(140.40463), 0, 0, 0, 0]), (88, [np.float32(-95.75206), np.float32(114.66714), np.float32(-134.67148), np.float32(139.2993), 0, 0, 0, 0]), (89, [np.float32(-90.97868), np.float32(112.59396), np.float32(-139.61217), np.float32(138.42339), 0, 0, 0, 0]), (90, [np.float32(-102.38529), np.float32(110.57184), np.float32(-145.2404), np.float32(139.83658), 0, 0, 0, 0]), (91, [np.float32(-165.27873), np.float32(110.72392), np.float32(-169.38757), np.float32(141.70813), 0, 0, 0, 0]), (92, [np.float32(-172.6769), np.float32(112.30739), np.float32(-177.3452), np.float32(140.35773), 0, 0, 0, 0]), (93, [np.float32(-179.54256), np.float32(113.55757), np.float32(178.06575), np.float32(139.3832), 0, 0, 0, 0]), (94, [np.float32(174.98294), np.float32(113.57226), np.float32(175.13823), np.float32(138.50168), 0, 0, 0, 0]), (95, [np.float32(172.45665), np.float32(113.583885), np.float32(172.8797), np.float32(137.7979), 0, 0, 0, 0]), (96, [np.float32(171.99323), np.float32(113.59312), np.float32(175.49107), np.float32(137.23596), 0, 0, 0, 0]), (97, [np.float32(178.0344), np.float32(112.550095), np.float32(177.63959), np.float32(137.93323), 0, 0, 0, 0]), (98, [np.float32(-165.7326), np.float32(111.07863), np.float32(-176.49493), np.float32(138.49036), 0, 0, 0, 0]), (99, [np.float32(-151.17813), np.float32(109.9045), np.float32(-167.08968), np.float32(138.93553), 0, 0, 0, 0]), (100, [np.float32(-138.2745), np.float32(108.967865), np.float32(-161.32912), np.float32(138.14696), 0, 0, 0, 0]), (101, [np.float32(-132.70343), np.float32(110.581604), np.float32(-154.63971), np.float32(136.47801), 0, 0, 0, 0]), (102, [np.float32(-127.17967), np.float32(111.8716), np.float32(-149.30565), np.float32(135.14091), 0, 0, 0, 0]), (103, [np.float32(-123.16848), np.float32(110.54169), np.float32(-145.11604), np.float32(135.11264), 0, 0, 0, 0]), (104, [np.float32(-120.21519), np.float32(108.34589), np.float32(-141.84482), np.float32(134.20844), 0, 0, 0, 0]), (105, [np.float32(-117.55712), np.float32(107.723595), np.float32(-140.39064), np.float32(134.36714), 0, 0, 0, 0]), (106, [np.float32(-117.51129), np.float32(110.760155), np.float32(-140.543), np.float32(139.15918), 0, 0, 0, 0]), (107, [np.float32(-117.476326), np.float32(113.20252), np.float32(-140.66214), np.float32(143.06827), 0, 0, 0, 0]), (108, [np.float32(-129.89436), np.float32(127.2386), np.float32(-144.9321), np.float32(146.38098), 0, 0, 0, 0]), (109, [np.float32(-142.86472), np.float32(142.61395), np.float32(-150.88062), np.float32(150.33803), 0, 0, 0, 0]), (110, [np.float32(-155.33846), np.float32(153.50363), np.float32(-155.71011), np.float32(141.02138), 0, 0, 0, 0]), (111, [np.float32(-165.87502), np.float32(159.84718), np.float32(-159.56946), np.float32(154.21132), 0, 0, 0, 0]), (112, [np.float32(-173.82352), np.float32(164.14807), np.float32(-162.49919), np.float32(163.88666), 0, 0, 0, 0]), (113, [np.float32(-178.33447), np.float32(167.12366), np.float32(-164.81244), np.float32(170.63316), 0, 0, 0, 0]), (114, [np.float32(178.33392), np.float32(169.33806), np.float32(-166.63837), np.float32(175.38054), 0, 0, 0, 0]), (115, [np.float32(175.84334), np.float32(171.18196), np.float32(-168.08095), np.float32(178.78517), 0, 0, 0, 0]), (116, [np.float32(173.95944), np.float32(172.01833), np.float32(-169.22226), np.float32(-178.29631), 0, 0, 0, 0]), (117, [np.float32(172.51959), np.float32(172.66199), np.float32(-170.1266), np.float32(-176.12106), 0, 0, 0, 0]), (118, [np.float32(171.4095), np.float32(173.16136), np.float32(-170.84425), np.float32(-174.47803), 0, 0, 0, 0]), (119, [np.float32(170.54744), np.float32(173.93652), np.float32(-171.41455), np.float32(-173.62332), 0, 0, 0, 0]), (120, [np.float32(169.8417), np.float32(174.545), np.float32(-171.9954), np.float32(-172.96487), 0, 0, 0, 0]), (121, [np.float32(169.45013), np.float32(175.392), np.float32(-172.45805), np.float32(-172.45366), 0, 0, 0, 0]), (122, [np.float32(169.14171), np.float32(175.69579), np.float32(-172.82681), np.float32(-172.0543), 0, 0, 0, 0]), (123, [np.float32(168.89798), np.float32(175.9364), np.float32(-173.12094), np.float32(-171.7408), 0, 0, 0, 0]), (124, [np.float32(168.7049), np.float32(176.55673), np.float32(-173.35567), np.float32(-171.7673), 0, 0, 0, 0]), (125, [np.float32(169.83656), np.float32(176.6217), np.float32(-173.54308), np.float32(-171.51614), 0, 0, 0, 0]), (126, [np.float32(169.4547), np.float32(176.5463), np.float32(-173.69276), np.float32(-171.31714), 0, 0, 0, 0]), (127, [np.float32(169.15074), np.float32(176.61311), np.float32(-173.81236), np.float32(-171.15912), 0, 0, 0, 0]), (128, [np.float32(168.78177), np.float32(176.66637), np.float32(-173.78178), np.float32(-171.0335), 0, 0, 0, 0]), (129, [np.float32(168.61423), np.float32(176.58234), np.float32(-173.75737), np.float32(-170.93346), 0, 0, 0, 0]), (130, [np.float32(168.35405), np.float32(177.06665), np.float32(-173.73782), np.float32(-171.12204), 0, 0, 0, 0]), (131, [np.float32(168.27277), np.float32(177.02876), np.float32(-173.84824), np.float32(-171.00452), 0, 0, 0, 0]), (132, [np.float32(168.08153), np.float32(177.26564), np.float32(-173.81052), np.float32(-171.17836), 0, 0, 0, 0]), (133, [np.float32(168.055), np.float32(177.45491), np.float32(-173.90634), np.float32(-171.31714), 0, 0, 0, 0]), (134, [np.float32(167.90762), np.float32(177.33932), np.float32(-173.85701), np.float32(-171.16081), 0, 0, 0, 0]), (135, [np.float32(165.32097), np.float32(177.2469), np.float32(-172.5806), np.float32(-171.0359), 0, 0, 0, 0]), (136, [np.float32(165.84894), np.float32(177.17302), np.float32(-172.9227), np.float32(-170.93608), 0, 0, 0, 0]), (137, [np.float32(166.27055), np.float32(176.61818), np.float32(-173.07027), np.float32(-170.48625), 0, 0, 0, 0]), (138, [np.float32(166.6073), np.float32(176.67015), np.float32(-173.31427), np.float32(-170.49654), 0, 0, 0, 0]), (139, [np.float32(166.87636), np.float32(176.71173), np.float32(-173.5094), np.float32(-170.50476), 0, 0, 0, 0]), (140, [np.float32(167.09137), np.float32(176.74498), np.float32(-173.66547), np.float32(-170.51135), 0, 0, 0, 0]), (141, [np.float32(167.10599), np.float32(176.77156), np.float32(-173.7903), np.float32(-170.5166), 0, 0, 0, 0]), (142, [np.float32(167.11769), np.float32(176.79283), np.float32(-173.89015), np.float32(-170.52081), 0, 0, 0, 0]), (143, [np.float32(167.28416), np.float32(176.80983), np.float32(-173.97002), np.float32(-170.52417), 0, 0, 0, 0]), (144, [np.float32(167.29128), np.float32(176.82346), np.float32(-173.90796), np.float32(-170.52687), 0, 0, 0, 0]), (145, [np.float32(167.42294), np.float32(176.83432), np.float32(-173.98425), np.float32(-170.52902), 0, 0, 0, 0]), (146, [np.float32(167.52826), np.float32(176.84305), np.float32(-174.04529), np.float32(-170.53075), 0, 0, 0, 0]), (147, [np.float32(167.61249), np.float32(176.85), np.float32(-174.0941), np.float32(-170.53212), 0, 0, 0, 0]), (148, [np.float32(167.5539), np.float32(176.85558), np.float32(-174.00723), np.float32(-170.53323), 0, 0, 0, 0]), (149, [np.float32(167.50702), np.float32(176.86003), np.float32(-173.93774), np.float32(-170.53413), 0, 0, 0, 0]), (150, [np.float32(167.59549), np.float32(176.73766), np.float32(-174.00806), np.float32(-170.53484), 0, 0, 0, 0]), (151, [np.float32(167.66624), np.float32(176.63974), np.float32(-174.06433), np.float32(-170.53539), 0, 0, 0, 0]), (152, [np.float32(167.59691), np.float32(177.1108), np.float32(-173.98341), np.float32(-170.80229), 0, 0, 0, 0]), (153, [np.float32(167.54144), np.float32(177.3307), np.float32(-173.91869), np.float32(-171.01582), 0, 0, 0, 0]), (154, [np.float32(167.49707), np.float32(177.6636), np.float32(-173.8669), np.float32(-171.18665), 0, 0, 0, 0]), (155, [np.float32(167.58751), np.float32(177.3805), np.float32(-173.9514), np.float32(-171.05684), 0, 0, 0, 0]), (156, [np.float32(167.65985), np.float32(177.70346), np.float32(-174.019), np.float32(-171.21947), 0, 0, 0, 0]), (157, [np.float32(167.59181), np.float32(177.41237), np.float32(-173.94714), np.float32(-171.0831), 0, 0, 0, 0]), (158, [np.float32(167.6633), np.float32(177.30545), np.float32(-174.0156), np.float32(-170.974), 0, 0, 0, 0]), (159, [np.float32(167.56349), np.float32(177.09395), np.float32(-173.78746), np.float32(-170.88673), 0, 0, 0, 0]), (160, [np.float32(167.51472), np.float32(177.47421), np.float32(-173.76193), np.float32(-171.08337), 0, 0, 0, 0]), (161, [np.float32(167.60162), np.float32(177.35492), np.float32(-173.7415), np.float32(-170.97421), 0, 0, 0, 0]), (162, [np.float32(167.54523), np.float32(177.13354), np.float32(-173.72516), np.float32(-170.88692), 0, 0, 0, 0]), (163, [np.float32(166.19153), np.float32(175.81557), np.float32(-172.17513), np.float32(-170.81705), 0, 0, 0, 0]), (164, [np.float32(166.14592), np.float32(177.91684), np.float32(-170.93254), np.float32(-167.78476), 0, 0, 0, 0])]
|
|
|
|
displayMoveTime = 0
|
|
|
|
currTimeIndex = 0
|
|
currIndex = None
|
|
currMove = None
|
|
currStatus = "Zacznij tanczyc"
|
|
|
|
FPS = 12
|
|
frame_time = 1.0 / FPS
|
|
|
|
delays = 0
|
|
delaysCount = 0
|
|
|
|
failRate = 10
|
|
goodCount = 0
|
|
failCount = 0
|
|
delay = 0
|
|
|
|
last_time = time.time()
|
|
startTime = time.time() / 1000
|
|
|
|
try:
|
|
while True:
|
|
current_time = time.time()
|
|
delta = current_time - last_time
|
|
last_time = current_time
|
|
|
|
fps = 1 / delta if delta > 0 else float('inf')
|
|
# print(f"Delta: {delta:.4f}s, FPS: {fps:.2f}")
|
|
|
|
start_time = time.time()
|
|
|
|
ret, frame = cap.read()
|
|
if not ret:
|
|
print("Error: Cannot read frame from camera")
|
|
break
|
|
|
|
# convert BGR to RGB
|
|
img = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
|
|
|
|
# inference
|
|
batch_results = inference_topdown(model, img)
|
|
results = merge_data_samples(batch_results)
|
|
|
|
person_kpts = results.pred_instances.keypoints[0]
|
|
person_visible = results.pred_instances.keypoints_visible[0]
|
|
|
|
left_hand_visible = person_visible[5] > 0.75 and person_visible[5] > 0.75 and person_visible[9] > 0.75
|
|
right_hand_visible = person_visible[6] > 0.75 and person_visible[8] > 0.75 and person_visible[10] > 0.75
|
|
|
|
left_leg_visible = person_visible[11] > 0.75 and person_visible[13] > 0.75 and person_visible[15] > 0.75
|
|
right_leg_visible = person_visible[12] > 0.75 and person_visible[14] > 0.75 and person_visible[16] > 0.75
|
|
|
|
position = Position(person_kpts)
|
|
position = Position(ema.update(position.to_array()))
|
|
|
|
if currMove is not None:
|
|
currTimeIndex += 1
|
|
|
|
if currTimeIndex == len(moves) -1:
|
|
print(f"PODSUMOWANIE: FAIL {failCount} GOOD: {goodCount} MISS PERCENTAGE: {failCount / goodCount * 100}%")
|
|
break
|
|
|
|
if currMove is None:
|
|
if position.distance_to(Position(moves[0][1]))[0]:
|
|
currIndex = 1
|
|
deltaTime = time.time()
|
|
currStatus = f"Zaczoles tanczyc {currIndex}"
|
|
currMove = moves[0]
|
|
|
|
# thread = Thread(target=print_animation, args=(moves, False))
|
|
# thread.start()
|
|
else:
|
|
if currTimeIndex - currIndex > failRate:
|
|
currStatus = f"FAIL!"
|
|
failCount += 1
|
|
|
|
if position.distance_to(Position(moves[currTimeIndex][1])) [0]:
|
|
# delays += (time.time() - deltaTime - moves[0][0]) * 1000
|
|
# delaysCount += 1
|
|
|
|
currStatus = f"SUPER! {currIndex} Zostalo {len(moves)} Delay {(time.time() - deltaTime - moves[0][0]) * 1000}ms"
|
|
deltaTime = time.time()
|
|
|
|
currIndex = currTimeIndex + 1
|
|
goodCount += 1
|
|
|
|
if position.distance_to(Position(moves[currIndex][1])) [0]:
|
|
# delays += (time.time() - deltaTime - moves[0][0]) * 1000
|
|
# delaysCount += 1
|
|
|
|
currStatus = f"SUPER! {currIndex} Zostalo {len(moves)} Delay {(time.time() - deltaTime - moves[0][0]) * 1000}ms"
|
|
deltaTime = time.time()
|
|
|
|
currIndex += 1
|
|
goodCount += 1
|
|
|
|
|
|
try:
|
|
stick = draw_skeleton(moves[currTimeIndex][1], show_angles=True)
|
|
cv2.imshow("skeleton", stick)
|
|
except:
|
|
pass
|
|
|
|
# try:
|
|
# stick = draw_skeleton(position.to_array(), show_angles=True)
|
|
# cv2.imshow("skeleton", stick)
|
|
# except:
|
|
# pass
|
|
|
|
# if position.distance_to(tPose)[0]:
|
|
# print("\rT POSE!", end="")
|
|
# elif position.distance_to(krakowiak)[0]:
|
|
# print("\rKrakowiak", end="")
|
|
# elif position.distance_to(krakowiakRight)[0]:
|
|
# print("\rKrakowiak right", end="")
|
|
# else:
|
|
# print("\rNIC!", end="")
|
|
|
|
# RECORD
|
|
|
|
# if position.distance_to(tPose)[0] and not start:
|
|
# start = True
|
|
# deltaTime = time.time() * 1000
|
|
# moves.append((0, position.to_array()))
|
|
# print("\rT POSE!", end="")
|
|
#
|
|
# if oldpos is None:
|
|
# oldpos = position
|
|
#
|
|
# if oldpos.distance_to(position)[0] and start:
|
|
# print(f"\r{numPositions}", end="")
|
|
# moves.append((numPositions, position.to_array()))
|
|
# numPositions += 1
|
|
#
|
|
# oldpos = position
|
|
|
|
# if error > 200:
|
|
|
|
# if numPositions > 100:
|
|
# printX = []
|
|
#
|
|
# for x in moves:
|
|
# printX.append((x[0], x[1].to_array()))
|
|
#
|
|
# print(printX)
|
|
# exit()
|
|
# else:
|
|
# print(f"\r{numPositions}", end="")
|
|
# moves.append((time.time() * 1000 - deltaTime, position))
|
|
# numPositions += 1
|
|
#
|
|
# if numPositions > 100:
|
|
# avgPosition = [0, 0, 0, 0, 0, 0, 0, 0]
|
|
#
|
|
# for element in positions:
|
|
# for i, data in enumerate(element):
|
|
# avgPosition[i] += data
|
|
#
|
|
# for i, element in enumerate(avgPosition):
|
|
# avgPosition[i] = avgPosition[i] / numPositions
|
|
#
|
|
# print(avgPosition)
|
|
#
|
|
# break
|
|
# else:
|
|
# print(f"\r{numPositions}", end="")
|
|
# if oldpos is None:
|
|
# oldpos = position
|
|
#
|
|
# if oldpos.distance_to(position)[0]:
|
|
# positions.insert(0, position.to_array())
|
|
# numPositions += 1
|
|
#
|
|
# oldpos = position
|
|
|
|
# else:
|
|
# print(f"\rOK!", end="")
|
|
|
|
# if waiting != 0 and not start:
|
|
# print(f"\r{waiting}", end="")
|
|
# waiting -= 1
|
|
# else:
|
|
# start = True
|
|
|
|
# lastDegs.insert(0, (left_elbow_angle, right_elbow_angle))
|
|
#
|
|
# last = 0
|
|
# lastCount = 0
|
|
#
|
|
# for element in lastDegs:
|
|
# last += element[1]
|
|
# lastCount += 1
|
|
#
|
|
# last = last / lastCount
|
|
# dist = right_elbow_angle - last
|
|
#
|
|
# if not right_visible:
|
|
# visible = 0
|
|
# print("\rNie widać prawej ręki!!!!!!!!", end="")
|
|
# else:
|
|
# if maha == 0:
|
|
# print("\rWidać rękę, nie maha!", end="")
|
|
# else:
|
|
# maha -= 1
|
|
#
|
|
# visible += 1
|
|
#
|
|
# if 15 < abs(dist) < 60 and visible > 5:
|
|
# if lastMove != dist > 0:
|
|
# maha = 10
|
|
# print("\rmaha!", end="")
|
|
#
|
|
# lastMove = dist > 0
|
|
#
|
|
# if len(lastDegs) > 5:
|
|
# lastDegs.pop()
|
|
|
|
# visualize
|
|
# vis_img = visualizer.add_datasample(
|
|
# 'result',
|
|
# img,
|
|
# data_sample=results,
|
|
# draw_gt=False,
|
|
# draw_bbox=True,
|
|
# kpt_thr=args.kpt_thr,
|
|
# draw_heatmap=args.draw_heatmap,
|
|
# show_kpt_idx=args.show_kpt_idx,
|
|
# skeleton_style=args.skeleton_style,
|
|
# show=False,
|
|
# out_file=None)
|
|
|
|
# convert RGB back to BGR for OpenCV
|
|
vis_img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
|
|
scale = 2 # np. zmniejszenie o połowę
|
|
vis_img_resized = cv2.resize(vis_img, None, fx=scale, fy=scale)
|
|
|
|
cv2.putText(
|
|
vis_img_resized, # obraz
|
|
currStatus, # tekst
|
|
(50, 100), # pozycja (x, y) lewego dolnego rogu tekstu
|
|
cv2.FONT_HERSHEY_SIMPLEX, # czcionka
|
|
1, # rozmiar (skalowanie)
|
|
(0, 0, 255), # kolor (BGR) - tutaj czerwony
|
|
2, # grubość linii
|
|
cv2.LINE_AA # typ antyaliasingu
|
|
)
|
|
|
|
cv2.imshow('Live Pose Estimation', vis_img_resized)
|
|
|
|
# press 'q' to quit
|
|
if cv2.waitKey(1) & 0xFF == ord('q'):
|
|
break
|
|
|
|
elapsed = time.time() - start_time
|
|
sleep_time = frame_time - elapsed
|
|
if sleep_time > 0:
|
|
time.sleep(sleep_time)
|
|
except KeyboardInterrupt:
|
|
print(moves)
|
|
|
|
cap.release()
|
|
cv2.destroyAllWindows()
|
|
|
|
|
|
if __name__ == '__main__':
|
|
main()
|