DuOtto
506 lines
21 KiB
Python
506 lines
21 KiB
Python
import cv2
|
|
import numpy
|
|
|
|
from Tbd.helper import CameraObject, CharucoDetection, CalibrationSample
|
|
from UI.CameraSetupWidget import CameraSetupWidget
|
|
from Helpers import saveManager as sm
|
|
|
|
from PySide6.QtCore import Qt, QTimer
|
|
from PySide6.QtGui import QImage, QPixmap
|
|
|
|
from PySide6.QtWidgets import QWidget, QLabel, QVBoxLayout, QSizePolicy
|
|
from typing import List
|
|
from PySide6.QtWidgets import QScrollArea, QFrame, QPushButton
|
|
|
|
from UI.UILogger import UILogger
|
|
|
|
MIN_CHARUCO_CORNERS = 15 # absolute minimum
|
|
#GOOD_CHARUCO_CORNERS = 25 # ideal
|
|
|
|
DICT_ID = cv2.aruco.DICT_4X4_50
|
|
SQUARES_X = 6 # number of chessboard squares in X
|
|
SQUARES_Y = 9 # number of chessboard squares in Y
|
|
SQUARE_LEN_MM = 25.0 # square size in mm
|
|
MARKER_LEN_MM = 18.0 # marker size in mm (must be < square)
|
|
|
|
#FRAME_SIZE = [3264, 2448]
|
|
#FRAME_SIZE = [1536, 2048]
|
|
FRAME_SIZE = [2048, 1536]
|
|
|
|
REFERENCE_CAMERA_INDEX = 1
|
|
|
|
class HomeWindow(QWidget):
|
|
|
|
def __init__(self, cameraList: List[CameraObject], on_cameraList_changed, P_mats, logger : UILogger):
|
|
super().__init__()
|
|
|
|
# Initiating variables
|
|
self.P_mats = P_mats
|
|
self.on_cameraList_changed = on_cameraList_changed
|
|
self.cameraList = cameraList
|
|
self.logger = logger
|
|
self.calibration_samples: list[CalibrationSample] = []
|
|
self.camera_Setup_Widget_List = []
|
|
|
|
|
|
# Charcuo detection stuff
|
|
dictionary = cv2.aruco.getPredefinedDictionary(DICT_ID)
|
|
self.board = cv2.aruco.CharucoBoard(
|
|
(SQUARES_X, SQUARES_Y),
|
|
SQUARE_LEN_MM,
|
|
MARKER_LEN_MM,
|
|
dictionary
|
|
)
|
|
self.charuco_detector = cv2.aruco.CharucoDetector(self.board)
|
|
# Detector parameters
|
|
detector_params = cv2.aruco.DetectorParameters()
|
|
# You can tweak these if detection is unstable:
|
|
# detector_params.adaptiveThreshWinSizeMin = 3
|
|
# detector_params.adaptiveThreshWinSizeMax = 23
|
|
# detector_params.adaptiveThreshWinSizeStep = 10
|
|
|
|
self.arcuo_detector = cv2.aruco.ArucoDetector(dictionary, detector_params)
|
|
|
|
# Creating the main_Layout and other Widgets
|
|
self.main_Layout = QVBoxLayout(self)
|
|
self.scroll_Box = QScrollArea()
|
|
self.calibration_button = QPushButton("Calibrate")
|
|
self.caputre_calibration_sample_button = QPushButton("Capture calibration sample")
|
|
self.clear_calibration_samples_button = QPushButton("Clear calibration samples")
|
|
self.load_calibration_button = QPushButton("Load calibration")
|
|
self.save_calibration_button = QPushButton("Save calibration")
|
|
|
|
|
|
# Creating the Scrollable area
|
|
self.scroll_Box.setWidgetResizable(True)
|
|
self.scroll_Content = QWidget()
|
|
self.scroll_Layout = QVBoxLayout(self.scroll_Content)
|
|
self.scroll_Layout.setAlignment(Qt.AlignTop)
|
|
self.scroll_Box.setWidget(self.scroll_Content)
|
|
|
|
# Adding Primary show elements
|
|
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)
|
|
|
|
|
|
def _clear_calibration_samples(self):
|
|
self.calibration_samples.clear()
|
|
self.logger.log("Calibration samples where cleared.")
|
|
|
|
def _load_calibration(self):
|
|
loaded = sm.load_calibration_json("C:\\git\\Table\\Test\\test.json", self.logger)
|
|
self.cameraList.clear()
|
|
self.cameraList.extend(loaded)
|
|
self.P_mats.clear()
|
|
self.P_mats.update({
|
|
camera.index: camera.camera_projection_matrix
|
|
for camera in self.cameraList
|
|
if camera.camera_projection_matrix is not None
|
|
})
|
|
self.on_cameraList_changed()
|
|
|
|
def _save_calibration(self):
|
|
sm.save_calibration_json(self.cameraList, "C:\\git\\Table\\Test\\test.json", self.logger)
|
|
|
|
def _calibrate_camera(self):
|
|
self.logger.log("Fuck you")
|
|
|
|
if(len(self.cameraList) < 2):
|
|
self.logger.log("You need at least 2 cameras for calibration")
|
|
return
|
|
|
|
self._clear_camera_calibrations()
|
|
|
|
REFERENCE_CAMERA_INDEX = self.cameraList[0].index
|
|
|
|
for camera in self.cameraList:
|
|
reprojection_error_rms, camera_matrix, distortion_coefficients = self._calibrate_intrinsics_characuo_for_camera(
|
|
sample_list = self.calibration_samples,
|
|
camera_index = camera.index,
|
|
board = self.board
|
|
)
|
|
|
|
camera.camera_matrix = camera_matrix
|
|
camera.distortion_coefficients = distortion_coefficients
|
|
|
|
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)
|
|
|
|
self.logger.log("Intrinsics done")
|
|
|
|
self.logger.log("Start extrinsics1")
|
|
# Reference camera projection
|
|
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(
|
|
sample_list=self.calibration_samples,
|
|
board=self.board,
|
|
camera_refernece=reference_camera,
|
|
camera_to_calibrate=camera_to_calibrate
|
|
)
|
|
|
|
|
|
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
|
|
|
|
self.P_mats[camera_to_calibrate.index] = camera_to_calibrate.camera_projection_matrix
|
|
|
|
|
|
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)))
|
|
self.log_all_camera_calibration(self.cameraList)
|
|
|
|
|
|
def rotation_matrix_to_rpy_deg(self, R: numpy.ndarray):
|
|
"""
|
|
Convert rotation matrix to roll, pitch, yaw in degrees.
|
|
Assumes right-handed, OpenCV convention.
|
|
"""
|
|
sy = numpy.sqrt(R[0,0]*R[0,0] + R[1,0]*R[1,0])
|
|
singular = sy < 1e-6
|
|
|
|
if not singular:
|
|
roll = numpy.arctan2(R[2,1], R[2,2])
|
|
pitch = numpy.arctan2(-R[2,0], sy)
|
|
yaw = numpy.arctan2(R[1,0], R[0,0])
|
|
else:
|
|
roll = numpy.arctan2(-R[1,2], R[1,1])
|
|
pitch = numpy.arctan2(-R[2,0], sy)
|
|
yaw = 0.0
|
|
|
|
return numpy.degrees([roll, pitch, yaw])
|
|
|
|
def log_all_camera_calibration(self, camera_list):
|
|
self.logger.log("========== CAMERA CALIBRATION SUMMARY ==========")
|
|
|
|
for cam in camera_list:
|
|
self.logger.log(f"--- Camera {cam.index} ---")
|
|
|
|
# -------- Intrinsics --------
|
|
if cam.camera_matrix is not None:
|
|
K = cam.camera_matrix
|
|
fx, fy = K[0,0], K[1,1]
|
|
cx, cy = K[0,2], K[1,2]
|
|
|
|
self.logger.log(
|
|
f"Intrinsics:"
|
|
f" fx={fx:.2f}, fy={fy:.2f},"
|
|
f" cx={cx:.2f}, cy={cy:.2f}"
|
|
)
|
|
|
|
if cam.distortion_coefficients is not None:
|
|
d = cam.distortion_coefficients.flatten()
|
|
d_short = ", ".join(f"{v:.4f}" for v in d[:5])
|
|
self.logger.log(f"Distortion: [{d_short}{'...' if len(d) > 5 else ''}]")
|
|
else:
|
|
self.logger.log("Intrinsics: NOT SET")
|
|
|
|
# -------- Extrinsics --------
|
|
if cam.rotation_matrix_world_to_camera is not None and cam.translation_vector_world_to_camera is not None:
|
|
R = cam.rotation_matrix_world_to_camera
|
|
t = cam.translation_vector_world_to_camera.reshape(3)
|
|
|
|
roll, pitch, yaw = self.rotation_matrix_to_rpy_deg(R)
|
|
dist = numpy.linalg.norm(t)
|
|
|
|
self.logger.log(
|
|
f"Extrinsics (world → cam):"
|
|
f" t=({t[0]:.1f}, {t[1]:.1f}, {t[2]:.1f})"
|
|
f" | |t|={dist:.1f}"
|
|
)
|
|
self.logger.log(
|
|
f"Rotation (deg):"
|
|
f" roll={roll:.2f}, pitch={pitch:.2f}, yaw={yaw:.2f}"
|
|
)
|
|
else:
|
|
self.logger.log("Extrinsics: NOT SET")
|
|
|
|
# -------- Projection --------
|
|
if cam.camera_projection_matrix is not None:
|
|
P = cam.camera_projection_matrix
|
|
self.logger.log(f"Projection matrix: shape={P.shape}")
|
|
else:
|
|
self.logger.log("Projection matrix: NOT SET")
|
|
|
|
self.logger.log("==============================================")
|
|
|
|
def _build_stereo_correspondences_from_samples(
|
|
self,
|
|
sample_list,
|
|
board,
|
|
camera_index_reference,
|
|
camera_index_to_calibrate
|
|
):
|
|
object_points_per_frame = []
|
|
frame_points_per_frame_reference = []
|
|
frame_points_per_frame_to_calibrate = []
|
|
|
|
for sample in sample_list:
|
|
|
|
# Get detections for both cameras and check if valide
|
|
detection_reference = sample.detections.get(camera_index_reference)
|
|
detection_to_calibrate = sample.detections.get(camera_index_to_calibrate)
|
|
|
|
if detection_reference is None or detection_to_calibrate is None:
|
|
continue
|
|
|
|
if detection_reference.charuco_ids is None or detection_reference.charuco_corners is None:
|
|
continue
|
|
|
|
if detection_to_calibrate.charuco_ids is None or detection_to_calibrate.charuco_corners is None:
|
|
continue
|
|
|
|
# Find corner IDs that both cameras can see and check if the amount is enough
|
|
ids_reference = detection_reference.charuco_ids.reshape(-1)
|
|
ids_to_calibrate = detection_to_calibrate.charuco_ids.reshape(-1)
|
|
|
|
common_ids = numpy.intersect1d(ids_reference, ids_to_calibrate)
|
|
|
|
if len(common_ids) < MIN_CHARUCO_CORNERS:
|
|
continue
|
|
|
|
# Build ordered correspondences by common_ids
|
|
# Map id -> corner for each cam
|
|
map_a = {int(i): detection_reference.charuco_corners[idx] for idx, i in enumerate(ids_reference)}
|
|
map_b = {int(i): detection_to_calibrate.charuco_corners[idx] for idx, i in enumerate(ids_to_calibrate)}
|
|
|
|
# Assemble corners/ids arrays in matching order
|
|
corners_reference = numpy.array([map_a[int(i)] for i in common_ids], dtype=numpy.float32).reshape(-1, 1, 2)
|
|
corners_to_calibrate = numpy.array([map_b[int(i)] for i in common_ids], dtype=numpy.float32).reshape(-1, 1, 2)
|
|
ids_common = common_ids.astype(numpy.int32).reshape(-1, 1)
|
|
|
|
# Convert ChArUco corners+ids -> (objectPts, imagePts) for the board
|
|
object_points_reference, frame_pts_reference = board.matchImagePoints(corners_reference, ids_common)
|
|
_, frame_pts_to_calibrate = board.matchImagePoints(corners_to_calibrate, ids_common)
|
|
|
|
if object_points_reference is None or frame_pts_reference is None or frame_pts_to_calibrate is None:
|
|
continue
|
|
|
|
object_points_reference = self._as_np_float32(object_points_reference)
|
|
frame_pts_reference = self._as_np_float32(frame_pts_reference)
|
|
frame_pts_to_calibrate = self._as_np_float32(frame_pts_to_calibrate)
|
|
|
|
if len(object_points_reference) < MIN_CHARUCO_CORNERS:
|
|
continue
|
|
|
|
object_points_per_frame.append(object_points_reference)
|
|
frame_points_per_frame_reference.append(frame_pts_reference)
|
|
frame_points_per_frame_to_calibrate.append(frame_pts_to_calibrate)
|
|
|
|
return object_points_per_frame, frame_points_per_frame_reference, frame_points_per_frame_to_calibrate
|
|
|
|
def _as_np_float32(self, x):
|
|
return numpy.asarray(x, dtype=numpy.float32)
|
|
|
|
def _stereo_calibrate_from_charuco_samples(
|
|
self,
|
|
sample_list,
|
|
board,
|
|
camera_refernece,
|
|
camera_to_calibrate
|
|
):
|
|
|
|
if camera_refernece.camera_matrix is None or camera_refernece.distortion_coefficients is None:
|
|
self.logger.log(f"cam{camera_refernece.index} missing intrinsics")
|
|
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")
|
|
|
|
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,
|
|
board=board,
|
|
camera_index_reference=camera_refernece.index,
|
|
camera_index_to_calibrate=camera_to_calibrate.index
|
|
)
|
|
|
|
frame_width, frame_height = FRAME_SIZE
|
|
|
|
# Keep intrinsics fixed (recommended since you already calibrated them)
|
|
flags = cv2.CALIB_FIX_INTRINSIC
|
|
|
|
# Termination criteria for stereo calibration optimizer
|
|
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 1e-6)
|
|
|
|
stereo_rms_reprojection_error_px, camera_matrix_reference, dist_reference, camera_matrix_to_calibrate, dist_to_calibrate, \
|
|
rotation_matrix_reference_to_calibrate, translation_vector_reference_to_calibrate, \
|
|
essential_matrix, fundamental_matrix = cv2.stereoCalibrate(
|
|
objectPoints=object_points_per_frame,
|
|
imagePoints1=frame_points_per_frame_reference,
|
|
imagePoints2=frame_points_per_frame_to_calibrate,
|
|
cameraMatrix1=camera_refernece.camera_matrix,
|
|
distCoeffs1=camera_refernece.distortion_coefficients,
|
|
cameraMatrix2=camera_to_calibrate.camera_matrix,
|
|
distCoeffs2=camera_to_calibrate.distortion_coefficients,
|
|
imageSize=(frame_width, frame_height),
|
|
criteria=criteria,
|
|
flags=flags
|
|
)
|
|
|
|
projection_matrix_camA = camera_refernece.camera_matrix @ numpy.hstack([numpy.eye(3, dtype=numpy.float64), numpy.zeros((3,1), dtype=numpy.float64)])
|
|
projection_matrix_camB = camera_to_calibrate.camera_matrix @ numpy.hstack([rotation_matrix_reference_to_calibrate, translation_vector_reference_to_calibrate])
|
|
|
|
|
|
|
|
return (
|
|
float(stereo_rms_reprojection_error_px),
|
|
rotation_matrix_reference_to_calibrate,
|
|
translation_vector_reference_to_calibrate,
|
|
essential_matrix,
|
|
fundamental_matrix,
|
|
projection_matrix_camA,
|
|
projection_matrix_camB,
|
|
)
|
|
|
|
def _clear_camera_calibrations(self):
|
|
for camera in self.cameraList:
|
|
camera.camera_matrix = None
|
|
camera.distortion_coefficients = None
|
|
camera.rotation_matrix_world_to_camera = None
|
|
camera.translation_vector_world_to_camera = None
|
|
|
|
def _calibrate_intrinsics_characuo_for_camera(
|
|
self,
|
|
sample_list: CalibrationSample,
|
|
camera_index: int,
|
|
board
|
|
):
|
|
object_points_per_frame = []
|
|
frame_points_per_frame = []
|
|
|
|
for sample in sample_list:
|
|
detection = sample.detections.get(camera_index)
|
|
|
|
if detection is None:
|
|
continue
|
|
|
|
if detection.charuco_ids is None or detection.charuco_corners is None:
|
|
continue
|
|
|
|
if len(detection.charuco_ids) < MIN_CHARUCO_CORNERS:
|
|
continue
|
|
|
|
# map detected 2D corners + ids to corresponding 3D board points
|
|
object_points, frame_points = board.matchImagePoints(detection.charuco_corners, detection.charuco_ids)
|
|
|
|
if object_points is None or frame_points is None:
|
|
continue
|
|
|
|
object_points_per_frame.append(object_points)
|
|
frame_points_per_frame.append(frame_points)
|
|
|
|
|
|
|
|
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}")
|
|
|
|
reprojection_error_rms, camera_matrix, distortion_coefficients, rvecs, tvecs = cv2.calibrateCamera(
|
|
objectPoints=object_points_per_frame,
|
|
imagePoints=frame_points_per_frame,
|
|
imageSize=(frame_width, frame_height),
|
|
cameraMatrix=None,
|
|
distCoeffs=None
|
|
)
|
|
|
|
return float(reprojection_error_rms), camera_matrix, distortion_coefficients
|
|
|
|
def _caputre_calibration_sample(self):
|
|
detections = {}
|
|
|
|
for camera in self.cameraList: # Loop all Cameras
|
|
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
|
|
self.logger.log(f"Could not grab frame for calibration sample, from camera {camera.index}.")
|
|
return
|
|
|
|
h, w = frame.shape[:2]
|
|
self.logger.log(f"cam{camera.index} capture size = {w}x{h}")
|
|
|
|
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
|
|
self.logger.log(f"Not enought markers found for calibration sample, from camera {camera.index}.")
|
|
return
|
|
|
|
# Detecting the Charcuo board and evaluating if its good as a sample
|
|
result = self.charuco_detector.detectBoard(gray)
|
|
|
|
charuco_corners = result[0]
|
|
charuco_ids = result[1]
|
|
|
|
if charuco_ids is None:
|
|
self.logger.log(f"Not enought markers found for calibration sample, from camera {camera.index}.")
|
|
return
|
|
|
|
if len(charuco_ids) < MIN_CHARUCO_CORNERS:
|
|
self.logger.log(f"Not enought markers found for calibration sample, from camera {camera.index}.")
|
|
return
|
|
|
|
# Adding the sample for the current camera to detections
|
|
detections[camera.index] = CharucoDetection(
|
|
camera_id = camera.index,
|
|
charuco_corners = charuco_corners,
|
|
charuco_ids=charuco_ids,
|
|
gray_frame = gray
|
|
)
|
|
|
|
# All cameras found valide markers so the sample capture was succesfull
|
|
self.calibration_samples.append(
|
|
CalibrationSample(detections=detections)
|
|
)
|
|
|
|
self.logger.log("Added calibration sample.")
|
|
|
|
def updateCameras(self):
|
|
self._clearLayout()
|
|
|
|
for camera_Object in self.cameraList:
|
|
camera_Setup_Widget = CameraSetupWidget(camera_Object)
|
|
|
|
self.scroll_Layout.addWidget(camera_Setup_Widget)
|
|
self.camera_Setup_Widget_List.append(camera_Setup_Widget)
|
|
|
|
def _clearLayout(self):
|
|
self.camera_Setup_Widget_List.clear()
|
|
|
|
while self.scroll_Layout.count():
|
|
item = self.scroll_Layout.takeAt(0)
|
|
widget = item.widget()
|
|
if widget is not None:
|
|
widget.setParent(None)
|
|
widget.deleteLater()
|
|
|