diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..7a60b85 --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +__pycache__/ +*.pyc diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..4988ec0 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,9 @@ +{ + "python-envs.pythonProjects": [ + { + "path": ".", + "envManager": "ms-python.python:system", + "packageManager": "ms-python.python:pip" + } + ] +} \ No newline at end of file diff --git a/source/Helpers/Multi_Camera_Stream_Test.py b/source/Helpers/Multi_Camera_Stream_Test.py new file mode 100644 index 0000000..8bbf12e --- /dev/null +++ b/source/Helpers/Multi_Camera_Stream_Test.py @@ -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() diff --git a/source/Helpers/Multi_Camera_Stream_Test_Threaded.py b/source/Helpers/Multi_Camera_Stream_Test_Threaded.py new file mode 100644 index 0000000..441f0c6 --- /dev/null +++ b/source/Helpers/Multi_Camera_Stream_Test_Threaded.py @@ -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() diff --git a/source/Tbd/TrackingController.py b/source/Tbd/TrackingController.py index e533ace..ec15bc9 100644 --- a/source/Tbd/TrackingController.py +++ b/source/Tbd/TrackingController.py @@ -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,30 +99,33 @@ 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, handedness=label, diff --git a/source/Tbd/helper.py b/source/Tbd/helper.py index 9cce31a..f289faf 100644 --- a/source/Tbd/helper.py +++ b/source/Tbd/helper.py @@ -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 @@ -122,7 +123,39 @@ class CameraObject(): #self.capture = cv2.VideoCapture(self.index, cv2.CAP_DSHOW) 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(): diff --git a/source/Tbd/models/hand_landmarker.task b/source/Tbd/models/hand_landmarker.task new file mode 100644 index 0000000..0d53faf Binary files /dev/null and b/source/Tbd/models/hand_landmarker.task differ diff --git a/source/UI/CameraSetupWidget.py b/source/UI/CameraSetupWidget.py index 5b68584..a94afe2 100644 --- a/source/UI/CameraSetupWidget.py +++ b/source/UI/CameraSetupWidget.py @@ -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 diff --git a/source/UI/GameWindow.py b/source/UI/GameWindow.py deleted file mode 100644 index 4474df7..0000000 --- a/source/UI/GameWindow.py +++ /dev/null @@ -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) - diff --git a/source/UI/HomePageWidget.py b/source/UI/HomePageWidget.py index 72565e9..db2bedb 100644 --- a/source/UI/HomePageWidget.py +++ b/source/UI/HomePageWidget.py @@ -69,7 +69,9 @@ 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 self.scroll_Box.setWidgetResizable(True) @@ -79,21 +81,43 @@ 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) self.main_Layout.addWidget(self.caputre_calibration_sample_button) self.main_Layout.addWidget(self.calibration_button) self.main_Layout.addWidget(self.scroll_Box) - + # Connecting Buttons to functions self.calibration_button.clicked.connect(self._calibrate_camera) self.caputre_calibration_sample_button.clicked.connect(self._caputre_calibration_sample) 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,44 +158,84 @@ 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 - - stereo_rms,R_ref_to_cam,t_ref_to_cam,E, F, P_ref, P_cam = self._stereo_calibrate_from_charuco_samples( + # 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 + + 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 + ) - 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 + 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 - self.P_mats[camera_to_calibrate.index] = camera_to_calibrate.camera_projection_matrix + # 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 + + 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))) self.logger.log("Amount of cameras:" + str(len(self.cameraList))) @@ -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,8 +493,11 @@ 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 self.logger.log(f"Calibrating cam{camera_index} with imageSize={FRAME_SIZE} (w,h)") self.logger.log(f"H= {frame_width} W= {frame_height}") @@ -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 diff --git a/source/UI/SetupPageWidget.py b/source/UI/SetupPageWidget.py index 065cb5a..5d3d7d7 100644 --- a/source/UI/SetupPageWidget.py +++ b/source/UI/SetupPageWidget.py @@ -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 = [0, 3] - #self.cameraObject = CameraObject( - # capture=None, - # index=None - #) + indexToAdd = [1, 2, 3, 4, 5, 6] + + # 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,15 +169,16 @@ 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 - + self.camera_preview.setText("") return True @@ -193,10 +186,8 @@ 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() event.accept() diff --git a/source/main.py b/source/main.py index 4dd8e51..2121096 100644 --- a/source/main.py +++ b/source/main.py @@ -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):