From 8f5a45632514643f0975236954b73e547b2f5a27 Mon Sep 17 00:00:00 2001 From: Jens Ahrensfeld Date: Fri, 5 Dec 2025 15:10:08 +0100 Subject: [PATCH] improved parameter handling load/save --- ar_tag_pose/detector_board_main.py | 85 +++++++++++++++--------------- 1 file changed, 43 insertions(+), 42 deletions(-) diff --git a/ar_tag_pose/detector_board_main.py b/ar_tag_pose/detector_board_main.py index f5c9f40..3c11e18 100644 --- a/ar_tag_pose/detector_board_main.py +++ b/ar_tag_pose/detector_board_main.py @@ -1,6 +1,5 @@ import os import abc -import math import cv2 as cv import numpy as np import argparse @@ -14,18 +13,9 @@ from detector_board_charuco import BoardDetectorCharuco RESULTS_FOLDER = "results" -def cap_init(cap: cv.VideoCapture): - cap.set(cv.CAP_PROP_SETTINGS, 1) - cap.set(cv.CAP_PROP_FOURCC, cv.VideoWriter.fourcc('M', 'J', 'P', 'G')) - cap.set(cv.CAP_PROP_FPS, 60.0) - cap.set(cv.CAP_PROP_FRAME_WIDTH, 1280) - cap.set(cv.CAP_PROP_FRAME_HEIGHT, 1024) - cap.set(cv.CAP_PROP_EXPOSURE, 30) - cap.set(cv.CAP_PROP_AUTO_EXPOSURE, 1) - - class Detector(abc.ABC): - def __init__(self, source: str, detector: list[BoardDetector], out_file: str|None = None, rotation_order: str=''): + def __init__(self, params: dict, source: str, detector: list[BoardDetector], out_file: str|None = None, rotation_order: str=''): + self.params = params self.detector = detector self.out_file = out_file self.rotation_order = rotation_order @@ -47,16 +37,33 @@ class Detector(abc.ABC): self.src_images = f"{os.path.dirname(source)}/{name}_%0{len(numeric_part)}d.{suffix}" Path.mkdir(Path(RESULTS_FOLDER), exist_ok=True) + self.camera_name = "YourCameraNameHere" - @staticmethod - def cap_init(cap: cv.VideoCapture): - cap.set(cv.CAP_PROP_SETTINGS, 1) - cap.set(cv.CAP_PROP_FOURCC, cv.VideoWriter.fourcc('M', 'J', 'P', 'G')) - cap.set(cv.CAP_PROP_FPS, 60.0) - cap.set(cv.CAP_PROP_FRAME_WIDTH, 1280) - cap.set(cv.CAP_PROP_FRAME_HEIGHT, 1024) - cap.set(cv.CAP_PROP_EXPOSURE, 30) - cap.set(cv.CAP_PROP_AUTO_EXPOSURE, 1) + def params_save(self): + with open(f"{RESULTS_FOLDER}/{self.params["name"]}.json", 'w') as fp: + json.dump(self.params, fp, indent=4) + + def cap_init(self, cap: cv.VideoCapture): + camera_params_default = { + cv.CAP_PROP_FRAME_WIDTH: 1920 * 2, + cv.CAP_PROP_FRAME_HEIGHT: 1080 * 2, + cv.CAP_PROP_FPS: 60, + cv.CAP_PROP_GAIN: 128, + cv.CAP_PROP_EXPOSURE: 200, + cv.CAP_PROP_AUTO_EXPOSURE: 1 + } + camera = {} + if not "camera" in self.params: + camera = camera_params_default + + for item in camera: + key = item + value = camera[key] + print(f"{item}: {value}") + cap.set(key, value) + + self.params['camera'] = {self.camera_name: camera} + self.params_save() @staticmethod def print_angle(img: np.ndarray, title: str, angle: np.array, org: tuple[int, int], rot_order: str = 'XYZ'): @@ -72,20 +79,25 @@ class Detector(abc.ABC): name = det.get_board_name() cv.imwrite(f"{RESULTS_FOLDER}/{name}.png", image) - @staticmethod - def calibrate_save(det: BoardDetector): + def calibrate_save(self, det: BoardDetector): filename = f"{RESULTS_FOLDER}/{det.get_board_name()}_cal.npz" mtx, dist, r_vecs, t_vecs = det.calibration_get() np.savez(f"{filename}", mtx=mtx, dist=dist, rvecs=r_vecs, tvecs=t_vecs) + calib = {self.camera_name: {str(det): {'mtx': mtx.tolist(), 'dist': dist.tolist()}}} + self.params['calib'] = calib + self.params_save() print(f"Saved {filename} successfully!") def calibrate_load(self): + self.params['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', 'rvecs', 'tvecs')] + calib = {self.camera_name: {str(det): {'mtx': mtx.tolist(), 'dist': dist.tolist()}}} det.calibration_set(mtx, dist, None, None) + self.params['calib'] = calib print(f"Loaded {filename} successfully!") except FileNotFoundError: pass @@ -97,7 +109,7 @@ class Detector(abc.ABC): if self.device_id is not None: # Open camera cap = cv.VideoCapture(self.device_id) - cap_init(cap) + self.cap_init(cap) elif self.src_video is not None: # Open video file cap = cv.VideoCapture(self.src_video) @@ -112,8 +124,6 @@ class Detector(abc.ABC): frame_go = True frame_num = 0 out = None - r_vec = None - t_vec = None while cap.isOpened(): if self.src_images: cap.set(cv.CAP_PROP_POS_FRAMES, frame_num) @@ -152,29 +162,23 @@ class Detector(abc.ABC): elif k == ord('x'): for det in self.detector: det.calibration_clear() - elif k == ord('e'): - if t_vec is not None and r_vec is not None: - rvec_world, tvec_world = r_vec, t_vec - elif k == ord('r'): - tvec_world = None - rvec_world = None + img_scaled = img y_pos = 30 for det in self.detector: success, r_vec, t_vec = det.process(img) + img_scaled = cv.resize(img, dsize=None, fx=0.25, fy=0.25) if success: if len(self.rotation_order) == 3: rot = to_euler(to_tf(r_vec, t_vec), rotation_order=self.rotation_order) - self.print_angle(img, f"{str(det)}-{self.rotation_order}", rot, (30, y_pos), self.rotation_order) + self.print_angle(img_scaled, f"{str(det)}-{self.rotation_order}", rot, (30, y_pos), self.rotation_order) y_pos += 30 else: for rotation_order in ['XYZ', 'XZY', 'YXZ', 'YZX', 'ZXY', 'ZYX']: rot = to_euler(to_tf(r_vec, t_vec), rotation_order=rotation_order) - self.print_angle(img, f"{str(det)}-{rotation_order}", rot, (30, y_pos), rotation_order) + self.print_angle(img_scaled, f"{str(det)}-{rotation_order}", rot, (30, y_pos), rotation_order) y_pos += 30 - - img_scaled = cv.resize(img, dsize=None, fx=0.5, fy=0.5) cv.imshow('img', img_scaled) if self.out_file is not None: out = out_writer(out, img_scaled, filename=self.out_file) @@ -243,13 +247,10 @@ def main(): var_args = vars(args) # save project file - prj = {} + params = {} for arg in var_args: value = getattr(args, arg) - prj[arg] = value - - with open(f"{RESULTS_FOLDER}/{var_args["name"]}.json", 'w') as fp: - json.dump(prj, fp, indent=4) + params[arg] = value # Create BoardDetector msg_size = 3 @@ -272,7 +273,7 @@ def main(): out_file = f"{var_args["name"]}.mp4" # Create Detector app - detector = Detector(var_args["source"], board_detector, out_file=out_file, rotation_order=var_args["rotation_order"]) + detector = Detector(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()