From 8949c93800a42c380c4cc0082d339d48b3ec3624 Mon Sep 17 00:00:00 2001
From: Jens Ahrensfeld <jens@jayfield.org>
Date: Tue, 25 Nov 2025 19:16:12 +0100
Subject: [PATCH] refactored calibration

---
 cam_pose_charuco_board.py | 44 ++++++++++++++++++++-------------------
 1 file changed, 23 insertions(+), 21 deletions(-)

diff --git a/cam_pose_charuco_board.py b/cam_pose_charuco_board.py
index 90ed4c9..9e4c2e2 100644
--- a/cam_pose_charuco_board.py
+++ b/cam_pose_charuco_board.py
@@ -5,7 +5,6 @@ import argparse
 from aruco_types import ARUCO_DICT
 import math
 
-ENTRY = {'charuco_corners': [], 'charuco_ids': [], 'obj_points': [], 'img_points': []}
 CAM_CALIB_FILE = "results/cam_calib_charuco.npz"
 
 def factory(dict_type: int, board_size: cv.typing.Size, square_length: float=5.0, marker_length: float=3.0):
@@ -42,17 +41,17 @@ def main():
 	process_video(detector, board)
 
 def process_video(detector: cv.aruco.CharucoDetector, board):
-	global ENTRY
 	mtx = None
 	dist = None
-	rvecs = None
-	tvecs = None
-
+	obj_points_list = []
+	img_points_list = []
 	try:
 		with np.load(CAM_CALIB_FILE) as X:
 			mtx, dist, _, _ = [X[i] for i in ('mtx', 'dist', 'rvecs', 'tvecs')]
 	except FileNotFoundError:
 		pass
+	except KeyError:
+		pass
 
 	# Open camera
 	video = cv.VideoCapture(0)
@@ -70,27 +69,29 @@ def process_video(detector: cv.aruco.CharucoDetector, board):
 		if k == ord('q'):
 			break
 		elif k == ord('s'):
-			img_size = store_frame(detector, board, img)
+			img_size, obj_points, img_points = store_frame(detector, board, img)
+			if img_size is not None:
+				obj_points_list.append(obj_points)
+				img_points_list.append(img_points)
 		elif k == ord('c'):
-			mtx, dist, _, _ = calibrate_camera(img_size, mtx, dist, None, None)
+			mtx, dist, rvecs, tvecs = calibrate_camera(img_size, obj_points_list, img_points_list, mtx, dist, None, None)
+			np.savez(f"{CAM_CALIB_FILE}", mtx=mtx, dist=dist, rvecs=rvecs, tvecs=tvecs)
 		elif k == ord('x'):
-			ENTRY = {'charuco_corners': [], 'charuco_ids': [], 'obj_points': [], 'img_points': []}
+			obj_points_list = []
+			img_points_list = []
 		else:
-			process(detector, board, img, mtx, dist, rvecs, tvecs)
+			process(detector, board, img, mtx, dist, None, None)
 
 def store_frame(detector: cv.aruco.CharucoDetector, board: cv.aruco.Board, img):
-	global ENTRY
-	img_size = 0
+	img_size = None
+	obj_points = None
+	img_points = None
 	gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
 	charuco_corners, charuco_ids, _, _ = detector.detectBoard(gray)
 	if charuco_corners is not None:
 		if len(charuco_ids) >= len(board.getIds()):
 			obj_points, img_points = board.matchImagePoints(charuco_corners, charuco_ids)
 			if len(obj_points) > 0 and len(img_points) > 0:
-				ENTRY['charuco_corners'].append(charuco_corners)
-				ENTRY['charuco_ids'].append(charuco_ids)
-				ENTRY['obj_points'].append(obj_points)
-				ENTRY['img_points'].append(img_points)
 				img_size = gray.shape
 				print("Frame captured")
 			else:
@@ -100,13 +101,14 @@ def store_frame(detector: cv.aruco.CharucoDetector, board: cv.aruco.Board, img):
 	else:
 		print("Point matching failed, try again")
 
-	return img_size
+	return img_size, obj_points, img_points
 
-def calibrate_camera(img_size, mtx, dist, rvecs=None, tvecs=None):
-	if len(ENTRY['obj_points']) > 0 and len(ENTRY['img_points']) > 0:
-		flags = cv.CALIB_USE_INTRINSIC_GUESS
-		ret, mtx, dist, rvecs, tvecs = cv.calibrateCamera(ENTRY['obj_points'], ENTRY['img_points'], img_size, mtx, dist, rvecs=rvecs, tvecs=tvecs, flags=flags)
-		np.savez(f"{CAM_CALIB_FILE}", mtx=mtx, dist=dist, rvecs=rvecs, tvecs=tvecs)
+def calibrate_camera(img_size, obj_points_list, img_points_list, mtx, dist, rvecs=None, tvecs=None):
+	if len(obj_points_list) > 0 and len(img_points_list) > 0:
+		flags = 0
+		if mtx is not None:
+			flags = cv.CALIB_USE_INTRINSIC_GUESS
+		ret, mtx, dist, rvecs, tvecs = cv.calibrateCamera(obj_points_list, img_points_list, img_size, mtx, dist, rvecs=rvecs, tvecs=tvecs, flags=flags)
 		print(f"Camera calibration finished. Result = {ret}")
 	return mtx, dist, rvecs, tvecs