281 lines
9.3 KiB
Python
281 lines
9.3 KiB
Python
import numpy as np
|
|
import cv2
|
|
import os
|
|
from PySide6.QtCore import QObject, QTimer
|
|
import mediapipe as mp
|
|
from mediapipe.tasks.python import BaseOptions
|
|
from mediapipe.tasks.python.vision import HandLandmarker, HandLandmarkerOptions, RunningMode
|
|
from Tbd.helper import Hand2D
|
|
from collections import defaultdict
|
|
from Helpers.HandUdpSender import HandUdpSender
|
|
|
|
import struct
|
|
import time
|
|
|
|
MODEL_PATH = os.path.join(os.path.dirname(__file__), "models", "hand_landmarker.task")
|
|
|
|
class TrackingController(QObject):
|
|
def __init__(self, cameraList, P_mats, logger=None, tick_ms=30):
|
|
super().__init__()
|
|
|
|
self.cameraList = cameraList
|
|
self.P_mats = P_mats
|
|
self.logger = logger
|
|
|
|
self.udp = HandUdpSender(host="127.0.0.1", port=9000)
|
|
|
|
self.timer = QTimer(self)
|
|
self.timer.timeout.connect(self._tick)
|
|
self.tick_ms = tick_ms
|
|
|
|
self.running = False
|
|
|
|
# MediaPipe's legacy solutions.hands API no longer exists on the
|
|
# installed mediapipe version, so hand detection runs on the newer
|
|
# Tasks API instead (IMAGE mode: each camera frame is detected
|
|
# independently, since frames from different cameras aren't a single
|
|
# monotonic video stream that VIDEO mode requires).
|
|
options = HandLandmarkerOptions(
|
|
base_options=BaseOptions(model_asset_path=MODEL_PATH),
|
|
running_mode=RunningMode.IMAGE,
|
|
num_hands=4,
|
|
min_hand_detection_confidence=0.5,
|
|
min_hand_presence_confidence=0.5,
|
|
min_tracking_confidence=0.5,
|
|
)
|
|
self.detector = HandLandmarker.create_from_options(options)
|
|
|
|
def start(self):
|
|
# Guard: need projections
|
|
self.logger.log("Amount of P_mats:" + str(len(self.P_mats)))
|
|
if not self.P_mats or len(self.P_mats) < 2:
|
|
if self.logger: self.logger.log("[Tracking] Missing P_mats (need >=2 cameras calibrated)")
|
|
return
|
|
|
|
if self.running:
|
|
return
|
|
|
|
for camera in self.cameraList:
|
|
if not camera.open():
|
|
if self.logger: self.logger.log(f"[Tracking] cam{camera.index}: failed to open, will be skipped.")
|
|
|
|
self.running = True
|
|
self.timer.start(self.tick_ms)
|
|
if self.logger: self.logger.log("[Tracking] Started")
|
|
|
|
def stop(self):
|
|
if not self.running:
|
|
return
|
|
self.running = False
|
|
self.timer.stop()
|
|
|
|
for camera in self.cameraList:
|
|
camera.close()
|
|
|
|
if self.logger: self.logger.log("[Tracking] Stopped")
|
|
|
|
def _tick(self):
|
|
hands2d = self._detectHands_all_cameras()
|
|
if not hands2d:
|
|
return
|
|
|
|
# Group hands across cameras (you already have _allocateDetectedHands logic)
|
|
hand_groups = self._allocateDetectedHands(hands2d)
|
|
|
|
# Triangulate (your existing triangulation approach)
|
|
hands3d = self._triangulateHands(hand_groups)
|
|
|
|
if len(hands3d) > 0:
|
|
self.udp.send_hands3d(hands3d)
|
|
|
|
print(
|
|
f"hands2d={len(hands2d)} groups={len(hand_groups)} hands3d={len(hands3d)}",
|
|
flush=True
|
|
)
|
|
|
|
if len(hands2d) > 0:
|
|
print("cam_ids:", sorted(set(h.camera_id for h in hands2d)), flush=True)
|
|
|
|
def _detectHands_all_cameras(self):
|
|
out = []
|
|
for camera in self.cameraList:
|
|
if camera.capture is None:
|
|
continue
|
|
|
|
ok, frameBGR = camera.capture.read()
|
|
if not ok or frameBGR is None:
|
|
continue
|
|
|
|
h, w = frameBGR.shape[:2]
|
|
frameRGB = cv2.cvtColor(frameBGR, cv2.COLOR_BGR2RGB)
|
|
|
|
mp_image = mp.Image(image_format=mp.ImageFormat.SRGB, data=frameRGB)
|
|
result = self.detector.detect(mp_image)
|
|
|
|
if not result.hand_landmarks:
|
|
continue
|
|
|
|
for hi, lm_list in enumerate(result.hand_landmarks):
|
|
landmarks_px = np.array(
|
|
[[lm.x * w, lm.y * h] for lm in lm_list],
|
|
dtype=np.float32
|
|
)
|
|
|
|
label, score = "Unknown", 0.0
|
|
if hi < len(result.handedness) and result.handedness[hi]:
|
|
label = result.handedness[hi][0].category_name
|
|
score = result.handedness[hi][0].score
|
|
|
|
out.append(Hand2D(
|
|
camera_id=camera.index,
|
|
handedness=label,
|
|
score=score,
|
|
landmarks_px=landmarks_px
|
|
))
|
|
return out
|
|
|
|
def _allocateDetectedHands(self, hands2d):
|
|
frames_by_camera = defaultdict(list)
|
|
for hand in hands2d:
|
|
frames_by_camera[hand.camera_id].append(hand)
|
|
|
|
used = set()
|
|
hand_groups = []
|
|
reproj_threshold = 50.0
|
|
|
|
camera_ids = sorted(frames_by_camera.keys())
|
|
|
|
for cam_a in camera_ids:
|
|
if cam_a not in self.P_mats:
|
|
continue
|
|
for hand_a in frames_by_camera[cam_a]:
|
|
if id(hand_a) in used:
|
|
continue
|
|
|
|
group = {cam_a: hand_a}
|
|
used.add(id(hand_a))
|
|
|
|
P_a = self.P_mats[cam_a]
|
|
|
|
for cam_b in camera_ids:
|
|
if cam_b == cam_a:
|
|
continue
|
|
if cam_b not in self.P_mats:
|
|
continue
|
|
|
|
P_b = self.P_mats[cam_b]
|
|
best_hand = None
|
|
best_err = np.inf
|
|
|
|
for hand_b in frames_by_camera[cam_b]:
|
|
if id(hand_b) in used:
|
|
continue
|
|
|
|
err = self.pair_reprojection_error(
|
|
hand_a, hand_b, P_a, P_b, key_idxs=[0, 9]
|
|
)
|
|
if err < best_err:
|
|
best_err = err
|
|
best_hand = hand_b
|
|
|
|
if best_hand is not None and best_err < reproj_threshold:
|
|
group[cam_b] = best_hand
|
|
used.add(id(best_hand))
|
|
|
|
hand_groups.append(group)
|
|
|
|
return hand_groups
|
|
|
|
def triangulate_hands_with_metadata(self, hand_groups):
|
|
hands3d = []
|
|
for group in hand_groups:
|
|
if len(group) < 2:
|
|
continue
|
|
|
|
handedness, confidence = self.choose_metadata_from_group(group)
|
|
|
|
cams = list(group.keys())
|
|
P1 = self.P_mats[cams[0]]
|
|
P2 = self.P_mats[cams[1]]
|
|
lm1 = group[cams[0]].landmarks_px
|
|
lm2 = group[cams[1]].landmarks_px
|
|
|
|
pts3d = []
|
|
for i in range(lm1.shape[0]):
|
|
X = self.triangulate_point(P1, P2, lm1[i], lm2[i])
|
|
pts3d.append(X)
|
|
|
|
hands3d.append({
|
|
"handedness": handedness, # "Left"/"Right"/"Unknown"
|
|
"confidence": confidence, # 0..1
|
|
"landmarks": np.asarray(pts3d, dtype=np.float32) # (21,3)
|
|
})
|
|
return hands3d
|
|
|
|
# --- math helpers (copy from your existing code) ---
|
|
def triangulate_point(self, P1, P2, x1, x2):
|
|
A = np.zeros((4, 4), dtype=np.float32)
|
|
A[0] = x1[0] * P1[2] - P1[0]
|
|
A[1] = x1[1] * P1[2] - P1[1]
|
|
A[2] = x2[0] * P2[2] - P2[0]
|
|
A[3] = x2[1] * P2[2] - P2[1]
|
|
_, _, Vt = np.linalg.svd(A)
|
|
X_h = Vt[-1]
|
|
X_h /= X_h[3]
|
|
|
|
return X_h[:3]
|
|
|
|
def project_point(self, P, X):
|
|
X_h = np.array([X[0], X[1], X[2], 1.0], dtype=np.float32)
|
|
x = P @ X_h
|
|
return np.array([x[0] / x[2], x[1] / x[2]], dtype=np.float32)
|
|
|
|
def pair_reprojection_error(self, hand_a, hand_b, P_a, P_b, key_idxs):
|
|
errors = []
|
|
for idx in key_idxs:
|
|
x1 = hand_a.landmarks_px[idx]
|
|
x2 = hand_b.landmarks_px[idx]
|
|
X = self.triangulate_point(P_a, P_b, x1, x2)
|
|
x1_hat = self.project_point(P_a, X)
|
|
x2_hat = self.project_point(P_b, X)
|
|
errors.append(np.linalg.norm(x1_hat - x1))
|
|
errors.append(np.linalg.norm(x2_hat - x2))
|
|
return float(np.mean(errors))
|
|
|
|
def choose_metadata_from_group(group: dict):
|
|
# group: {cam_id: Hand2D, ...}
|
|
best = max(group.values(), key=lambda h: float(getattr(h, "score", 0.0)))
|
|
handedness = getattr(best, "handedness", "Unknown")
|
|
confidence = float(getattr(best, "score", 0.0))
|
|
return handedness, confidence
|
|
|
|
def _triangulateHands(self, hand_groups):
|
|
hands3d = []
|
|
|
|
for group in hand_groups:
|
|
cams = list(group.keys())
|
|
if len(cams) < 2:
|
|
continue
|
|
|
|
# Pick metadata from best 2D view
|
|
best = max(group.values(), key=lambda hh: float(getattr(hh, "score", 0.0)))
|
|
handedness = getattr(best, "handedness", "Unknown")
|
|
confidence = float(getattr(best, "score", 0.0))
|
|
|
|
P1 = self.P_mats[cams[0]]
|
|
P2 = self.P_mats[cams[1]]
|
|
lm1 = group[cams[0]].landmarks_px
|
|
lm2 = group[cams[1]].landmarks_px
|
|
|
|
pts3d = []
|
|
for i in range(lm1.shape[0]):
|
|
X = self.triangulate_point(P1, P2, lm1[i], lm2[i])
|
|
pts3d.append(X)
|
|
|
|
hands3d.append({
|
|
"landmarks": np.asarray(pts3d, dtype=np.float32),
|
|
"handedness": handedness,
|
|
"confidence": confidence,
|
|
})
|
|
|
|
return hands3d |