Ottos_Projects/Table-Python
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fix camera capture reliability and rewrite tracking for available mediapipe/hardware

Browse Source
This commit is contained in:
otto
2026-07-31 14:01:06 +02:00
parent 2e6f444302
commit db5d09e156
12 changed files with 400 additions and 420 deletions
Download Patch File Download Diff File Expand all files Collapse all files
source/UI/CameraSetupWidget.py
+9 -3
View File
@@ -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
-333
View File
@@ -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
+111 -28
View File
@@ -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
source/UI/SetupPageWidget.py
+27 -36
View File
@@ -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()