Added continous hand tracking

.gitignore

@@ -0,0 +1,2 @@
+__pycache__/
+*.pyc

.vscode/settings.json

@@ -0,0 +1,9 @@
+{
+    "python-envs.pythonProjects": [
+        {
+            "path": ".",
+            "envManager": "ms-python.python:system",
+            "packageManager": "ms-python.python:pip"
+        }
+    ]
+}

Test/Gestures/gesture_config.json

@@ -5,6 +5,12 @@
       "left": "PointIndex",
       "right": "",
       "extensions": []
+    },
+    {
+      "action": "Pointer",
+      "left": "",
+      "right": "PointIndex",
+      "extensions": []
     }
   ]
 }

source/Helpers/ChArUco.py

@@ -2,10 +2,10 @@ import cv2
 import numpy as np
 
 # ---- Board parameters (CHANGE THESE ONLY IF YOU REPRINT) ----
-squares_x = 6        # number of chessboard squares in X
+squares_x = 4        # number of chessboard squares in X
-squares_y = 9        # number of chessboard squares in Y
+squares_y = 5        # number of chessboard squares in Y
-square_length = 25  # mm
+square_length = 50  # mm
-marker_length = 18  # mm (must be < square_length)
+marker_length = 36  # mm (must be < square_length)
 dictionary_id = cv2.aruco.DICT_4X4_50
 dpi = 300            # print DPI
 # ------------------------------------------------------------

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()

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()

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,27 @@ class TrackingController(QObject):
 
         self.running = False
 
-        # init mediapipe here (or inject it)
+        # MediaPipe's legacy solutions.hands API no longer exists on the
-
+        # installed mediapipe version, so hand detection runs on the newer
-        self.mp_hands = mp.solutions.hands
+        # Tasks API instead. Each camera gets its own HandLandmarker running
-        self.hands = self.mp_hands.Hands(
+        # in VIDEO mode, fed only that camera's own frames with a monotonically
-            static_image_mode=False,
+        # increasing per-camera timestamp - this lets MediaPipe track hands
-            max_num_hands=4,
+        # frame-to-frame per view (much less jitter than re-detecting blind
-            model_complexity=1,
+        # every frame), which wouldn't be valid if frames from different
-            min_detection_confidence=0.5,
+        # camera viewpoints were interleaved through a single VIDEO-mode
-            min_tracking_confidence=0.5,
+        # detector.
-        )
+        self._frame_counter = defaultdict(int)
+        self.detectors = {}
+        for camera in self.cameraList:
+            options = HandLandmarkerOptions(
+                base_options=BaseOptions(model_asset_path=MODEL_PATH),
+                running_mode=RunningMode.VIDEO,
+                num_hands=12,
+                min_hand_detection_confidence=0.5,
+                min_hand_presence_confidence=0.5,
+                min_tracking_confidence=0.5,
+            )
+            self.detectors[camera.index] = HandLandmarker.create_from_options(options)
 
     def start(self):
         # Guard: need projections
@@ -45,6 +61,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 +75,13 @@ class TrackingController(QObject):
             return
         self.running = False
         self.timer.stop()
+
+        for camera in self.cameraList:
+            camera.close()
+
+        for detector in self.detectors.values():
+            detector.close()
+
         if self.logger: self.logger.log("[Tracking] Stopped")
 
     def _tick(self):
@@ -81,29 +109,39 @@ class TrackingController(QObject):
     def _detectHands_all_cameras(self):
         out = []
         for camera in self.cameraList:
+            if camera.capture is None:
+                continue
+
+            detector = self.detectors.get(camera.index)
+            if detector 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)
+            mp_image = mp.Image(image_format=mp.ImageFormat.SRGB, data=frameRGB)
-            if not hand_tracking_result.multi_hand_landmarks:
+
+            self._frame_counter[camera.index] += 1
+            timestamp_ms = self._frame_counter[camera.index] * self.tick_ms
+            result = detector.detect_for_video(mp_image, timestamp_ms)
+
+            if not result.hand_landmarks:
                 continue
 
-            handed = hand_tracking_result.multi_handedness
+            for hi, lm_list in enumerate(result.hand_landmarks):
-            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],
+                    [[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:
+                if hi < len(result.handedness) and result.handedness[hi]:
-                    label = handed[hi].classification[0].label
+                    label = result.handedness[hi][0].category_name
-                    score = handed[hi].classification[0].score
+                    score = result.handedness[hi][0].score
 
                 out.append(Hand2D(
                     camera_id=camera.index,

source/Tbd/helper.py

@@ -1,10 +1,93 @@
 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
 
+from pygrabber.dshow_graph import FilterGraph
+
+
+class DShowMJPGCapture:
+    """
+    cv2.VideoCapture-like wrapper (read/release/isOpened) that selects an exact
+    DirectShow media type (e.g. MJPG at a given resolution) before capturing.
+
+    This exists because cv2.VideoCapture(..., cv2.CAP_DSHOW).set(CAP_PROP_FOURCC, ...)
+    is unreliable on some webcams: OpenCV opens its own separate DirectShow filter
+    graph, which renegotiates its own format independently of anything set through
+    pygrabber beforehand. Doing format selection AND frame grabbing in the same
+    graph (via pygrabber's sample grabber) avoids that.
+    """
+
+    def __init__(self, index: int, width: int, height: int,
+                 fourcc_substr: str = "MJPG", timeout: float = 2.0):
+        self.index = index
+        self.width = width
+        self.height = height
+        self.selected_format = None
+        self._opened = False
+        self._latest_frame = None
+        self._frame_event = threading.Event()
+        self._timeout = timeout
+
+        self._graph = FilterGraph()
+        self._graph.add_video_input_device(index)
+
+        device = self._graph.get_input_device()
+        formats = device.get_formats()
+        match = next(
+            (f for f in formats
+             if f["width"] == width and f["height"] == height
+             and fourcc_substr.upper() in f["media_type_str"].upper()),
+            None,
+        )
+        if match is None:
+            raise RuntimeError(
+                f"No {fourcc_substr} format at {width}x{height} available on camera {index}. "
+                f"Available formats: {formats}"
+            )
+        device.set_format(match["index"])
+        self.selected_format = match
+
+        self._graph.add_sample_grabber(self._on_frame)
+        self._graph.add_null_render()
+        self._graph.prepare_preview_graph()
+        self._graph.run()
+        self._opened = True
+
+    def _on_frame(self, frame):
+        self._latest_frame = frame
+        self._frame_event.set()
+
+    def isOpened(self) -> bool:
+        return self._opened
+
+    def read(self):
+        if not self._opened:
+            return False, None
+        self._frame_event.clear()
+        self._graph.grab_frame()
+        if not self._frame_event.wait(self._timeout):
+            return False, None
+        return True, self._latest_frame
+
+    def release(self):
+        if self._opened:
+            self._graph.stop()
+            self._graph.remove_filters()
+            self._opened = False
+
+    # No-ops for compatibility with code paths that still call .set()/.get()
+    # on the capture object (real config happens via device.set_format above).
+    def set(self, prop_id, value):
+        return False
+
+    def get(self, prop_id):
+        return 0.0
+
 
 # ----- Helper Functions -----
 def compute_distance(p1, p2):
@@ -41,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():

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

source/UI/GameWindow.py

@@ -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)
-

source/UI/HomePageWidget.py

@@ -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")
+        self.logger.log("Start extrinsics")
-        # Reference camera projection
+        # 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_refernece=known_camera,
-                camera_to_calibrate=camera_to_calibrate
+                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
+            # Compose known camera's world pose with the known->target relative pose
-            camera_to_calibrate.translation_vector_world_to_camera = t_ref_to_cam
+            R_world_known = known_camera.rotation_matrix_world_to_camera
-            camera_to_calibrate.camera_projection_matrix = P_cam
+            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,12 +515,16 @@ 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: # Check if captured frame is valid, if even one frame is invalide the whole capture failed
+            if not ok: # A single failed grab shouldn't discard what other cameras saw
                 self.logger.log(f"Could not grab frame for calibration sample, from camera {camera.index}.")
-                return
+                continue
 
             h, w = frame.shape[:2]
             self.logger.log(f"cam{camera.index} capture size = {w}x{h}")
@@ -446,14 +532,11 @@ 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: #Check if Markers where found, if none where found the sample is invalid
+            if ids is None or len(ids) == 0: #Board not visible from this camera this time, skip it, not the whole sample
                 self.logger.log(f"Not enought markers found for calibration sample, from camera {camera.index}.")
-                return
+                continue
 
             # Detecting the Charcuo board and evaluating if its good as a sample
             result = self.charuco_detector.detectBoard(gray)
@@ -463,11 +546,11 @@ class HomeWindow(QWidget):
 
             if charuco_ids is None:
                 self.logger.log(f"Not enought markers found for calibration sample, from camera {camera.index}.")
-                return
+                continue
 
             if len(charuco_ids) < MIN_CHARUCO_CORNERS:
                 self.logger.log(f"Not enought markers found for calibration sample, from camera {camera.index}.")
-                return
+                continue
 
             # Adding the sample for the current camera to detections
             detections[camera.index] = CharucoDetection(
@@ -477,12 +560,18 @@ class HomeWindow(QWidget):
                 gray_frame = gray
             )
 
-        # All cameras found valide markers so the sample capture was succesfull
+        # Need at least two cameras' worth of detections for this sample to be
+        # useful for stereo pairing (a single-camera detection can still help
+        # that camera's intrinsics, but can't contribute to any camera pair).
+        if len(detections) < 2:
+            self.logger.log("Not enough cameras saw the board in this sample, discarding.")
+            return
+
         self.calibration_samples.append(
             CalibrationSample(detections=detections)
         )
 
-        self.logger.log("Added calibration sample.")
+        self.logger.log(f"Added calibration sample ({len(detections)}/{len(self.cameraList)} cameras saw the board).")
 
     def updateCameras(self):
         self._clearLayout()

source/UI/SetupPageWidget.py

@@ -1,11 +1,10 @@
 import cv2, numpy as np
 import os
 
-
 from Tbd.helper import CameraObject
 from UI.UILogger import UILogger
 
-from PySide6.QtCore import Qt, QTimer
+from PySide6.QtCore import Qt, QTimer, QThread, Signal
 from PySide6.QtGui import QImage, QPixmap
 from PySide6.QtWidgets import QLineEdit
 from PySide6.QtGui import QIntValidator
@@ -13,6 +12,35 @@ from PySide6.QtWidgets import QWidget, QLabel, QVBoxLayout, QPushButton, QHBoxLa
 from typing import List
 
 
+class LoadCamerasWorker(QThread):
+    """
+    Opens/verifies/closes each camera off the UI thread - camera.open() does
+    MSMF format negotiation with retries+sleeps, which used to block the Qt
+    event loop (window shows as "Not Responding") for the whole duration.
+    """
+    cameraOpened = Signal(object)
+    logMessage = Signal(str)
+
+    def __init__(self, indexToAdd: List[int]):
+        super().__init__()
+        self.indexToAdd = indexToAdd
+
+    def run(self):
+        for index in self.indexToAdd:
+            camera = CameraObject(capture=None, index=index)
+
+            if not camera.open():
+                self.logMessage.emit(f"cam{index}: failed to open, skipping.")
+                continue
+
+            ret, frame = camera.capture.read()
+            if ret:
+                self.logMessage.emit(f"cam{index}: verified, frame shape={frame.shape}")
+            else:
+                self.logMessage.emit(f"cam{index}: opened but failed to read a frame.")
+
+            camera.close()
+            self.cameraOpened.emit(camera)
 
 
 class SetupPage(QWidget):
@@ -91,47 +119,23 @@ class SetupPage(QWidget):
         else:
             self.logger.log("Path not found")
 
+        indexToAdd = [1, 2, 3, 4, 5, 6]
+        #indexToAdd = [0, 3]
+        # 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.
+        self.loadBTN.setEnabled(False)
+        self._loadWorker = LoadCamerasWorker(indexToAdd)
+        self._loadWorker.logMessage.connect(self.logger.log)
+        self._loadWorker.cameraOpened.connect(self._onCameraLoaded)
+        self._loadWorker.finished.connect(lambda: self.loadBTN.setEnabled(True))
+        self._loadWorker.start()
 
-        indexToAdd =  [0, 3]
+    def _onCameraLoaded(self, camera: CameraObject):
-        #self.cameraObject = CameraObject(
+        self.cameraList.append(camera)
-        #    capture=None,
+        self._cameraList_changed()
-        #    index=None
-        #)
-        for index in indexToAdd:
-            capture = cv2.VideoCapture(index, cv2.CAP_MSMF)
-            #capture = cv2.VideoCapture(index, cv2.CAP_DSHOW)
-
-            capture.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*'MJPG'))
-            capture.set(cv2.CAP_PROP_FPS, 30)
-            capture.set(cv2.CAP_PROP_FRAME_WIDTH, 2592)
-            capture.set(cv2.CAP_PROP_FRAME_HEIGHT, 1944)
-
-            #capture.set(cv2.CAP_PROP_FRAME_WIDTH, 2048.0)
-            #capture.set(cv2.CAP_PROP_FRAME_HEIGHT, 1536.0)
-            width = capture.get(cv2.CAP_PROP_FRAME_WIDTH)
-            height = capture.get(cv2.CAP_PROP_FRAME_HEIGHT)
-            fps = capture.get(cv2.CAP_PROP_FPS)
-
-            fourcc = int(capture.get(cv2.CAP_PROP_FOURCC))
-            fourcc_str = "".join([chr((fourcc >> 8*i) & 0xFF) for i in range(4)])
-
-            print(f"Resolution: {width}x{height}")
-            print(f"FPS: {fps}")
-            print(f"Format (FOURCC): {fourcc_str}")
-            self.logger.log(f"Resolution: {width}x{height}")
-            self.logger.log(f"FPS: {fps}")
-            self.logger.log(f"Format (FOURCC): {fourcc_str}")
-            #currentcapture = cv2.VideoCapture(index, cv2.CAP_MSMF)
-            
-            #self.logger.log("Initial frame:", w, "x", h)
-            self.cameraList.append(CameraObject(
-                capture=capture,
-                index=index
-            ))
-            self._cameraList_changed()
-
-        
-        return
 
         
         
@@ -185,12 +189,13 @@ class SetupPage(QWidget):
         index = int(self.cameraIndex.text())
 
         self.release_camera()
-        self.cameraObject.capture = cv2.VideoCapture(index)
-        self.cameraObject.index = index
 
-        if not self.cameraObject.capture.isOpened():
+        # Reuse the CameraObject from cameraList (if this index is already known)
-            self.cameraObject.capture = None
+        # instead of opening a second handle to the same physical device.
-            self.cameraObject.index = None
+        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
 
@@ -201,9 +206,7 @@ class SetupPage(QWidget):
     def release_camera(self):
         if self.cameraObject.capture is not None:
             self.timer.stop()
-            self.cameraObject.capture.release()
+            self.cameraObject.close()
-            self.cameraObject.capture = None
-            self.cameraObject.index = None
 
     def closeEvent(self, event):
         self.release_camera()

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):