# 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()