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/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