jens/ocv_cam_pose
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

improved parameter handling load/save

Browse Source
This commit is contained in:
jens
2025-12-05 15:10:08 +01:00
parent 4cbf5c4623
commit 8f5a456325
1 changed files with 43 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ar_tag_pose/detector_board_main.py
+43 -42
View File
@@ -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()