improved parameter handling load/save

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 params_save(self):
-	def cap_init(cap: cv.VideoCapture):
+		with open(f"{RESULTS_FOLDER}/{self.params["name"]}.json", 'w') as fp:
-		cap.set(cv.CAP_PROP_SETTINGS, 1)
+			json.dump(self.params, fp, indent=4)
-		cap.set(cv.CAP_PROP_FOURCC, cv.VideoWriter.fourcc('M', 'J', 'P', 'G'))
+
-		cap.set(cv.CAP_PROP_FPS, 60.0)
+	def cap_init(self, cap: cv.VideoCapture):
-		cap.set(cv.CAP_PROP_FRAME_WIDTH, 1280)
+		camera_params_default = {
-		cap.set(cv.CAP_PROP_FRAME_HEIGHT, 1024)
+			cv.CAP_PROP_FRAME_WIDTH: 1920 * 2,
-		cap.set(cv.CAP_PROP_EXPOSURE, 30)
+			cv.CAP_PROP_FRAME_HEIGHT: 1080 * 2,
-		cap.set(cv.CAP_PROP_AUTO_EXPOSURE, 1)
+			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(self, det: BoardDetector):
-	def calibrate_save(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
+		params[arg] = value
-
-	with open(f"{RESULTS_FOLDER}/{var_args["name"]}.json", 'w') as fp:
-		json.dump(prj, fp, indent=4)
 
 	# 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()