DuOtto
334 lines
11 KiB
Python
334 lines
11 KiB
Python
import cv2
|
|
import numpy
|
|
import mediapipe
|
|
from Tbd.helper import CameraObject, Hand2D
|
|
from PySide6.QtCore import Qt, QTimer
|
|
from PySide6.QtWidgets import QMainWindow, QLabel, QVBoxLayout, QWidget
|
|
from PySide6.QtGui import QImage, QPixmap
|
|
from typing import List
|
|
from collections import defaultdict
|
|
import winsound
|
|
|
|
mediapipe_options = mediapipe.solutions.hands
|
|
|
|
class GameWindow(QMainWindow):
|
|
"""
|
|
Separate window for the “game” part.
|
|
You can put your rendering, controls, etc. here.
|
|
"""
|
|
|
|
def __init__(self, cameraList: List[CameraObject], width_px=1920, height_px=1080, fps=30, max_num_hands=4):
|
|
super().__init__()
|
|
|
|
self.P_mats = {}
|
|
|
|
self.cameraList = cameraList
|
|
self._hands = mediapipe_options.Hands(
|
|
static_image_mode=False,
|
|
max_num_hands=max_num_hands,
|
|
model_complexity=1,
|
|
min_detection_confidence=0.5,
|
|
min_tracking_confidence=0.5,
|
|
)
|
|
|
|
self.setWindowTitle("Game Window")
|
|
self.canvas_w = width_px
|
|
self.canvas_h = height_px
|
|
|
|
|
|
self.view = QLabel("HUD")
|
|
self.view.setAlignment(Qt.AlignCenter)
|
|
self.view.setScaledContents(False) # keep aspect ratio
|
|
|
|
|
|
central = QWidget(self)
|
|
self.setCentralWidget(central)
|
|
layout = QVBoxLayout(central)
|
|
layout.setContentsMargins(0, 0, 0, 0)
|
|
layout.addWidget(self.view, 1)
|
|
|
|
# Create a Timer for my _tick function and start it
|
|
self.timer = QTimer(self)
|
|
self.timer.timeout.connect(self._tick)
|
|
self.timer.start(30) # ~33 FPS
|
|
|
|
def _tick(self):
|
|
|
|
# 1) create a black BGR canvas
|
|
frame_bgr = numpy.zeros((self.canvas_h, self.canvas_w, 3), dtype=numpy.uint8)
|
|
|
|
# 2) (optional) draw HUD widgets here
|
|
cv2.putText(frame_bgr, "HUD ready", (40, 60), cv2.FONT_HERSHEY_SIMPLEX, 1.2, (200,200,200), 2, cv2.LINE_AA)
|
|
|
|
hands2d = self._detectHands()
|
|
|
|
self.draw_hands_on_frame(frame_bgr, hands2d)
|
|
|
|
# 3) draw detected hands (in canvas pixel coords)
|
|
for h in hands2d:
|
|
for (x, y) in h.landmarks_px.astype(int):
|
|
cv2.circle(frame_bgr, (x, y), 4, (0, 255, 255), -1, cv2.LINE_AA)
|
|
|
|
# 4) show it
|
|
frame_rgb = cv2.cvtColor(frame_bgr, cv2.COLOR_BGR2RGB)
|
|
qimg = QImage(frame_rgb.data, self.canvas_w, self.canvas_h,
|
|
self.canvas_w * 3, QImage.Format_RGB888)
|
|
pix = QPixmap.fromImage(qimg)
|
|
# keep aspect ratio when fitting into the label
|
|
self.view.setPixmap(pix.scaled(self.view.size(), Qt.KeepAspectRatio, Qt.SmoothTransformation))
|
|
|
|
hand_groups = self._allocateDetectedHands(hands2d)
|
|
|
|
hands3d = self._triangulateHands(hand_groups)
|
|
print(hands3d)
|
|
self._updateGeasture()
|
|
self._checkInteractions()
|
|
self._upateHUD()
|
|
self._drawHUD()
|
|
|
|
|
|
# Loop all Cameras and return a List of all found Hands in all Cameras
|
|
def _detectHands(self):
|
|
out: List[Hand2D] = [] # List of all found Hands
|
|
|
|
|
|
for camera in self.cameraList:
|
|
|
|
# Get the frame of the camera and check if the camera is available
|
|
ok, frameBGR = camera.capture.read()
|
|
if not ok or frameBGR is None:
|
|
continue
|
|
|
|
frameHight, frameWidht = frameBGR.shape[:2] # Take only the the frame hight and widht from the Tuple
|
|
|
|
frameRGB = cv2.cvtColor(frameBGR, cv2.COLOR_BGR2RGB) # Convert to RGB becaus,e MediaPipe expects RGB
|
|
frameRGB.flags.writeable = False # (minor speed gain)
|
|
|
|
# Detect Hands in the frame and return a list of all found Hands, then check if
|
|
foundHands = self._hands.process(frameRGB)
|
|
if not foundHands.multi_hand_landmarks:
|
|
continue
|
|
|
|
# handedness info aligns with landmarks list
|
|
handed = foundHands.multi_handedness
|
|
|
|
for handIndex, landmarlList in enumerate(foundHands.multi_hand_landmarks):
|
|
landmarks_px = numpy.array([[landmark.x*frameWidht, landmark.y*frameHight] for landmark in landmarlList.landmark], dtype=numpy.float32)
|
|
|
|
label = "Unknown"
|
|
score = 0.0
|
|
|
|
if handIndex < len(handed):
|
|
classifications = handed[handIndex].classification
|
|
if classifications:
|
|
label = classifications[0].label # "Left" / "Right"
|
|
score = classifications[0].score # confidence
|
|
|
|
out.append(Hand2D(
|
|
camera_id=camera.index,
|
|
handedness=label,
|
|
score=score,
|
|
landmarks_px=landmarks_px
|
|
))
|
|
|
|
|
|
return out
|
|
|
|
def _allocateDetectedHands(self, hands2d: list):
|
|
"""
|
|
Take flat list of Hand2D from all cameras and group them into physical hands.
|
|
Writes self._hand_groups = [ {cam_id: Hand2D, ...}, ... ]
|
|
"""
|
|
|
|
# Group by camera
|
|
frames_by_camera = defaultdict(list)
|
|
for hand in hands2d:
|
|
frames_by_camera[hand.camera_id].append(hand)
|
|
|
|
# We will mark which Hand2D detections are already consumed
|
|
used = set() # set of id(hand2d)
|
|
hand_groups = []
|
|
|
|
# Threshold in pixels for “same hand”
|
|
reproj_threshold = 8.0 # tune this
|
|
|
|
camera_ids = sorted(frames_by_camera.keys())
|
|
|
|
for cam_a in camera_ids:
|
|
for hand_a in frames_by_camera[cam_a]:
|
|
if id(hand_a) in used:
|
|
continue
|
|
|
|
# Start a new group with this detection as the seed
|
|
group = {cam_a: hand_a}
|
|
used.add(id(hand_a))
|
|
|
|
P_a = self.P_mats[cam_a]
|
|
|
|
# Try to find matching hands in all other cameras
|
|
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 = numpy.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] # wrist + middle MCP for example
|
|
)
|
|
|
|
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 _triangulateHands(self, hand_groups):
|
|
|
|
hands3d = []
|
|
|
|
for group in hand_groups:
|
|
# use all cameras in 'group' to triangulate each landmark
|
|
cams = list(group.keys())
|
|
hands = [group[c] for c in cams]
|
|
|
|
# Example: simple pairwise triangulation using first two cameras
|
|
if len(cams) < 2:
|
|
continue # need at least 2 views
|
|
|
|
P1 = self.P_mats[cams[0]]
|
|
P2 = self.P_mats[cams[1]]
|
|
lm1 = hands[0].landmarks_px
|
|
lm2 = hands[1].landmarks_px
|
|
|
|
pts3d = []
|
|
for i in range(lm1.shape[0]):
|
|
X = self.triangulate_point(P1, P2, lm1[i], lm2[i])
|
|
pts3d.append(X)
|
|
|
|
pts3d = numpy.array(pts3d, dtype=numpy.float32)
|
|
hands3d.append(pts3d)
|
|
|
|
return hands3d
|
|
|
|
def _updateGeasture(self):
|
|
|
|
return
|
|
|
|
def _getMostConfindentHand(self):
|
|
|
|
return
|
|
|
|
def _checkInteractions(self):
|
|
|
|
return
|
|
|
|
def _upateHUD(self):
|
|
|
|
return
|
|
|
|
def _drawHUD(self):
|
|
|
|
return
|
|
|
|
|
|
def close(self):
|
|
print("What?")
|
|
self.timer.stop()
|
|
return super().close()
|
|
|
|
|
|
def draw_hands_on_frame(self, frame_bgr: numpy.ndarray, hands2d: list):
|
|
# Draw connections first, then points
|
|
for h in hands2d:
|
|
pts = h.landmarks_px.astype(int) # (21,2) in pixels
|
|
|
|
# bones
|
|
for a, b in mediapipe_options.HAND_CONNECTIONS:
|
|
cv2.line(frame_bgr,
|
|
(int(pts[a,0]), int(pts[a,1])),
|
|
(int(pts[b,0]), int(pts[b,1])),
|
|
(0, 255, 0), 2, cv2.LINE_AA)
|
|
|
|
# joints
|
|
for (x, y) in pts:
|
|
cv2.circle(frame_bgr, (int(x), int(y)), 3, (0, 0, 255), -1, cv2.LINE_AA)
|
|
|
|
# optional label
|
|
cv2.putText(frame_bgr, f"{h.handedness} {h.score:.2f}",
|
|
(int(pts[0,0]), int(pts[0,1])-8),
|
|
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (80,255,80), 1, cv2.LINE_AA)
|
|
|
|
def project_point(self, P, X):
|
|
"""
|
|
P: 3x4 projection matrix
|
|
X: 3D point (X, Y, Z)
|
|
returns: 2D point (u, v) in pixels
|
|
"""
|
|
X_h = numpy.array([X[0], X[1], X[2], 1.0], dtype=numpy.float32)
|
|
x = P @ X_h
|
|
return numpy.array([x[0] / x[2], x[1] / x[2]], dtype=numpy.float32)
|
|
|
|
def pair_reprojection_error(self, hand_a, hand_b, P_a, P_b, key_idxs):
|
|
"""
|
|
hand_a, hand_b: Hand2D objects
|
|
P_a, P_b: 3x4 projection matrices
|
|
key_idxs: list of landmark indices to use (e.g. [0, 9])
|
|
returns: average reprojection error in pixels
|
|
"""
|
|
errors = []
|
|
|
|
for idx in key_idxs:
|
|
x1 = hand_a.landmarks_px[idx] # (u, v)
|
|
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)
|
|
|
|
e1 = numpy.linalg.norm(x1_hat - x1)
|
|
e2 = numpy.linalg.norm(x2_hat - x2)
|
|
|
|
errors.append(e1)
|
|
errors.append(e2)
|
|
|
|
return float(numpy.mean(errors))
|
|
|
|
|
|
def triangulate_point(self, P1, P2, x1, x2):
|
|
"""
|
|
P1, P2: 3x4 projection matrices
|
|
x1, x2: 2D points (u, v) in pixels (float)
|
|
returns: 3D point in world coords (X, Y, Z)
|
|
"""
|
|
A = numpy.zeros((4, 4), dtype=numpy.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]
|
|
|
|
# Solve A * X = 0, X is homogeneous 4D
|
|
_, _, Vt = numpy.linalg.svd(A)
|
|
X_h = Vt[-1]
|
|
X_h /= X_h[3]
|
|
return X_h[:3] # (X, Y, Z)
|
|
|