1104 lines
43 KiB
Python
1104 lines
43 KiB
Python
import cv2
|
|
import numpy as np
|
|
import socket
|
|
import threading
|
|
from ultralytics import YOLO
|
|
from pythonosc.dispatcher import Dispatcher
|
|
from pythonosc.osc_server import ThreadingOSCUDPServer
|
|
from pythonosc.udp_client import SimpleUDPClient
|
|
from gesture_runtime import GestureRecognizer
|
|
|
|
MODEL_CANDIDATES = [
|
|
"yolo11n-pose.pt", # current Ultralytics small pose model
|
|
"yolov8n-pose.pt", # older fallback
|
|
]
|
|
CAMERA_INDEX = 0
|
|
VIDEO = r"C:\Users\enjaf\Documents\Michi & Konzi\Uni\6.Sem\Interaktion\AI_Interaktion_Python_Code_002\Interaktion\vid2.mp4"
|
|
|
|
OSC_TARGET_IP = "100.83.253.33"
|
|
|
|
#VIDEO_INPUT = VIDEO
|
|
VIDEO_INPUT = CAMERA_INDEX
|
|
CONF_THRESHOLD = 0.35
|
|
IOU_THRESHOLD = 0.50
|
|
KEYPOINT_CONF_THRESHOLD = 0.35
|
|
PERSON_PADDING_RATIO = 0.15
|
|
|
|
GESTURE_DATA_PICKLE = "gesture_samples.pkl"
|
|
GESTURE_ANALYSIS_PICKLE = "gesture_feature_analysis.pkl"
|
|
GESTURE_FEATURE_FAMILY = "ANGLES"
|
|
GESTURE_CLASSIFIER_TYPE = "knn"
|
|
GESTURE_RANKING_METHOD = "fisher"
|
|
GESTURE_NUM_FEATURES = 10
|
|
GESTURE_UNKNOWN_THRESHOLD = 0.7
|
|
GESTURE_MODEL_PATH = "hand_landmarker.task"
|
|
|
|
OSC_RECEIVER_IP = "0.0.0.0"
|
|
OSC_RECEIVER_PORT = 9000
|
|
OSC_TARGET_PORT = 9000
|
|
|
|
# COCO 17-keypoint skeleton used by YOLO human pose models
|
|
POSE_CONNECTIONS = [
|
|
(0, 1), (0, 2), (1, 3), (2, 4), # face
|
|
(5, 6), # shoulders
|
|
(5, 7), (7, 9), # left arm
|
|
(6, 8), (8, 10), # right arm
|
|
(5, 11), (6, 12), (11, 12), # torso
|
|
(11, 13), (13, 15), # left leg
|
|
(12, 14), (14, 16), # right leg
|
|
]
|
|
|
|
persons_of_interest = set() # Set, um die Indizes der interessanten Personen zu speichern
|
|
persons_of_interest_lock = threading.Lock()
|
|
|
|
target_persons = set() # Set mit Personen, auf die gezeigt werden kann
|
|
target_persons_lock = threading.Lock()
|
|
|
|
LEFT_SHOULDER = 5
|
|
RIGHT_SHOULDER = 6
|
|
LEFT_HIP = 11
|
|
RIGHT_HIP = 12
|
|
|
|
ARUCO_ASSIGN_DISTANCE_THRESHOLD = 840.0
|
|
ARUCO_SLEEP_MISSING_FRAMES = 3
|
|
POINTING_STABLE_FRAMES = 6
|
|
|
|
aruco_person_assignments = {}
|
|
aruco_person_missing_counts = {}
|
|
aruco_person_sleeping = {}
|
|
aruco_state_lock = threading.Lock()
|
|
|
|
# When True, do not auto-reassign ArUco markers during normal frame updates.
|
|
# Calling `/getAllIds` will perform a fresh assignment and set this to True.
|
|
aruco_mapping_locked = False
|
|
|
|
# Latest per-frame snapshots used by the `/getAllIds` handler to create a mapping
|
|
# from currently visible persons to markers when the game starts.
|
|
latest_marker_centers = {}
|
|
latest_person_centers = {}
|
|
|
|
pointing_target_tracker = {}
|
|
pointing_target_lock = threading.Lock()
|
|
osc_client_global = None
|
|
|
|
osc_status_enabled = True
|
|
|
|
# Confirmation wait state: set by OSC `on_confirmation` to a set of player track_ids to listen to.
|
|
# When all players show matching gestures (B or C), we send the result.
|
|
confirmation_poi_ids = set()
|
|
confirmation_gestures = {} # Maps track_id -> gesture_label (e.g., "A", "B", "C")
|
|
confirmation_stable_counter = 0
|
|
confirmation_last_consensus = None
|
|
confirmation_lock = threading.Lock()
|
|
|
|
|
|
def get_local_ip() -> str:
|
|
s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
|
try:
|
|
s.connect(("8.8.8.8", 80))
|
|
ip = s.getsockname()[0]
|
|
except OSError:
|
|
ip = "127.0.0.1"
|
|
finally:
|
|
s.close()
|
|
return ip
|
|
|
|
|
|
def toggle_person_of_interest(index):
|
|
with persons_of_interest_lock:
|
|
if index in persons_of_interest:
|
|
persons_of_interest.remove(index)
|
|
return False
|
|
persons_of_interest.add(index)
|
|
return True
|
|
|
|
|
|
def clear_persons_of_interest():
|
|
with persons_of_interest_lock:
|
|
persons_of_interest.clear()
|
|
|
|
|
|
def get_persons_of_interest_snapshot():
|
|
with persons_of_interest_lock:
|
|
return sorted(persons_of_interest)
|
|
|
|
|
|
def toggle_target_person(index):
|
|
with target_persons_lock:
|
|
if index in target_persons:
|
|
target_persons.remove(index)
|
|
return False
|
|
target_persons.add(index)
|
|
return True
|
|
|
|
|
|
def clear_target_persons():
|
|
with target_persons_lock:
|
|
target_persons.clear()
|
|
|
|
|
|
def add_all_target_persons(ids):
|
|
"""Initialize target_persons with the provided list of track IDs."""
|
|
with target_persons_lock:
|
|
target_persons.clear()
|
|
if ids is not None:
|
|
for tid in ids:
|
|
target_persons.add(int(tid))
|
|
|
|
|
|
def get_target_persons_snapshot():
|
|
with target_persons_lock:
|
|
return sorted(target_persons)
|
|
|
|
|
|
def parse_osc_id_list(value):
|
|
if isinstance(value, str):
|
|
raw_items = [item.strip() for item in value.split(",") if item.strip()]
|
|
elif isinstance(value, (list, tuple, set)):
|
|
raw_items = list(value)
|
|
elif hasattr(value, "__iter__") and not isinstance(value, (bytes, bytearray)):
|
|
raw_items = list(value)
|
|
else:
|
|
raw_items = [value]
|
|
|
|
if len(raw_items) == 1 and isinstance(raw_items[0], (list, tuple, set)):
|
|
raw_items = list(raw_items[0])
|
|
|
|
parsed_items = []
|
|
for item in raw_items:
|
|
try:
|
|
parsed_items.append(int(item))
|
|
except (TypeError, ValueError):
|
|
print(f"Ignoring non-numeric OSC id: {item!r}")
|
|
return parsed_items
|
|
|
|
|
|
def get_aruco_id_for_track_id(track_id):
|
|
with aruco_state_lock:
|
|
return aruco_person_assignments.get(track_id)
|
|
|
|
|
|
def get_track_id_for_aruco_id(aruco_id):
|
|
with aruco_state_lock:
|
|
for track_id, assigned_aruco_id in aruco_person_assignments.items():
|
|
if assigned_aruco_id == aruco_id:
|
|
return track_id
|
|
return None
|
|
|
|
|
|
def translate_aruco_ids_to_track_ids(aruco_ids):
|
|
track_ids = []
|
|
for aruco_id in aruco_ids:
|
|
track_id = get_track_id_for_aruco_id(aruco_id)
|
|
if track_id is None:
|
|
print(f"No internal track found for ArUco id: {aruco_id}")
|
|
continue
|
|
track_ids.append(track_id)
|
|
return track_ids
|
|
|
|
|
|
def translate_track_ids_to_aruco_ids(track_ids):
|
|
aruco_ids = []
|
|
for track_id in track_ids:
|
|
aruco_id = get_aruco_id_for_track_id(track_id)
|
|
if aruco_id is None:
|
|
continue
|
|
aruco_ids.append(int(aruco_id))
|
|
return sorted(set(aruco_ids))
|
|
|
|
|
|
def get_all_assigned_aruco_ids():
|
|
with aruco_state_lock:
|
|
return sorted({int(aruco_id) for aruco_id in aruco_person_assignments.values() if aruco_id is not None})
|
|
|
|
|
|
def get_box_center(box):
|
|
x1, y1, x2, y2 = [float(v) for v in box]
|
|
return (0.5 * (x1 + x2), 0.5 * (y1 + y2))
|
|
|
|
|
|
def distance_xy(point_a, point_b):
|
|
return ((point_a[0] - point_b[0]) ** 2 + (point_a[1] - point_b[1]) ** 2) ** 0.5
|
|
|
|
|
|
def scale_aruco_corners(marker_corners, scale_x, scale_y):
|
|
scaled_corners = []
|
|
for x, y in marker_corners:
|
|
scaled_corners.append((float(x) * scale_x, float(y) * scale_y))
|
|
return scaled_corners
|
|
|
|
|
|
def get_marker_center(marker_corners):
|
|
center_x = sum(point[0] for point in marker_corners) / len(marker_corners)
|
|
center_y = sum(point[1] for point in marker_corners) / len(marker_corners)
|
|
return center_x, center_y
|
|
|
|
|
|
def cleanup_missing_person_aruco_state(active_person_keys):
|
|
with aruco_state_lock:
|
|
for person_key in list(aruco_person_assignments.keys()):
|
|
if person_key not in active_person_keys:
|
|
aruco_person_assignments.pop(person_key, None)
|
|
aruco_person_missing_counts.pop(person_key, None)
|
|
aruco_person_sleeping.pop(person_key, None)
|
|
|
|
|
|
def update_person_aruco_state(person_key, person_center, current_marker_centers, claimed_marker_ids):
|
|
with aruco_state_lock:
|
|
assigned_marker_id = aruco_person_assignments.get(person_key)
|
|
missing_count = aruco_person_missing_counts.get(person_key, 0)
|
|
sleeping = aruco_person_sleeping.get(person_key, False)
|
|
|
|
# If mapping is locked (game started via /getAllIds), do not create new assignments
|
|
# here — only update presence/missing counters for the existing assignment.
|
|
if aruco_mapping_locked:
|
|
if assigned_marker_id is not None and assigned_marker_id in current_marker_centers:
|
|
claimed_marker_ids.add(assigned_marker_id)
|
|
missing_count = 0
|
|
sleeping = False
|
|
else:
|
|
missing_count += 1
|
|
sleeping = missing_count >= ARUCO_SLEEP_MISSING_FRAMES
|
|
else:
|
|
if assigned_marker_id is not None and assigned_marker_id in current_marker_centers:
|
|
claimed_marker_ids.add(assigned_marker_id)
|
|
missing_count = 0
|
|
sleeping = False
|
|
elif assigned_marker_id is None and person_center is not None and current_marker_centers:
|
|
best_marker_id = None
|
|
best_marker_distance = None
|
|
for marker_id, marker_center in current_marker_centers.items():
|
|
if marker_id in claimed_marker_ids:
|
|
continue
|
|
marker_distance = distance_xy(person_center, marker_center)
|
|
if best_marker_distance is None or marker_distance < best_marker_distance:
|
|
best_marker_distance = marker_distance
|
|
best_marker_id = marker_id
|
|
|
|
if best_marker_id is not None and best_marker_distance is not None and best_marker_distance <= ARUCO_ASSIGN_DISTANCE_THRESHOLD:
|
|
assigned_marker_id = best_marker_id
|
|
claimed_marker_ids.add(best_marker_id)
|
|
missing_count = 0
|
|
sleeping = False
|
|
else:
|
|
missing_count += 1
|
|
sleeping = missing_count >= ARUCO_SLEEP_MISSING_FRAMES
|
|
else:
|
|
missing_count += 1
|
|
sleeping = missing_count >= ARUCO_SLEEP_MISSING_FRAMES
|
|
|
|
aruco_person_assignments[person_key] = assigned_marker_id
|
|
aruco_person_missing_counts[person_key] = missing_count
|
|
aruco_person_sleeping[person_key] = sleeping
|
|
|
|
return assigned_marker_id, sleeping, missing_count
|
|
|
|
|
|
def set_osc_status_enabled(enabled):
|
|
global osc_status_enabled
|
|
osc_status_enabled = bool(enabled)
|
|
print(f"OSC status sending {'enabled' if osc_status_enabled else 'disabled'}")
|
|
|
|
|
|
def print_osc_message(prefix, address, *args):
|
|
print(f"{prefix} address={address} args={args}")
|
|
|
|
|
|
|
|
def on_werwolf_poi_toggle(address, *args):
|
|
print_osc_message("OSC poi/toggle", address, *args)
|
|
for track_id in translate_aruco_ids_to_track_ids(parse_osc_id_list(args)):
|
|
toggle_person_of_interest(track_id)
|
|
|
|
|
|
def on_getAllIds(address, *args):
|
|
print_osc_message("OSC poi/getAllIds", address, *args)
|
|
# Start the game: perform a one-time assignment of currently visible ArUco markers
|
|
# to detected person track IDs. If called again, reassign based on the latest
|
|
# frame snapshot. After assignment we lock automatic reassignment until
|
|
# `/getAllIds` is called again (which will reassign).
|
|
global aruco_mapping_locked
|
|
try:
|
|
with aruco_state_lock:
|
|
# Build fresh assignments using the latest per-frame snapshots
|
|
if not latest_person_centers:
|
|
print("on_getAllIds: no person snapshot available to assign")
|
|
new_assignments = {}
|
|
claimed = set()
|
|
for person_key in sorted(latest_person_centers.keys()):
|
|
best_marker_id = None
|
|
best_marker_distance = None
|
|
person_center = latest_person_centers.get(person_key)
|
|
for marker_id, marker_center in latest_marker_centers.items():
|
|
if marker_id in claimed:
|
|
continue
|
|
if person_center is None:
|
|
continue
|
|
d = distance_xy(person_center, marker_center)
|
|
if best_marker_distance is None or d < best_marker_distance:
|
|
best_marker_distance = d
|
|
best_marker_id = marker_id
|
|
|
|
if best_marker_id is not None and best_marker_distance is not None and best_marker_distance <= ARUCO_ASSIGN_DISTANCE_THRESHOLD:
|
|
new_assignments[person_key] = int(best_marker_id)
|
|
claimed.add(best_marker_id)
|
|
print("Assigned person to ArUco ")
|
|
else:
|
|
new_assignments[person_key] = None
|
|
print("No suitable ArUco marker found for person (closest distance if any)")
|
|
|
|
# Replace current assignments with the new mapping
|
|
aruco_person_assignments.clear()
|
|
aruco_person_assignments.update(new_assignments)
|
|
# Reset missing counts and sleeping flags for assigned persons
|
|
for pk in list(aruco_person_assignments.keys()):
|
|
aruco_person_missing_counts[pk] = 0
|
|
aruco_person_sleeping[pk] = False
|
|
|
|
aruco_mapping_locked = True
|
|
|
|
# Reply with the assigned ArUco IDs
|
|
if 'osc_client_global' in globals() and osc_client_global is not None:
|
|
osc_client_global.send_message('/getAllIds', get_all_assigned_aruco_ids())
|
|
print("Sent getAllIds reply with assigned ArUco IDs:", get_all_assigned_aruco_ids())
|
|
else:
|
|
print("No OSC client available to send getAllIds reply")
|
|
except Exception as e:
|
|
print(f"Failed to assign/send getAllIds reply: {e}")
|
|
|
|
|
|
def on_everyoneAsleep(address, *args):
|
|
print_osc_message("OSC everyoneAsleep", address, *args)
|
|
# Return True if all players are asleep, otherwise return array of ArUco IDs of awake players
|
|
with aruco_state_lock:
|
|
awake_aruco_ids = []
|
|
for pid in aruco_person_assignments.keys():
|
|
if not aruco_person_sleeping.get(pid, False):
|
|
aruco_id = aruco_person_assignments.get(pid)
|
|
if aruco_id is not None:
|
|
awake_aruco_ids.append(int(aruco_id))
|
|
|
|
# If no awake players, everyone is asleep
|
|
if not awake_aruco_ids:
|
|
result = True
|
|
else:
|
|
result = sorted(awake_aruco_ids)
|
|
|
|
print(f"everyoneAsleep -> {result}")
|
|
|
|
# Send result back via OSC
|
|
try:
|
|
if 'osc_client_global' in globals() and osc_client_global is not None:
|
|
osc_client_global.send_message('/everyoneAsleep', result)
|
|
else:
|
|
print("No OSC client available to send everyoneAsleep reply")
|
|
except Exception as e:
|
|
print(f"Failed to send everyoneAsleep reply: {e}")
|
|
|
|
|
|
def on_confirmation(address, *args):
|
|
print_osc_message("OSC confirmation", address, *args)
|
|
aruco_ids = parse_osc_id_list(args)
|
|
if not aruco_ids:
|
|
print("on_confirmation: no aruco ids")
|
|
return
|
|
|
|
# Translate all ArUco IDs to internal track IDs
|
|
track_ids = translate_aruco_ids_to_track_ids(aruco_ids)
|
|
if not track_ids:
|
|
print(f"on_confirmation: no internal track found for any aruco ids {aruco_ids}")
|
|
return
|
|
|
|
# Add all players to POIs so gesture detection runs for them
|
|
clear_persons_of_interest()
|
|
for tid in track_ids:
|
|
toggle_person_of_interest(tid)
|
|
|
|
# Set up confirmation wait state
|
|
with confirmation_lock:
|
|
confirmation_poi_ids.clear()
|
|
confirmation_poi_ids.update(track_ids)
|
|
confirmation_gestures.clear()
|
|
confirmation_stable_counter = 0
|
|
confirmation_last_consensus = None
|
|
|
|
print(f"Waiting for confirmation gestures from POI players: {track_ids} (aruco {aruco_ids})")
|
|
|
|
|
|
def on_getPlayerID(address, *args):
|
|
print_osc_message("OSC getPlayerID", address, *args)
|
|
if len(args) < 4:
|
|
print(f"ERROR: getPlayerID expects at least 4 args, got {len(args)}")
|
|
return
|
|
try:
|
|
num_pois = int(args[0])
|
|
poi_aruco_ids = args[1 : 1 + num_pois]
|
|
|
|
idx_num_targets = 1 + num_pois
|
|
num_targets = int(args[idx_num_targets])
|
|
target_aruco_ids = args[idx_num_targets + 1 : idx_num_targets + 1 + num_targets]
|
|
|
|
poi_list = translate_aruco_ids_to_track_ids(poi_aruco_ids)
|
|
target_list = translate_aruco_ids_to_track_ids(target_aruco_ids)
|
|
|
|
clear_persons_of_interest()
|
|
for poi_id in poi_list:
|
|
toggle_person_of_interest(poi_id)
|
|
|
|
clear_target_persons()
|
|
with target_persons_lock:
|
|
for target_id in target_list:
|
|
target_persons.add(target_id)
|
|
|
|
with pointing_target_lock:
|
|
pointing_target_tracker.clear()
|
|
|
|
print(f"Updated POI tracks: {poi_list}, Targets tracks: {target_list}")
|
|
except Exception as e:
|
|
print(f"ERROR in on_getPlayerID: {e}")
|
|
|
|
|
|
def on_osc_unknown(address, *args):
|
|
print_osc_message("OSC unknown", address, *args)
|
|
|
|
|
|
def start_osc_bridge():
|
|
global osc_client_global
|
|
local_ip = get_local_ip()
|
|
print(f"Local IP address: {local_ip}")
|
|
print(f"OSC receiver listening on {OSC_RECEIVER_IP}:{OSC_RECEIVER_PORT}")
|
|
print(f"OSC sender target: {OSC_TARGET_IP}:{OSC_TARGET_PORT}")
|
|
|
|
dispatcher = Dispatcher()
|
|
dispatcher.map("/getPlayerID", on_getPlayerID)
|
|
dispatcher.map("/confirmation", on_confirmation)
|
|
dispatcher.map("/everyoneAsleep", on_everyoneAsleep)
|
|
dispatcher.map("/getAllIds", on_getAllIds)
|
|
dispatcher.set_default_handler(on_osc_unknown)
|
|
|
|
server = ThreadingOSCUDPServer((OSC_RECEIVER_IP, OSC_RECEIVER_PORT), dispatcher)
|
|
thread = threading.Thread(target=server.serve_forever, daemon=True)
|
|
thread.start()
|
|
|
|
client = SimpleUDPClient(OSC_TARGET_IP, OSC_TARGET_PORT)
|
|
osc_client_global = client
|
|
return server, client, local_ip
|
|
|
|
|
|
def find_closest_target_person(ray_x, ray_y, ray_dx, ray_dy, person_positions, target_ids):
|
|
"""
|
|
Findet die Zielperson mit der geringsten Winkelabweichung zum Zeigestrahl.
|
|
"""
|
|
from math import acos, degrees, sqrt
|
|
|
|
if not target_ids or not person_positions:
|
|
return None, float('inf')
|
|
|
|
min_angle = float('inf')
|
|
closest_target_id = None
|
|
|
|
ray_len = sqrt(ray_dx**2 + ray_dy**2)
|
|
if ray_len < 1e-6:
|
|
return None, float('inf')
|
|
|
|
for target_id in target_ids:
|
|
if target_id not in person_positions:
|
|
continue
|
|
|
|
person_x, person_y = person_positions[target_id]
|
|
to_person_x = person_x - ray_x
|
|
to_person_y = person_y - ray_y
|
|
dist_to_person = sqrt(to_person_x**2 + to_person_y**2)
|
|
|
|
if dist_to_person < 1e-6:
|
|
continue
|
|
|
|
dot = (ray_dx * to_person_x + ray_dy * to_person_y)
|
|
cos_theta = max(-1.0, min(1.0, dot / (ray_len * dist_to_person)))
|
|
angle = degrees(acos(cos_theta))
|
|
|
|
# Nur Personen "vor" der Hand berücksichtigen
|
|
if dot > 0 and angle < min_angle:
|
|
min_angle = angle
|
|
closest_target_id = target_id
|
|
|
|
return closest_target_id, min_angle
|
|
|
|
|
|
def send_osc_status(client, person_count, selected_poi_ids, selected_detection):
|
|
if client is None or not osc_status_enabled:
|
|
return
|
|
|
|
client.send_message("/werwolf/status/person_count", int(person_count))
|
|
client.send_message("/werwolf/status/poi_ids", ",".join(str(person_id) for person_id in translate_track_ids_to_aruco_ids(selected_poi_ids)))
|
|
|
|
target_ids = get_target_persons_snapshot()
|
|
client.send_message("/werwolf/status/target_ids", ",".join(str(person_id) for person_id in translate_track_ids_to_aruco_ids(target_ids)))
|
|
|
|
if selected_detection is None:
|
|
return
|
|
|
|
track_id = selected_detection.get("track_id")
|
|
aruco_id = selected_detection.get("aruco_id")
|
|
if aruco_id is None and track_id is not None:
|
|
aruco_id = get_aruco_id_for_track_id(track_id)
|
|
label = selected_detection.get("label", "unknown")
|
|
confidence = float(selected_detection.get("confidence", 0.0))
|
|
handedness = selected_detection.get("handedness", "")
|
|
client.send_message(
|
|
"/werwolf/status/gesture",
|
|
[int(aruco_id) if aruco_id is not None else -1, label, confidence, handedness],
|
|
)
|
|
|
|
|
|
def compute_pointing_ray_from_hand(hand_landmarks, crop_x, crop_y, crop_w, crop_h):
|
|
"""Extract start point and direction vector from hand landmarks (index finger).
|
|
Returns (start_x, start_y, direction_x, direction_y) in full-frame coordinates."""
|
|
from gesture_runtime import INDEX_TIP, INDEX_MCP, landmark_to_pixel
|
|
|
|
index_tip_x, index_tip_y = landmark_to_pixel(hand_landmarks[INDEX_TIP], crop_w, crop_h)
|
|
index_mcp_x, index_mcp_y = landmark_to_pixel(hand_landmarks[INDEX_MCP], crop_w, crop_h)
|
|
|
|
start_x = index_tip_x + crop_x
|
|
start_y = index_tip_y + crop_y
|
|
direction_x = index_tip_x - index_mcp_x
|
|
direction_y = index_tip_y - index_mcp_y
|
|
|
|
return start_x, start_y, direction_x, direction_y
|
|
|
|
|
|
def distance_point_to_ray(point_x, point_y, ray_start_x, ray_start_y, ray_dir_x, ray_dir_y):
|
|
"""
|
|
Compute distance from point to an infinite line defined by ray_start and ray_dir.
|
|
Returns (distance, t) where t is the projection parameter along the ray direction.
|
|
"""
|
|
dx = point_x - ray_start_x
|
|
dy = point_y - ray_start_y
|
|
|
|
ray_len_sq = ray_dir_x * ray_dir_x + ray_dir_y * ray_dir_y
|
|
if ray_len_sq < 1e-6:
|
|
return float('inf'), 0.0
|
|
|
|
t = (dx * ray_dir_x + dy * ray_dir_y) / ray_len_sq
|
|
|
|
closest_x = ray_start_x + t * ray_dir_x
|
|
closest_y = ray_start_y + t * ray_dir_y
|
|
|
|
dist = ((point_x - closest_x) ** 2 + (point_y - closest_y) ** 2) ** 0.5
|
|
return dist, t
|
|
|
|
|
|
def expand_box(box, frame_width, frame_height, padding_ratio):
|
|
x1, y1, x2, y2 = [float(v) for v in box]
|
|
cx = (x1 + x2) / 2
|
|
cy = (y1 + y2) / 2
|
|
w = x2 - x1
|
|
h = y2 - y1
|
|
|
|
# Erstelle ein Quadrat (Square ROI) basierend auf der längeren Seite
|
|
side = max(w, h) * (1.0 + padding_ratio)
|
|
|
|
x1 = max(0, int(cx - side / 2))
|
|
y1 = max(0, int(cy - side / 2))
|
|
x2 = min(frame_width, int(cx + side / 2))
|
|
y2 = min(frame_height, int(cy + side / 2))
|
|
|
|
|
|
return x1, y1, x2, y2
|
|
|
|
|
|
def load_model():
|
|
last_error = None
|
|
for model_path in MODEL_CANDIDATES:
|
|
try:
|
|
model = YOLO(model_path)
|
|
return model, model_path
|
|
except Exception as exc:
|
|
last_error = exc
|
|
raise RuntimeError(f"Could not load any pose model: {last_error}")
|
|
|
|
|
|
def draw_person(frame, box, track_id, det_conf):
|
|
x1, y1, x2, y2 = [int(v) for v in box]
|
|
|
|
# Bounding box
|
|
cv2.rectangle(frame, (x1, y1), (x2, y2), (0, 180, 255), 2)
|
|
|
|
# Skeleton lines
|
|
#for a, b in POSE_CONNECTIONS:
|
|
# if kp_conf is not None:
|
|
# if kp_conf[a] < KEYPOINT_CONF_THRESHOLD or kp_conf[b] < KEYPOINT_CONF_THRESHOLD:
|
|
# continue
|
|
# xa, ya = int(xy[a][0]), int(xy[a][1])
|
|
# xb, yb = int(xy[b][0]), int(xy[b][1])
|
|
# cv2.line(frame, (xa, ya), (xb, yb), (0, 255, 0), 2)
|
|
|
|
# Keypoints
|
|
#for i, (x, y) in enumerate(xy):
|
|
# if kp_conf is not None and kp_conf[i] < KEYPOINT_CONF_THRESHOLD:
|
|
# continue
|
|
# cv2.circle(frame, (int(x), int(y)), 4, (0, 0, 255), -1)
|
|
|
|
# Center point from bounding box
|
|
cx = int((x1 + x2) / 2)
|
|
cy = int((y1 + y2) / 2)
|
|
cv2.circle(frame, (cx, cy), 4, (255, 255, 0), -1)
|
|
|
|
# Display ID in large text at the top
|
|
if track_id is not None:
|
|
id_label = f"ID: {track_id}"
|
|
cv2.putText(
|
|
frame,
|
|
id_label,
|
|
(x1, max(20, y1 - 30)),
|
|
cv2.FONT_HERSHEY_SIMPLEX,
|
|
0.8,
|
|
(0, 255, 255),
|
|
3,
|
|
cv2.LINE_AA,
|
|
)
|
|
|
|
# Display confidence below
|
|
conf_label = f"conf: {det_conf:.2f}"
|
|
cv2.putText(
|
|
frame,
|
|
conf_label,
|
|
(x1, max(20, y1 - 10)),
|
|
cv2.FONT_HERSHEY_SIMPLEX,
|
|
0.6,
|
|
(0, 255, 255),
|
|
2,
|
|
cv2.LINE_AA,
|
|
)
|
|
|
|
|
|
def get_point_xy(keypoints_xy, keypoints_conf, index):
|
|
if keypoints_xy is None or index >= len(keypoints_xy):
|
|
return None
|
|
if keypoints_conf is not None and index < len(keypoints_conf) and keypoints_conf[index] < KEYPOINT_CONF_THRESHOLD:
|
|
return None
|
|
point = keypoints_xy[index]
|
|
return float(point[0]), float(point[1])
|
|
|
|
|
|
def get_body_center(keypoints_xy, keypoints_conf):
|
|
points = []
|
|
for index in (LEFT_SHOULDER, RIGHT_SHOULDER, LEFT_HIP, RIGHT_HIP):
|
|
point = get_point_xy(keypoints_xy, keypoints_conf, index)
|
|
if point is not None:
|
|
points.append(point)
|
|
|
|
if points:
|
|
xs = [point[0] for point in points]
|
|
ys = [point[1] for point in points]
|
|
return (sum(xs) / len(xs), sum(ys) / len(ys))
|
|
|
|
center_point = get_point_xy(keypoints_xy, keypoints_conf, 0)
|
|
return center_point
|
|
|
|
|
|
def select_farthest_hand(detections, crop_x, crop_y, body_center):
|
|
if not detections:
|
|
return None
|
|
if body_center is None:
|
|
return detections[0]
|
|
|
|
best_detection = None
|
|
best_distance = -1.0
|
|
|
|
for detection in detections:
|
|
hand_landmarks = detection["hand_landmarks"]
|
|
wrist = hand_landmarks[0]
|
|
wrist_x = crop_x + float(wrist.x * detection["crop_width"])
|
|
wrist_y = crop_y + float(wrist.y * detection["crop_height"])
|
|
distance = ((wrist_x - body_center[0]) ** 2 + (wrist_y - body_center[1]) ** 2) ** 0.5
|
|
|
|
if distance > best_distance:
|
|
best_distance = distance
|
|
best_detection = detection
|
|
|
|
return best_detection
|
|
|
|
|
|
def main():
|
|
model, model_name = load_model()
|
|
|
|
cap = cv2.VideoCapture(VIDEO_INPUT)
|
|
if not cap.isOpened():
|
|
raise RuntimeError("Could not open webcam")
|
|
|
|
print(f"Using model: {model_name}")
|
|
print("Press q to quit.")
|
|
|
|
gesture = None
|
|
osc_server = None
|
|
osc_client = None
|
|
try:
|
|
osc_server, osc_client, _local_ip = start_osc_bridge()
|
|
gesture = GestureRecognizer(
|
|
data_pickle=GESTURE_DATA_PICKLE,
|
|
analysis_pickle=GESTURE_ANALYSIS_PICKLE,
|
|
feature_family=GESTURE_FEATURE_FAMILY,
|
|
classifier_type=GESTURE_CLASSIFIER_TYPE,
|
|
ranking_method=GESTURE_RANKING_METHOD,
|
|
num_features=GESTURE_NUM_FEATURES,
|
|
unknown_threshold=GESTURE_UNKNOWN_THRESHOLD,
|
|
hand_model_path=GESTURE_MODEL_PATH,
|
|
)
|
|
frame_count = 0
|
|
# ArUco marker detection setup (OpenCV 4.7+)
|
|
arucoDict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)
|
|
arucoParams = cv2.aruco.DetectorParameters()
|
|
detector = cv2.aruco.ArucoDetector(arucoDict, arucoParams)
|
|
while True:
|
|
ok, frame_orig = cap.read()
|
|
if not ok:
|
|
print("Failed to read frame")
|
|
break
|
|
orig_height, orig_width = frame_orig.shape[:2]
|
|
frame = cv2.resize(frame_orig, (980, 540))
|
|
#frame = frame_orig
|
|
#frame_count += 1
|
|
|
|
# Entfernt: Das Drosseln auf jeden 6. Frame macht die Erkennung zu instabil
|
|
# if frame_count % 6 != 0:
|
|
# continue
|
|
frame_height, frame_width = frame.shape[:2]
|
|
scale_x = frame_width / float(orig_width)
|
|
scale_y = frame_height / float(orig_height)
|
|
corners, ids, rejected = detector.detectMarkers(frame_orig)
|
|
|
|
# Track mode helps keep person IDs stable over time.
|
|
results = model.track(
|
|
frame,
|
|
conf=CONF_THRESHOLD,
|
|
iou=IOU_THRESHOLD,
|
|
classes=[0], # person
|
|
persist=True,
|
|
verbose=False,
|
|
)
|
|
|
|
display = frame.copy()
|
|
person_count = 0
|
|
selected_detection = None
|
|
person_positions = {}
|
|
selected_pointing_target_id = None
|
|
current_frame_pointing_targets = set()
|
|
person_centers = {}
|
|
|
|
# Visualize detected ArUco markers
|
|
current_marker_centers = {}
|
|
if ids is not None and len(ids) > 0:
|
|
for i, marker_id in enumerate(ids):
|
|
marker_corners = corners[i][0]
|
|
marker_id_value = int(marker_id[0])
|
|
marker_corners_scaled = scale_aruco_corners(marker_corners, scale_x, scale_y)
|
|
center_x, center_y = get_marker_center(marker_corners_scaled)
|
|
current_marker_centers[marker_id_value] = (center_x, center_y)
|
|
|
|
# Draw marker rectangle
|
|
pts = np.array(marker_corners_scaled, dtype=int)
|
|
cv2.polylines(display, [pts], True, (255, 0, 255), 2)
|
|
|
|
# Draw center point
|
|
cv2.circle(display, (int(center_x), int(center_y)), 6, (0, 255, 255), -1)
|
|
|
|
# Draw marker ID
|
|
marker_text = f"M:{marker_id_value}"
|
|
cv2.putText(
|
|
display,
|
|
marker_text,
|
|
(int(center_x) - 30, int(center_y) - 15),
|
|
cv2.FONT_HERSHEY_SIMPLEX,
|
|
0.7,
|
|
(255, 200, 0),
|
|
2,
|
|
cv2.LINE_AA,
|
|
)
|
|
|
|
claimed_marker_ids = set()
|
|
|
|
if results:
|
|
result = results[0].cpu().numpy()
|
|
|
|
if result.boxes is not None and result.keypoints is not None:
|
|
# Reset per-frame gesture observations for confirmation logic
|
|
with confirmation_lock:
|
|
confirmation_gestures.clear()
|
|
|
|
boxes_xyxy = result.boxes.xyxy
|
|
track_ids = result.boxes.id if result.boxes.id is not None else None
|
|
keypoints_xy = result.keypoints.xy
|
|
keypoints_conf = result.keypoints.conf
|
|
|
|
person_count = len(boxes_xyxy)
|
|
# Entfernt: Überschreibt sonst die via OSC gesetzten Targets
|
|
# if len(get_target_persons_snapshot()) == 0:
|
|
# add_all_target_persons(track_ids)
|
|
|
|
persons_of_interest_current = set(get_persons_of_interest_snapshot())
|
|
|
|
for i in range(person_count):
|
|
track_id = None
|
|
if track_ids is not None:
|
|
track_id = int(track_ids[i])
|
|
|
|
kp_conf = None
|
|
if keypoints_conf is not None:
|
|
kp_conf = keypoints_conf[i]
|
|
|
|
# Draw person with ID
|
|
det_conf = float(result.boxes.conf[i]) if result.boxes.conf is not None else 0.0
|
|
draw_person(
|
|
display,
|
|
boxes_xyxy[i],
|
|
track_id,
|
|
det_conf,
|
|
)
|
|
x1, y1, x2, y2 = expand_box(
|
|
boxes_xyxy[i],
|
|
frame_width,
|
|
frame_height,
|
|
PERSON_PADDING_RATIO,
|
|
)
|
|
crop = frame[y1:y2, x1:x2]
|
|
if crop.size == 0:
|
|
continue
|
|
|
|
body_center = get_body_center(keypoints_xy[i], kp_conf)
|
|
if body_center is not None and track_id is not None:
|
|
person_positions[track_id] = body_center
|
|
person_centers[track_id] = body_center
|
|
|
|
if track_id is not None:
|
|
assigned_marker_id, sleeping, missing_count = update_person_aruco_state(
|
|
track_id,
|
|
body_center,
|
|
current_marker_centers,
|
|
claimed_marker_ids,
|
|
)
|
|
else:
|
|
assigned_marker_id = None
|
|
sleeping = False
|
|
missing_count = 0
|
|
|
|
if sleeping:
|
|
cx, cy = get_box_center(boxes_xyxy[i])
|
|
radius = 10
|
|
cv2.circle(display, (int(cx), int(cy)), radius, (255, 0, 0), -1)
|
|
cv2.putText(
|
|
display,
|
|
f"Sleep | A:{assigned_marker_id if assigned_marker_id is not None else '-'}",
|
|
(int(cx) - radius, int(cy) - radius - 10),
|
|
cv2.FONT_HERSHEY_SIMPLEX,
|
|
0.7,
|
|
(255, 255, 255),
|
|
2,
|
|
cv2.LINE_AA,
|
|
)
|
|
|
|
# Einrückung korrigiert: Diese Anzeige muss pro Person in der Schleife erfolgen
|
|
if assigned_marker_id is not None and body_center is not None:
|
|
cv2.putText(
|
|
display,
|
|
f"Aruco: {assigned_marker_id}",
|
|
(x1, max(20, y2 + 20)),
|
|
cv2.FONT_HERSHEY_SIMPLEX,
|
|
0.6,
|
|
(0, 255, 255),
|
|
2,
|
|
cv2.LINE_AA,
|
|
)
|
|
|
|
if track_id is not None and (int(track_id) in persons_of_interest_current or int(track_id) in confirmation_poi_ids):
|
|
detections = gesture.detect_gestures(crop)
|
|
|
|
for detection in detections:
|
|
detection["crop_width"] = crop.shape[1]
|
|
detection["crop_height"] = crop.shape[0]
|
|
detection["track_id"] = track_id
|
|
detection["aruco_id"] = assigned_marker_id
|
|
|
|
selected_detection = select_farthest_hand(
|
|
detections,
|
|
x1,
|
|
y1,
|
|
body_center,
|
|
)
|
|
|
|
if selected_detection is not None:
|
|
label = selected_detection.get("label", "unknown")
|
|
if label.upper() == "A" and body_center is not None:
|
|
ray_x, ray_y, ray_dx, ray_dy = compute_pointing_ray_from_hand(
|
|
selected_detection["hand_landmarks"],
|
|
x1, y1,
|
|
selected_detection["crop_width"],
|
|
selected_detection["crop_height"],
|
|
)
|
|
target_ids = get_target_persons_snapshot()
|
|
target_id, target_dist = find_closest_target_person(
|
|
ray_x, ray_y, ray_dx, ray_dy,
|
|
person_positions,
|
|
target_ids,
|
|
)
|
|
if target_id is not None and target_dist < 25:
|
|
current_frame_pointing_targets.add(target_id)
|
|
selected_pointing_target_id = target_id
|
|
|
|
with pointing_target_lock:
|
|
if target_id not in pointing_target_tracker:
|
|
pointing_target_tracker[target_id] = 0
|
|
pointing_target_tracker[target_id] += 1
|
|
|
|
gesture.draw_gesture(
|
|
display,
|
|
selected_detection,
|
|
x1,
|
|
y1,
|
|
crop.shape[1],
|
|
crop.shape[0],
|
|
)
|
|
|
|
# Collect gestures for confirmation logic (Lock wird bereits außen gehalten)
|
|
if confirmation_poi_ids and track_id in confirmation_poi_ids:
|
|
lab = selected_detection.get("label", "").upper()
|
|
if lab in ("B", "C"):
|
|
confirmation_gestures[track_id] = lab
|
|
|
|
# Confirmation logic: evaluate gathered gestures after processing all persons in the frame
|
|
with confirmation_lock:
|
|
# Geändert: Prüft, ob mindestens alle POIs ihre Geste zeigen (weniger strikt als exakte Gleichheit)
|
|
if confirmation_poi_ids and confirmation_poi_ids.issubset(confirmation_gestures.keys()):
|
|
# Nur Gesten der relevanten Personen prüfen
|
|
relevant_gestures = [confirmation_gestures[tid] for tid in confirmation_poi_ids]
|
|
if len(set(relevant_gestures)) == 1:
|
|
current_consensus = relevant_gestures[0]
|
|
if current_consensus == confirmation_last_consensus:
|
|
confirmation_stable_counter += 1
|
|
else:
|
|
confirmation_last_consensus = current_consensus
|
|
confirmation_stable_counter = 1
|
|
|
|
# Da wir jetzt mehr Frames verarbeiten, erhöhen wir auf ca. 10 Frames (~0.3 Sek) für Stabilität
|
|
if confirmation_stable_counter >= 10:
|
|
result_conf = True if confirmation_last_consensus == "C" else False
|
|
if osc_client is not None:
|
|
osc_client.send_message("/confirmation", result_conf)
|
|
print(f"Sent confirmation {result_conf} after 3 stable frames (Gestures: {confirmation_last_consensus})")
|
|
|
|
# Reset confirmation state
|
|
confirmation_poi_ids.clear()
|
|
confirmation_stable_counter = 0
|
|
confirmation_last_consensus = None
|
|
clear_persons_of_interest()
|
|
else:
|
|
# Gestures are visible but don't match each other (e.g. one B, one C)
|
|
confirmation_stable_counter = 0
|
|
confirmation_last_consensus = None
|
|
else:
|
|
# Not all players of interest are showing a valid gesture in this frame
|
|
confirmation_stable_counter = 0
|
|
confirmation_last_consensus = None
|
|
|
|
cleanup_missing_person_aruco_state(set(person_positions.keys()))
|
|
# Publish latest per-frame snapshots for `/getAllIds` to use when assigning markers.
|
|
try:
|
|
with aruco_state_lock:
|
|
latest_marker_centers.clear()
|
|
latest_marker_centers.update(current_marker_centers)
|
|
latest_person_centers.clear()
|
|
latest_person_centers.update(person_positions)
|
|
except Exception:
|
|
pass
|
|
|
|
with pointing_target_lock:
|
|
for target_id, frame_count in list(pointing_target_tracker.items()):
|
|
# Prüfen, ob dieses Ziel in diesem Frame aktiv anvisiert wurde
|
|
if target_id in current_frame_pointing_targets:
|
|
if frame_count >= 4:
|
|
if osc_client is not None:
|
|
osc_client.send_message("/getPlayerID", int(get_aruco_id_for_track_id(target_id)))
|
|
print(f"Sent stable pointing target: {get_aruco_id_for_track_id(target_id)} after {frame_count} frames")
|
|
clear_persons_of_interest()
|
|
pointing_target_tracker.clear()
|
|
break
|
|
else:
|
|
# Langsam abbauen, wenn in diesem Frame nicht darauf gezeigt wurde
|
|
pointing_target_tracker[target_id] = max(0, frame_count - 1)
|
|
|
|
#send_osc_status(osc_client, person_count, get_persons_of_interest_snapshot(), selected_detection)
|
|
|
|
#if osc_client is not None and osc_status_enabled and selected_pointing_target_id is not None:
|
|
#osc_client.send_message("/werwolf/status/pointing_target", int(get_aruco_id_for_track_id(selected_pointing_target_id)) if get_aruco_id_for_track_id(selected_pointing_target_id) is not None else -1)
|
|
|
|
cv2.putText(
|
|
display,
|
|
f"People detected: {person_count}",
|
|
(20, 40),
|
|
cv2.FONT_HERSHEY_SIMPLEX,
|
|
1.0,
|
|
(0, 255, 0),
|
|
2,
|
|
cv2.LINE_AA,
|
|
)
|
|
cv2.putText(
|
|
display,
|
|
"q = quit",
|
|
(20, 80),
|
|
cv2.FONT_HERSHEY_SIMPLEX,
|
|
0.7,
|
|
(255, 255, 255),
|
|
2,
|
|
cv2.LINE_AA,
|
|
)
|
|
cv2.putText(
|
|
display,
|
|
f"Gesture: {GESTURE_FEATURE_FAMILY} | {GESTURE_CLASSIFIER_TYPE} | top {GESTURE_NUM_FEATURES} | POI IDs: {get_persons_of_interest_snapshot()} | OSC: {'on' if osc_status_enabled else 'off'}",
|
|
(20, 120),
|
|
cv2.FONT_HERSHEY_SIMPLEX,
|
|
0.7,
|
|
(255, 200, 0),
|
|
2,
|
|
cv2.LINE_AA,
|
|
)
|
|
|
|
target_str = f"Target IDs: {get_target_persons_snapshot()}"
|
|
if selected_pointing_target_id is not None:
|
|
target_str += f" | Pointing at: {selected_pointing_target_id}"
|
|
cv2.putText(
|
|
display,
|
|
target_str,
|
|
(20, 160),
|
|
cv2.FONT_HERSHEY_SIMPLEX,
|
|
0.7,
|
|
(0, 255, 255),
|
|
2,
|
|
cv2.LINE_AA,
|
|
)
|
|
|
|
cv2.imshow("YOLO Multi-Person Pose", display)
|
|
|
|
key = cv2.waitKey(1) & 0xFF
|
|
if key == ord("q"):
|
|
break
|
|
elif key == ord("a"):
|
|
osc_client.send_message("/demo/number", 42)
|
|
print("Sent: /demo/number 42")
|
|
elif key == ord("s"):
|
|
osc_client.send_message("/speak", "Impertinent")
|
|
print("Sent: /demo/xy [0.25, 0.75]")
|
|
elif ord("0") <= key <= ord("9"):
|
|
toggle_person_of_interest(key - ord("0"))
|
|
finally:
|
|
cap.release()
|
|
cv2.destroyAllWindows()
|
|
if gesture is not None:
|
|
gesture.close()
|
|
if osc_server is not None:
|
|
osc_server.shutdown()
|
|
osc_server.server_close()
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|