Fix camera capture reliability and rewrite tracking for available mediapipe/hardware
This commit is contained in:
@@ -0,0 +1,2 @@
|
||||
__pycache__/
|
||||
*.pyc
|
||||
Vendored
+9
@@ -0,0 +1,9 @@
|
||||
{
|
||||
"python-envs.pythonProjects": [
|
||||
{
|
||||
"path": ".",
|
||||
"envManager": "ms-python.python:system",
|
||||
"packageManager": "ms-python.python:pip"
|
||||
}
|
||||
]
|
||||
}
|
||||
@@ -0,0 +1,76 @@
|
||||
"""
|
||||
Multi-camera concurrent streaming stress test.
|
||||
|
||||
What it does:
|
||||
- Opens all cameras in CAM_INDICES simultaneously via CameraObject.open() (MSMF)
|
||||
- Continuously reads frames from all of them in a round-robin loop for TEST_DURATION_SECONDS
|
||||
- Tallies successful vs failed grabs per camera
|
||||
- Prints a summary so we know whether this hardware/USB topology can sustain
|
||||
continuous simultaneous streaming from all cameras, which is what real-time
|
||||
hand tracking (sending data to Unreal over UDP every frame) will need.
|
||||
|
||||
Run standalone, no Qt/app required:
|
||||
python Multi_Camera_Stream_Test.py
|
||||
"""
|
||||
|
||||
import sys
|
||||
import os
|
||||
import time
|
||||
|
||||
sys.path.append(os.path.join(os.path.dirname(__file__), ".."))
|
||||
|
||||
from Tbd.helper import CameraObject
|
||||
|
||||
CAM_INDICES = [1, 2, 3, 4, 5, 6]
|
||||
TEST_DURATION_SECONDS = 15
|
||||
WIDTH = 1920
|
||||
HEIGHT = 1080
|
||||
|
||||
|
||||
def main():
|
||||
cameras = []
|
||||
|
||||
print("Opening cameras...")
|
||||
for index in CAM_INDICES:
|
||||
cam = CameraObject(capture=None, index=index)
|
||||
ok = cam.open(width=WIDTH, height=HEIGHT)
|
||||
print(f"cam{index}: {'opened' if ok else 'FAILED TO OPEN'}")
|
||||
if ok:
|
||||
cameras.append(cam)
|
||||
|
||||
if not cameras:
|
||||
print("No cameras opened, aborting.")
|
||||
return
|
||||
|
||||
print(f"\n{len(cameras)}/{len(CAM_INDICES)} cameras opened. Streaming for {TEST_DURATION_SECONDS}s...\n")
|
||||
|
||||
successes = {cam.index: 0 for cam in cameras}
|
||||
failures = {cam.index: 0 for cam in cameras}
|
||||
|
||||
start = time.time()
|
||||
while time.time() - start < TEST_DURATION_SECONDS:
|
||||
for cam in cameras:
|
||||
ok, frame = cam.capture.read()
|
||||
if ok and frame is not None:
|
||||
successes[cam.index] += 1
|
||||
else:
|
||||
failures[cam.index] += 1
|
||||
|
||||
elapsed = time.time() - start
|
||||
|
||||
print("=" * 50)
|
||||
print(f"Results after {elapsed:.1f}s:")
|
||||
for cam in cameras:
|
||||
total = successes[cam.index] + failures[cam.index]
|
||||
rate = successes[cam.index] / total * 100 if total else 0
|
||||
fps = successes[cam.index] / elapsed
|
||||
print(f"cam{cam.index}: {successes[cam.index]} ok / {failures[cam.index]} failed "
|
||||
f"({rate:.1f}% success, ~{fps:.1f} fps)")
|
||||
print("=" * 50)
|
||||
|
||||
for cam in cameras:
|
||||
cam.close()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,92 @@
|
||||
"""
|
||||
Multi-camera concurrent streaming stress test - threaded version.
|
||||
|
||||
Same as Multi_Camera_Stream_Test.py, but reads each camera on its own thread
|
||||
instead of round-robining on a single thread, so cameras aren't serialized
|
||||
against each other. This is closer to what real-time hand tracking (reading
|
||||
every camera every frame, then sending over UDP) will actually look like.
|
||||
|
||||
Run standalone, no Qt/app required:
|
||||
python Multi_Camera_Stream_Test_Threaded.py
|
||||
"""
|
||||
|
||||
import sys
|
||||
import os
|
||||
import time
|
||||
import threading
|
||||
|
||||
sys.path.append(os.path.join(os.path.dirname(__file__), ".."))
|
||||
|
||||
from Tbd.helper import CameraObject
|
||||
|
||||
CAM_INDICES = [1, 2, 3, 4, 5, 6]
|
||||
TEST_DURATION_SECONDS = 15
|
||||
WIDTH = 1920
|
||||
HEIGHT = 1080
|
||||
|
||||
|
||||
def stream_camera(cam: CameraObject, stop_event: threading.Event, results: dict):
|
||||
successes = 0
|
||||
failures = 0
|
||||
|
||||
while not stop_event.is_set():
|
||||
ok, frame = cam.capture.read()
|
||||
if ok and frame is not None:
|
||||
successes += 1
|
||||
else:
|
||||
failures += 1
|
||||
|
||||
results[cam.index] = (successes, failures)
|
||||
|
||||
|
||||
def main():
|
||||
cameras = []
|
||||
|
||||
print("Opening cameras...")
|
||||
for index in CAM_INDICES:
|
||||
cam = CameraObject(capture=None, index=index)
|
||||
ok = cam.open(width=WIDTH, height=HEIGHT)
|
||||
print(f"cam{index}: {'opened' if ok else 'FAILED TO OPEN'}")
|
||||
if ok:
|
||||
cameras.append(cam)
|
||||
|
||||
if not cameras:
|
||||
print("No cameras opened, aborting.")
|
||||
return
|
||||
|
||||
print(f"\n{len(cameras)}/{len(CAM_INDICES)} cameras opened. Streaming (threaded) for {TEST_DURATION_SECONDS}s...\n")
|
||||
|
||||
stop_event = threading.Event()
|
||||
results = {}
|
||||
threads = [
|
||||
threading.Thread(target=stream_camera, args=(cam, stop_event, results), daemon=True)
|
||||
for cam in cameras
|
||||
]
|
||||
|
||||
start = time.time()
|
||||
for t in threads:
|
||||
t.start()
|
||||
|
||||
time.sleep(TEST_DURATION_SECONDS)
|
||||
stop_event.set()
|
||||
for t in threads:
|
||||
t.join()
|
||||
elapsed = time.time() - start
|
||||
|
||||
print("=" * 50)
|
||||
print(f"Results after {elapsed:.1f}s:")
|
||||
for cam in cameras:
|
||||
successes, failures = results.get(cam.index, (0, 0))
|
||||
total = successes + failures
|
||||
rate = successes / total * 100 if total else 0
|
||||
fps = successes / elapsed
|
||||
print(f"cam{cam.index}: {successes} ok / {failures} failed "
|
||||
f"({rate:.1f}% success, ~{fps:.1f} fps)")
|
||||
print("=" * 50)
|
||||
|
||||
for cam in cameras:
|
||||
cam.close()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -1,7 +1,10 @@
|
||||
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
|
||||
@@ -9,6 +12,8 @@ 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__()
|
||||
@@ -25,16 +30,20 @@ class TrackingController(QObject):
|
||||
|
||||
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,
|
||||
# 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
|
||||
@@ -45,6 +54,11 @@ class TrackingController(QObject):
|
||||
|
||||
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")
|
||||
@@ -54,6 +68,10 @@ class TrackingController(QObject):
|
||||
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):
|
||||
@@ -81,29 +99,32 @@ class TrackingController(QObject):
|
||||
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)
|
||||
frameRGB.flags.writeable = False
|
||||
|
||||
hand_tracking_result = self.hands.process(frameRGB)
|
||||
if not hand_tracking_result.multi_hand_landmarks:
|
||||
mp_image = mp.Image(image_format=mp.ImageFormat.SRGB, data=frameRGB)
|
||||
result = self.detector.detect(mp_image)
|
||||
|
||||
if not result.hand_landmarks:
|
||||
continue
|
||||
|
||||
handed = hand_tracking_result.multi_handedness
|
||||
for hi, lm_list in enumerate(hand_tracking_result.multi_hand_landmarks):
|
||||
for hi, lm_list in enumerate(result.hand_landmarks):
|
||||
landmarks_px = np.array(
|
||||
[[lm.x * w, lm.y * h] for lm in lm_list.landmark],
|
||||
[[lm.x * w, lm.y * h] for lm in lm_list],
|
||||
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
|
||||
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,
|
||||
|
||||
@@ -2,6 +2,7 @@ import numpy as np
|
||||
import json
|
||||
import cv2
|
||||
import threading
|
||||
import time
|
||||
|
||||
from dataclasses import dataclass, field, asdict
|
||||
from typing import List, Optional, Tuple, Dict
|
||||
@@ -123,6 +124,38 @@ class CameraObject():
|
||||
self.capture.set(cv2.CAP_PROP_FRAME_WIDTH, 2048.0)
|
||||
self.capture.set(cv2.CAP_PROP_FRAME_HEIGHT, 1536.0)
|
||||
|
||||
def open(self, width: int = 1920, height: int = 1080, max_attempts: int = 5) -> bool:
|
||||
"""
|
||||
Open this camera on demand via MSMF. Only MSMF has been able to open all
|
||||
6 cameras on this rig at all (DSHOW hits a hard concurrent-instance limit
|
||||
around 2-3 devices on this hardware) - the tradeoff is MSMF cameras should
|
||||
not be kept open simultaneously in large numbers, so callers are expected
|
||||
to open() right before use and close() right after.
|
||||
"""
|
||||
if self.capture is not None:
|
||||
return True
|
||||
|
||||
for attempt in range(1, max_attempts + 1):
|
||||
capture = cv2.VideoCapture(self.index, cv2.CAP_MSMF)
|
||||
if capture.isOpened():
|
||||
capture.set(cv2.CAP_PROP_FRAME_WIDTH, width)
|
||||
capture.set(cv2.CAP_PROP_FRAME_HEIGHT, height)
|
||||
capture.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*'MJPG'))
|
||||
self.capture = capture
|
||||
self.pxWidth = width
|
||||
self.pxHeight = height
|
||||
return True
|
||||
|
||||
capture.release()
|
||||
time.sleep(0.3 * attempt)
|
||||
|
||||
return False
|
||||
|
||||
def close(self):
|
||||
if self.capture is not None:
|
||||
self.capture.release()
|
||||
self.capture = None
|
||||
|
||||
|
||||
@dataclass
|
||||
class Hand2D():
|
||||
|
||||
Binary file not shown.
@@ -43,14 +43,20 @@ class CameraSetupWidget(QWidget):
|
||||
|
||||
|
||||
def _start_camera_preview(self):
|
||||
if not self.cameraObject.open():
|
||||
return
|
||||
self.frame_update_timer.start(30)
|
||||
|
||||
def _stop_camera_preview(self):
|
||||
self.frame_update_timer.stop()
|
||||
self.cameraObject.close()
|
||||
|
||||
|
||||
|
||||
def _update_frame(self):
|
||||
if self.cameraObject.capture is None:
|
||||
return
|
||||
|
||||
ok, frame = self.cameraObject.capture.read()
|
||||
if not ok or frame is None:
|
||||
return
|
||||
|
||||
@@ -1,333 +0,0 @@
|
||||
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)
|
||||
|
||||
+103
-20
@@ -69,6 +69,8 @@ class HomeWindow(QWidget):
|
||||
self.clear_calibration_samples_button = QPushButton("Clear calibration samples")
|
||||
self.load_calibration_button = QPushButton("Load calibration")
|
||||
self.save_calibration_button = QPushButton("Save calibration")
|
||||
self.connect_cameras_button = QPushButton("Connect cameras")
|
||||
self.disconnect_cameras_button = QPushButton("Disconnect cameras")
|
||||
|
||||
|
||||
# Creating the Scrollable area
|
||||
@@ -79,6 +81,8 @@ class HomeWindow(QWidget):
|
||||
self.scroll_Box.setWidget(self.scroll_Content)
|
||||
|
||||
# Adding Primary show elements
|
||||
self.main_Layout.addWidget(self.connect_cameras_button)
|
||||
self.main_Layout.addWidget(self.disconnect_cameras_button)
|
||||
self.main_Layout.addWidget(self.clear_calibration_samples_button)
|
||||
self.main_Layout.addWidget(self.load_calibration_button)
|
||||
self.main_Layout.addWidget(self.save_calibration_button)
|
||||
@@ -92,8 +96,28 @@ class HomeWindow(QWidget):
|
||||
self.clear_calibration_samples_button.clicked.connect(self._clear_calibration_samples)
|
||||
self.load_calibration_button.clicked.connect(self._load_calibration)
|
||||
self.save_calibration_button.clicked.connect(self._save_calibration)
|
||||
self.connect_cameras_button.clicked.connect(self._connect_cameras)
|
||||
self.disconnect_cameras_button.clicked.connect(self._disconnect_cameras)
|
||||
|
||||
|
||||
def _connect_cameras(self):
|
||||
# Keep all cameras open simultaneously during calibration - this hardware
|
||||
# has been confirmed to sustain 6 concurrent MSMF streams once split
|
||||
# across separate USB controllers, so per-sample open/close is no longer
|
||||
# needed and was just adding latency to every capture.
|
||||
for camera in self.cameraList:
|
||||
if camera.capture is not None:
|
||||
continue
|
||||
if camera.open():
|
||||
self.logger.log(f"cam{camera.index}: connected.")
|
||||
else:
|
||||
self.logger.log(f"cam{camera.index}: failed to connect.")
|
||||
|
||||
def _disconnect_cameras(self):
|
||||
for camera in self.cameraList:
|
||||
camera.close()
|
||||
self.logger.log("All cameras disconnected.")
|
||||
|
||||
def _clear_calibration_samples(self):
|
||||
self.calibration_samples.clear()
|
||||
self.logger.log("Calibration samples where cleared.")
|
||||
@@ -134,43 +158,83 @@ class HomeWindow(QWidget):
|
||||
camera.camera_matrix = camera_matrix
|
||||
camera.distortion_coefficients = distortion_coefficients
|
||||
|
||||
if camera_matrix is None:
|
||||
continue
|
||||
|
||||
self.logger.log(f"[Intrinsics] cam{camera.index}: RMS={reprojection_error_rms:.4f} fx={camera_matrix[0,0]:.2f} fy={camera_matrix[1,1]:.2f} cx={camera_matrix[0,2]:.2f} cy={camera_matrix[1,2]:.2f}")
|
||||
|
||||
reference_camera = next(camera for camera in self.cameraList if camera.index == REFERENCE_CAMERA_INDEX)
|
||||
|
||||
if reference_camera.camera_matrix is None:
|
||||
self.logger.log(f"[Calibration] Reference camera cam{reference_camera.index} has no intrinsics, aborting calibration.")
|
||||
return
|
||||
|
||||
self.logger.log("Intrinsics done")
|
||||
|
||||
self.logger.log("Start extrinsics1")
|
||||
# Reference camera projection
|
||||
self.logger.log("Start extrinsics")
|
||||
# Reference camera projection (world origin)
|
||||
reference_camera.rotation_matrix_world_to_camera = numpy.eye(3)
|
||||
reference_camera.translation_vector_world_to_camera = numpy.zeros((3,1))
|
||||
reference_camera.camera_projection_matrix = (
|
||||
reference_camera.camera_matrix
|
||||
@ numpy.hstack([numpy.eye(3), numpy.zeros((3,1))])
|
||||
)
|
||||
self.logger.log("Start extrinsics2")
|
||||
self.P_mats.clear()
|
||||
self.P_mats[reference_camera.index] = reference_camera.camera_projection_matrix
|
||||
self.logger.log("Start extrinsics3")
|
||||
for camera_to_calibrate in self.cameraList:
|
||||
if camera_to_calibrate.index == REFERENCE_CAMERA_INDEX:
|
||||
continue
|
||||
|
||||
# Not every camera necessarily overlaps with the reference camera directly
|
||||
# (e.g. cameras on opposite sides of the table) - chain extrinsics through
|
||||
# whichever already-posed camera has the most shared samples with each
|
||||
# remaining camera instead of requiring a direct link to the reference.
|
||||
cameras_by_index = {camera.index: camera for camera in self.cameraList}
|
||||
posed_indices = {reference_camera.index}
|
||||
remaining_indices = set(cameras_by_index.keys()) - posed_indices
|
||||
|
||||
stereo_rms,R_ref_to_cam,t_ref_to_cam,E, F, P_ref, P_cam = self._stereo_calibrate_from_charuco_samples(
|
||||
while remaining_indices:
|
||||
best_known_index, best_target_index, best_count = None, None, 0
|
||||
|
||||
for known_index in posed_indices:
|
||||
for target_index in remaining_indices:
|
||||
count = self._count_common_samples(known_index, target_index)
|
||||
if count > best_count:
|
||||
best_known_index, best_target_index, best_count = known_index, target_index, count
|
||||
|
||||
if best_known_index is None:
|
||||
for index in remaining_indices:
|
||||
self.logger.log(f"cam{index}: no chain of overlapping samples back to the reference camera, skipping extrinsics.")
|
||||
break
|
||||
|
||||
known_camera = cameras_by_index[best_known_index]
|
||||
target_camera = cameras_by_index[best_target_index]
|
||||
|
||||
stereo_rms, R_known_to_target, t_known_to_target, E, F, P_known, P_target = self._stereo_calibrate_from_charuco_samples(
|
||||
sample_list=self.calibration_samples,
|
||||
board=self.board,
|
||||
camera_refernece=reference_camera,
|
||||
camera_to_calibrate=camera_to_calibrate
|
||||
camera_refernece=known_camera,
|
||||
camera_to_calibrate=target_camera
|
||||
)
|
||||
|
||||
if R_known_to_target is None:
|
||||
self.logger.log(f"cam{best_known_index} <-> cam{best_target_index}: stereo calibration failed despite {best_count} shared samples, skipping.")
|
||||
remaining_indices.discard(best_target_index)
|
||||
continue
|
||||
|
||||
camera_to_calibrate.rotation_matrix_world_to_camera = R_ref_to_cam
|
||||
camera_to_calibrate.translation_vector_world_to_camera = t_ref_to_cam
|
||||
camera_to_calibrate.camera_projection_matrix = P_cam
|
||||
# Compose known camera's world pose with the known->target relative pose
|
||||
R_world_known = known_camera.rotation_matrix_world_to_camera
|
||||
t_world_known = known_camera.translation_vector_world_to_camera
|
||||
|
||||
self.P_mats[camera_to_calibrate.index] = camera_to_calibrate.camera_projection_matrix
|
||||
R_world_target = R_known_to_target @ R_world_known
|
||||
t_world_target = R_known_to_target @ t_world_known + t_known_to_target
|
||||
|
||||
target_camera.rotation_matrix_world_to_camera = R_world_target
|
||||
target_camera.translation_vector_world_to_camera = t_world_target
|
||||
target_camera.camera_projection_matrix = target_camera.camera_matrix @ numpy.hstack([R_world_target, t_world_target])
|
||||
|
||||
self.P_mats[target_camera.index] = target_camera.camera_projection_matrix
|
||||
self.logger.log(f"[Extrinsics] cam{target_camera.index} chained via cam{known_camera.index} (stereo_rms={stereo_rms:.4f}, shared_samples={best_count})")
|
||||
|
||||
posed_indices.add(best_target_index)
|
||||
remaining_indices.discard(best_target_index)
|
||||
|
||||
self.logger.log("Extrinsics done")
|
||||
self.logger.log("Amount of P_mats:" + str(len(self.P_mats)))
|
||||
@@ -319,6 +383,15 @@ class HomeWindow(QWidget):
|
||||
def _as_np_float32(self, x):
|
||||
return numpy.asarray(x, dtype=numpy.float32)
|
||||
|
||||
def _count_common_samples(self, camera_index_a: int, camera_index_b: int) -> int:
|
||||
object_points_per_frame, _, _ = self._build_stereo_correspondences_from_samples(
|
||||
sample_list=self.calibration_samples,
|
||||
board=self.board,
|
||||
camera_index_reference=camera_index_a,
|
||||
camera_index_to_calibrate=camera_index_b
|
||||
)
|
||||
return len(object_points_per_frame)
|
||||
|
||||
def _stereo_calibrate_from_charuco_samples(
|
||||
self,
|
||||
sample_list,
|
||||
@@ -328,9 +401,11 @@ class HomeWindow(QWidget):
|
||||
):
|
||||
|
||||
if camera_refernece.camera_matrix is None or camera_refernece.distortion_coefficients is None:
|
||||
self.logger.log(f"cam{camera_refernece.index} missing intrinsics")
|
||||
self.logger.log(f"cam{camera_refernece.index} missing intrinsics, skipping extrinsics.")
|
||||
return None, None, None, None, None, None, None
|
||||
if camera_to_calibrate.camera_matrix is None or camera_to_calibrate.distortion_coefficients is None:
|
||||
self.logger.log(f"cam{camera_to_calibrate.index} missing intrinsics")
|
||||
self.logger.log(f"cam{camera_to_calibrate.index} missing intrinsics, skipping extrinsics.")
|
||||
return None, None, None, None, None, None, None
|
||||
|
||||
object_points_per_frame, frame_points_per_frame_reference, frame_points_per_frame_to_calibrate = self._build_stereo_correspondences_from_samples(
|
||||
sample_list=sample_list,
|
||||
@@ -339,6 +414,10 @@ class HomeWindow(QWidget):
|
||||
camera_index_to_calibrate=camera_to_calibrate.index
|
||||
)
|
||||
|
||||
if len(object_points_per_frame) == 0:
|
||||
self.logger.log(f"cam{camera_refernece.index} <-> cam{camera_to_calibrate.index}: no overlapping calibration samples, skipping extrinsics.")
|
||||
return None, None, None, None, None, None, None
|
||||
|
||||
frame_width, frame_height = FRAME_SIZE
|
||||
|
||||
# Keep intrinsics fixed (recommended since you already calibrated them)
|
||||
@@ -414,6 +493,9 @@ class HomeWindow(QWidget):
|
||||
object_points_per_frame.append(object_points)
|
||||
frame_points_per_frame.append(frame_points)
|
||||
|
||||
if len(object_points_per_frame) == 0:
|
||||
self.logger.log(f"cam{camera_index}: no valid calibration samples, skipping intrinsics.")
|
||||
return None, None, None
|
||||
|
||||
|
||||
frame_width, frame_height = FRAME_SIZE
|
||||
@@ -433,7 +515,11 @@ class HomeWindow(QWidget):
|
||||
def _caputre_calibration_sample(self):
|
||||
detections = {}
|
||||
|
||||
for camera in self.cameraList: # Loop all Cameras
|
||||
for camera in self.cameraList: # Loop all cameras - expects them already connected via "Connect cameras"
|
||||
if camera.capture is None:
|
||||
self.logger.log(f"cam{camera.index}: not connected, click 'Connect cameras' first. Skipping.")
|
||||
continue
|
||||
|
||||
ok, frame = camera.capture.read() #Capture a frame
|
||||
|
||||
if not ok: # A single failed grab shouldn't discard what other cameras saw
|
||||
@@ -446,9 +532,6 @@ class HomeWindow(QWidget):
|
||||
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) # Turn the frame black and white
|
||||
gray = 255 - gray # Invert the color of the frame, as the ChArCuo board I use is inverted (to save printer Ink)
|
||||
|
||||
h, w = gray.shape[:2]
|
||||
self.logger.log(f"cam{camera.index} capture size = {w}x{h}")
|
||||
|
||||
#This is apparently a fast check, bevor i detect the actuall board
|
||||
corner, ids, _ = self.arcuo_detector.detectMarkers(gray) #Detect ChArCuo Markers
|
||||
if ids is None or len(ids) == 0: #Board not visible from this camera this time, skip it, not the whole sample
|
||||
|
||||
@@ -1,7 +1,7 @@
|
||||
import cv2, numpy as np
|
||||
import os
|
||||
|
||||
from Tbd.helper import CameraObject, DShowMJPGCapture
|
||||
from Tbd.helper import CameraObject
|
||||
from UI.UILogger import UILogger
|
||||
|
||||
from PySide6.QtCore import Qt, QTimer
|
||||
@@ -90,39 +90,31 @@ class SetupPage(QWidget):
|
||||
else:
|
||||
self.logger.log("Path not found")
|
||||
|
||||
indexToAdd = [1, 2, 3, 4, 5, 6]
|
||||
|
||||
indexToAdd = [0, 3]
|
||||
#self.cameraObject = CameraObject(
|
||||
# capture=None,
|
||||
# index=None
|
||||
#)
|
||||
# Cameras are verified one at a time and left closed afterwards - this
|
||||
# hardware can't sustain many concurrent capture graphs (confirmed via
|
||||
# both CAP_DSHOW and a raw DirectShow graph), so callers open a camera
|
||||
# right before they need it (CameraSetupWidget.Start, calibration capture)
|
||||
# and close it right after, instead of keeping all 6 open simultaneously.
|
||||
for index in indexToAdd:
|
||||
try:
|
||||
capture = DShowMJPGCapture(index, 2592, 1944, fourcc_substr="MJPG")
|
||||
except RuntimeError as e:
|
||||
self.logger.log(str(e))
|
||||
print(str(e))
|
||||
camera = CameraObject(capture=None, index=index)
|
||||
|
||||
if not camera.open():
|
||||
self.logger.log(f"cam{index}: failed to open, skipping.")
|
||||
continue
|
||||
|
||||
self.logger.log(f"Selected format: {capture.selected_format}")
|
||||
print(f"Selected format: {capture.selected_format}")
|
||||
|
||||
ret, frame = capture.read()
|
||||
ret, frame = camera.capture.read()
|
||||
if ret:
|
||||
self.logger.log(f"Actual frame shape: {frame.shape}")
|
||||
print(f"Actual frame shape: {frame.shape}")
|
||||
self.logger.log(f"cam{index}: verified, frame shape={frame.shape}")
|
||||
else:
|
||||
self.logger.log("Failed to read a frame after configuring camera")
|
||||
print("Failed to read a frame after configuring camera")
|
||||
self.logger.log(f"cam{index}: opened but failed to read a frame.")
|
||||
|
||||
#self.logger.log("Initial frame:", w, "x", h)
|
||||
self.cameraList.append(CameraObject(
|
||||
capture=capture,
|
||||
index=index
|
||||
))
|
||||
camera.close()
|
||||
|
||||
self.cameraList.append(camera)
|
||||
self._cameraList_changed()
|
||||
|
||||
|
||||
return
|
||||
|
||||
|
||||
@@ -177,12 +169,13 @@ class SetupPage(QWidget):
|
||||
index = int(self.cameraIndex.text())
|
||||
|
||||
self.release_camera()
|
||||
self.cameraObject.capture = cv2.VideoCapture(index, cv2.CAP_DSHOW)
|
||||
self.cameraObject.index = index
|
||||
|
||||
if not self.cameraObject.capture.isOpened():
|
||||
self.cameraObject.capture = None
|
||||
self.cameraObject.index = None
|
||||
# Reuse the CameraObject from cameraList (if this index is already known)
|
||||
# instead of opening a second handle to the same physical device.
|
||||
existing = next((cam for cam in self.cameraList if cam.index == index), None)
|
||||
self.cameraObject = existing if existing is not None else CameraObject(capture=None, index=index)
|
||||
|
||||
if not self.cameraObject.open():
|
||||
self.camera_preview.setText(f"Failed to open camera {index}")
|
||||
return False
|
||||
|
||||
@@ -193,9 +186,7 @@ class SetupPage(QWidget):
|
||||
def release_camera(self):
|
||||
if self.cameraObject.capture is not None:
|
||||
self.timer.stop()
|
||||
self.cameraObject.capture.release()
|
||||
self.cameraObject.capture = None
|
||||
self.cameraObject.index = None
|
||||
self.cameraObject.close()
|
||||
|
||||
def closeEvent(self, event):
|
||||
self.release_camera()
|
||||
|
||||
+1
-1
@@ -7,7 +7,6 @@ from pathlib import Path
|
||||
from Tbd.helper import default_profile, CameraObject
|
||||
from UI.SetupPageWidget import SetupPage
|
||||
from UI.HomePageWidget import HomeWindow
|
||||
from UI.GameWindow import GameWindow
|
||||
from UI.UILogger import UILogger
|
||||
from Tbd.TrackingController import TrackingController
|
||||
|
||||
@@ -122,6 +121,7 @@ class MainWindow(QMainWindow):
|
||||
self.btn_exit.clicked.connect(self.close)
|
||||
|
||||
self.game_window = None
|
||||
self.tracking = None
|
||||
|
||||
# --------------------
|
||||
def on_cameraList_changed(self):
|
||||
|
||||
Reference in New Issue
Block a user