Commit for Gitea
This commit is contained in:
DuOtto
@@ -0,0 +1,260 @@
|
||||
import numpy as np
|
||||
import cv2
|
||||
from PySide6.QtCore import QObject, QTimer
|
||||
import mediapipe as mp
|
||||
from Tbd.helper import Hand2D
|
||||
from collections import defaultdict
|
||||
from Helpers.HandUdpSender import HandUdpSender
|
||||
|
||||
import struct
|
||||
import time
|
||||
|
||||
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
|
||||
|
||||
# init mediapipe here (or inject it)
|
||||
|
||||
self.mp_hands = mp.solutions.hands
|
||||
self.hands = self.mp_hands.Hands(
|
||||
static_image_mode=False,
|
||||
max_num_hands=4,
|
||||
model_complexity=1,
|
||||
min_detection_confidence=0.5,
|
||||
min_tracking_confidence=0.5,
|
||||
)
|
||||
|
||||
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
|
||||
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()
|
||||
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:
|
||||
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)
|
||||
frameRGB.flags.writeable = False
|
||||
|
||||
hand_tracking_result = self.hands.process(frameRGB)
|
||||
if not hand_tracking_result.multi_hand_landmarks:
|
||||
continue
|
||||
|
||||
handed = hand_tracking_result.multi_handedness
|
||||
for hi, lm_list in enumerate(hand_tracking_result.multi_hand_landmarks):
|
||||
landmarks_px = np.array(
|
||||
[[lm.x * w, lm.y * h] for lm in lm_list.landmark],
|
||||
dtype=np.float32
|
||||
)
|
||||
|
||||
label, score = "Unknown", 0.0
|
||||
if hi < len(handed) and handed[hi].classification:
|
||||
label = handed[hi].classification[0].label
|
||||
score = handed[hi].classification[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
|
||||
Reference in New Issue
Block a user