From 51ffd9c1ee322e2b42e16ff4d1f5960628d2e6cc Mon Sep 17 00:00:00 2001 From: Jens Ahrensfeld Date: Wed, 10 Dec 2025 18:51:30 +0100 Subject: [PATCH] - refactored calibration into separate class - load/store camera calibration for physical camera using device name - load/store camera calibration for synthetic images using project name --- ar_tag_pose/calibration.py | 56 +++++++++++++++++++++++++++ ar_tag_pose/detector_board.py | 42 +++----------------- ar_tag_pose/detector_board_aruco.py | 14 ++++--- ar_tag_pose/detector_board_charuco.py | 16 ++++---- ar_tag_pose/detector_board_main.py | 52 +++++++------------------ 5 files changed, 91 insertions(+), 89 deletions(-) create mode 100644 ar_tag_pose/calibration.py diff --git a/ar_tag_pose/calibration.py b/ar_tag_pose/calibration.py new file mode 100644 index 0000000..3effae6 --- /dev/null +++ b/ar_tag_pose/calibration.py @@ -0,0 +1,56 @@ +import numpy as np +import cv2 as cv + + +class Calibration: + def __init__(self): + self.mtx = None + self.dist = None + self.obj_points_list = [] + self.img_points_list = [] + + def calibration_clear(self): + self.mtx = None + self.dist = None + self.obj_points_list = [] + self.img_points_list = [] + + def is_calibrated(self): + return self.mtx is not None and self.dist is not None + + def get(self): + return self.mtx, self.dist + + def set(self, mtx, dist): + self.mtx = mtx + self.dist = dist + + def load(self, filename: str): + try: + with np.load(filename) as X: + mtx, dist = [X[i] for i in ('mtx', 'dist')] + self.set(mtx, dist) + print(f"Loaded {filename} successfully!") + except FileNotFoundError: + pass + except KeyError: + pass + + def save(self, filename: str): + mtx, dist = self.get() + np.savez(f"{filename}", mtx=mtx, dist=dist) + calib = {'mtx': mtx.tolist(), 'dist': dist.tolist()} + print(f"Saved {filename} successfully!") + + def process(self, image: np.ndarray, r_vecs=None, t_vecs=None, flags=0): + success = False + if len(self.obj_points_list) > 0 and len(self.img_points_list) > 0: + ret, mtx, dist, r_vecs, t_vecs = cv.calibrateCamera(self.obj_points_list, self.img_points_list, image.shape, self.mtx, self.dist, + rvecs=r_vecs, tvecs=t_vecs, flags=flags) + + if ret < 3: + success = True + self.mtx = mtx + self.dist = dist + + return success diff --git a/ar_tag_pose/detector_board.py b/ar_tag_pose/detector_board.py index b5b6aff..aa89fd1 100644 --- a/ar_tag_pose/detector_board.py +++ b/ar_tag_pose/detector_board.py @@ -1,6 +1,6 @@ import abc -import cv2 as cv import numpy as np +from calibration import Calibration RESULTS_FOLDER = "results" @@ -9,11 +9,7 @@ class BoardDetector(abc.ABC): return self.name def __init__(self, name: str, board_id: str, n_syms: int): - self.mtx = None - self.dist = None self.name = name - self.obj_points_list = [] - self.img_points_list = [] self.board_id = board_id # Create IDs @@ -46,42 +42,14 @@ class BoardDetector(abc.ABC): pass @abc.abstractmethod - def process(self, image: np.ndarray, draw_marker_box=True, draw_marker_id=False, draw_marker_axis=False, pose_center: bool = True) -> (bool, np.ndarray, np.ndarray): + def process(self, image: np.ndarray, calibration: Calibration, draw_marker_box=True, draw_marker_id=False, draw_marker_axis=False, pose_center: bool = True) -> (bool, np.ndarray, np.ndarray): pass - def is_calibrated(self): - return self.mtx is not None and self.dist is not None - - def collect_points(self, image: np.ndarray) -> bool: + def collect_points(self, image: np.ndarray, calibration: Calibration) -> bool: success, obj_points, img_points = self.match_points(image) if success: - self.obj_points_list.append(obj_points) - self.img_points_list.append(img_points) + calibration.obj_points_list.append(obj_points) + calibration.img_points_list.append(img_points) return success - def calibration_clear(self): - self.mtx = None - self.dist = None - self.obj_points_list = [] - self.img_points_list = [] - - def calibration_set(self, mtx, dist): - self.mtx = mtx - self.dist = dist - - def calibration_get(self): - return self.mtx, self.dist - - def calibrate_camera(self, image: np.ndarray, r_vecs=None, t_vecs=None, flags=0): - success = False - if len(self.obj_points_list) > 0 and len(self.img_points_list) > 0: - ret, mtx, dist, r_vecs, t_vecs = cv.calibrateCamera(self.obj_points_list, self.img_points_list, image.shape, self.mtx, self.dist, - rvecs=r_vecs, tvecs=t_vecs, flags=flags) - - if ret < 3: - success = True - self.mtx = mtx - self.dist = dist - - return success diff --git a/ar_tag_pose/detector_board_aruco.py b/ar_tag_pose/detector_board_aruco.py index 68a4500..1e8a4fd 100644 --- a/ar_tag_pose/detector_board_aruco.py +++ b/ar_tag_pose/detector_board_aruco.py @@ -1,6 +1,8 @@ import cv2 as cv import numpy as np + from ar_tag_pose.detector_board import BoardDetector +from ar_tag_pose.calibration import Calibration from ar_tag_pose.aruco_types import ARUCO_DICT def factory(dict_type: int, board_size: cv.typing.Size, marker_length: float=100.0, marker_sep=10, ids: np.array=None): @@ -48,7 +50,7 @@ class BoardDetectorAruco(BoardDetector): return success, obj_points, img_points - def process(self, image: np.ndarray, draw_marker_box=True, draw_marker_id=False, draw_marker_axis=False, pose_center: bool = True): + def process(self, image: np.ndarray, calibration: Calibration, draw_marker_box=True, draw_marker_id=False, draw_marker_axis=False, pose_center: bool = True): pose = False r_vec = None t_vec = None @@ -66,15 +68,15 @@ class BoardDetectorAruco(BoardDetector): if draw_marker_id: cv.aruco.drawDetectedMarkers(image, corners, ids) - if not self.is_calibrated(): + if not calibration.is_calibrated(): return pose, None, None # Draw marker axis if draw_marker_axis: - _r_vecs, _t_vecs, _ = cv.aruco.estimatePoseSingleMarkers(corners, markerLength=0.1, cameraMatrix=self.mtx, distCoeffs=self.dist) + _r_vecs, _t_vecs, _ = cv.aruco.estimatePoseSingleMarkers(corners, markerLength=0.1, cameraMatrix=calibration.mtx, distCoeffs=calibration.dist) if _r_vecs is not None: for i in range(len(_r_vecs)): - cv.drawFrameAxes(image, self.mtx, self.dist, _r_vecs[i], _t_vecs[i], 0.05) + cv.drawFrameAxes(image, calibration.mtx, calibration.dist, _r_vecs[i], _t_vecs[i], 0.05) # Estimate pose of each marker and return the values r_vec and t_vec---(different from those of camera coefficients) obj_points, img_points = self.board.matchImagePoints(corners, ids) @@ -82,10 +84,10 @@ class BoardDetectorAruco(BoardDetector): offset_w = self.board.getGridSize()[0] * (self.board.getMarkerLength() + self.board.getMarkerSeparation()) offset_h = self.board.getGridSize()[1] * (self.board.getMarkerLength() + self.board.getMarkerSeparation()) obj_points -= [offset_w/2, offset_h/2, 0] - pose, r_vec, t_vec = cv.solvePnP(obj_points, img_points, self.mtx, self.dist, rvec=None, tvec=None) + pose, r_vec, t_vec = cv.solvePnP(obj_points, img_points, calibration.mtx, calibration.dist, rvec=None, tvec=None) if pose: # Draw Axis - cv.drawFrameAxes(image, self.mtx, self.dist, r_vec, t_vec, self.board.getMarkerLength()*1.5) + cv.drawFrameAxes(image, calibration.mtx, calibration.dist, r_vec, t_vec, self.board.getMarkerLength()*1.5) return pose, r_vec, t_vec diff --git a/ar_tag_pose/detector_board_charuco.py b/ar_tag_pose/detector_board_charuco.py index 0ac721a..059cb90 100644 --- a/ar_tag_pose/detector_board_charuco.py +++ b/ar_tag_pose/detector_board_charuco.py @@ -1,6 +1,8 @@ import cv2 as cv import numpy as np + from ar_tag_pose.detector_board import BoardDetector +from ar_tag_pose.calibration import Calibration from ar_tag_pose.aruco_types import ARUCO_DICT def factory(dict_type: int, board_size: cv.typing.Size, square_length: float = 5.0, marker_length: float = 3.0, ids: np.array=None): @@ -49,7 +51,7 @@ class BoardDetectorCharuco(BoardDetector): return success, obj_points, img_points - def process(self, image: np.ndarray, draw_marker_box=True, draw_marker_id=False, draw_marker_axis=False, pose_center: bool = True): + def process(self, image: np.ndarray, calibration: Calibration, draw_marker_box=True, draw_marker_id=False, draw_marker_axis=False, pose_center: bool = True): pose = False r_vec = None t_vec = None @@ -68,11 +70,11 @@ class BoardDetectorCharuco(BoardDetector): cv.aruco.drawDetectedCornersCharuco(image, charuco_corners, charuco_ids) # Draw marker axis - if draw_marker_axis and self.is_calibrated(): + if draw_marker_axis and calibration.is_calibrated(): _r_vecs, _t_vecs, _ = cv.aruco.estimatePoseSingleMarkers(marker_corners, markerLength=0.1, - cameraMatrix=self.mtx, distCoeffs=self.dist) + cameraMatrix=calibration.mtx, distCoeffs=calibration.dist) for i in range(len(_r_vecs)): - cv.drawFrameAxes(image, self.mtx, self.dist, _r_vecs[i], _t_vecs[i], 0.05) + cv.drawFrameAxes(image, calibration.mtx, calibration.dist, _r_vecs[i], _t_vecs[i], 0.05) if len(charuco_ids) >= len(self.board.getIds()): obj_points, img_points = self.board.matchImagePoints(charuco_corners, charuco_ids) @@ -80,12 +82,12 @@ class BoardDetectorCharuco(BoardDetector): offset_w = self.board.getChessboardSize()[0] * self.board.getSquareLength() offset_h = self.board.getChessboardSize()[1] * self.board.getSquareLength() obj_points -= [offset_w/2, offset_h/2, 0] - if self.is_calibrated(): - pose, r_vec, t_vec = cv.solvePnP(obj_points, img_points, self.mtx, self.dist, useExtrinsicGuess=False, + if calibration.is_calibrated(): + pose, r_vec, t_vec = cv.solvePnP(obj_points, img_points, calibration.mtx, calibration.dist, useExtrinsicGuess=False, rvec=None, tvec=None, flags=cv.SOLVEPNP_ITERATIVE) if pose: # Draw - cv.drawFrameAxes(image, self.mtx, self.dist, r_vec, t_vec, 10) + cv.drawFrameAxes(image, calibration.mtx, calibration.dist, r_vec, t_vec, 10) return pose, r_vec, t_vec diff --git a/ar_tag_pose/detector_board_main.py b/ar_tag_pose/detector_board_main.py index fc38043..4760eff 100644 --- a/ar_tag_pose/detector_board_main.py +++ b/ar_tag_pose/detector_board_main.py @@ -7,6 +7,7 @@ import mimetypes import json from pathlib import Path from ar_tag_pose.utils import to_tf, to_euler, draw_text +from calibration import Calibration from detector_board import BoardDetector from detector_board_aruco import BoardDetectorAruco from detector_board_charuco import BoardDetectorCharuco @@ -15,7 +16,8 @@ from cv2_enumerate_cameras import enumerate_cameras RESULTS_FOLDER = "results" class Detector(abc.ABC): - def __init__(self, params: dict, source: str, detector: list[BoardDetector], out_file: str|None = None, rotation_order: str=''): + def __init__(self, name: str, params: dict, source: str, detector: list[BoardDetector], out_file: str|None = None, rotation_order: str=''): + self.name = name self.params = params self.detector = detector self.out_file = out_file @@ -51,7 +53,10 @@ class Detector(abc.ABC): elif self.src_images is not None: # Open image file sequence self.cap = cv.VideoCapture(self.src_images, cv.CAP_IMAGES) + self.camera_name = self.name + self.calibration = Calibration() + self.calibration.load(f"{RESULTS_FOLDER}/{self.camera_name}_cal.npz") def params_save(self): with open(f"{RESULTS_FOLDER}/{self.params["name"]}.json", 'w') as fp: @@ -98,34 +103,6 @@ class Detector(abc.ABC): name = det.get_board_name() cv.imwrite(f"{RESULTS_FOLDER}/{name}.png", image) - def calibrate_save(self, det: BoardDetector): - filename = f"{RESULTS_FOLDER}/{det.get_board_name()}_cal.npz" - mtx, dist = det.calibration_get() - np.savez(f"{filename}", mtx=mtx, dist=dist) - calib = {'mtx': mtx.tolist(), 'dist': dist.tolist()} - self.params['calib'][self.camera_name][str(det)] = calib - self.params_save() - print(f"Saved {filename} successfully!") - - def calibrate_load(self): - calib = {} - for det in self.detector: - filename = f"{RESULTS_FOLDER}/{det.get_board_name()}_cal.npz" - try: - with np.load(filename) as X: - mtx, dist = [X[i] for i in ('mtx', 'dist')] - calib[str(det)] = {'mtx': mtx.tolist(), 'dist': dist.tolist()} - det.calibration_set(mtx, dist) - self.params['calib'] = calib - self.params_save() - print(f"Loaded {filename} successfully!") - except FileNotFoundError: - pass - except KeyError: - pass - - self.params['calib'] = {self.camera_name: calib} - def process(self): cap = self.cap if cap is None or not cap.isOpened(): @@ -162,25 +139,23 @@ class Detector(abc.ABC): elif k == ord('p'): gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY) for det in self.detector: - success = det.collect_points(gray) + success = det.collect_points(gray, self.calibration) if success: print(f"{det}: Points matching success") elif k == ord('c'): gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY) - for det in self.detector: - if det.calibrate_camera(gray): - print(f"{det}: Camera calibration success") - self.calibrate_save(det) + if self.calibration.process(gray): + print(f"{self.name}: Camera calibration success") + self.calibration.save(f"{RESULTS_FOLDER}/{self.camera_name}_cal.npz") elif k == ord('x'): - for det in self.detector: - det.calibration_clear() + self.calibration.calibration_clear() elif k == ord('f'): pose_center = not pose_center k = 800 / img.shape[1] y_pos = 30 for det in self.detector: - success, r_vec, t_vec = det.process(img, True, False, False, pose_center) + success, r_vec, t_vec = det.process(img, self.calibration, True, False, False, pose_center) if success: if len(self.rotation_order) == 3: rot = to_euler(to_tf(r_vec, t_vec), rotation_order=self.rotation_order) @@ -287,9 +262,8 @@ def main(): out_file = f"{var_args["name"]}.mp4" # Create Detector app - detector = Detector(params, var_args["source"], board_detector, out_file=out_file, rotation_order=var_args["rotation_order"]) + detector = Detector(var_args["name"], params, var_args["source"], board_detector, out_file=out_file, rotation_order=var_args["rotation_order"]) detector.board_image_generate(margin_length=var_args["margin_length"]) - detector.calibrate_load() detector.process()