From f114130e6009038e6d021ba1a7e062b0c5128f1a Mon Sep 17 00:00:00 2001
From: Jens Ahrensfeld <jens@jayfield.org>
Date: Fri, 12 Dec 2025 10:50:20 +0100
Subject: [PATCH] - revised calibration - cleaned up unused parameter - use
 CALIB_RATIONAL_MODEL for better wide angle matching

---
 ar_tag_pose/calibration.py         | 18 +++++++++---------
 ar_tag_pose/detector_board_main.py |  7 +++++--
 2 files changed, 14 insertions(+), 11 deletions(-)

diff --git a/ar_tag_pose/calibration.py b/ar_tag_pose/calibration.py
index dd5b354..38e3053 100644
--- a/ar_tag_pose/calibration.py
+++ b/ar_tag_pose/calibration.py
@@ -37,22 +37,22 @@ class Calibration:
 			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!")
+		if self.mtx is not None and self.dist is not None:
+			np.savez(f"{filename}", mtx=self.mtx, dist=self.dist)
+			print(f"Saved {filename} successfully!")
 
-	def process(self, image: np.ndarray, thresh=3.0, r_vecs=None, t_vecs=None, flags=0):
-		success = False
+	def process(self, image: np.ndarray, thresh=3.0):
+		criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 3000, 0.001)
+		error = None
 		no = len(self.obj_points_list)
 		ni = len(self.img_points_list)
 		if no == ni and no > 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)
+															  rvecs=None, tvecs=None, flags=cv.CALIB_RATIONAL_MODEL, criteria=criteria)
 
 			if ret < thresh:
-				success = True
+				error = ret
 				self.mtx = mtx
 				self.dist = dist
 
-		return success
+		return error
diff --git a/ar_tag_pose/detector_board_main.py b/ar_tag_pose/detector_board_main.py
index 21f387c..ca57596 100644
--- a/ar_tag_pose/detector_board_main.py
+++ b/ar_tag_pose/detector_board_main.py
@@ -186,9 +186,12 @@ class Detector(abc.ABC):
 						print(f"{det}: Points matching success")
 			elif k == ord('c'):
 				gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
-				if self.calibration.process(gray, thresh=3*len(self.detector)):
-					print(f"{self.camera_name}: Camera calibration success")
+				calib_err = self.calibration.process(gray, thresh=3*len(self.detector))
+				if calib_err is not None:
+					print(f"{self.camera_name}: Calibration success: Reprojection error={calib_err}")
 					self.calibration.save(f"{RESULTS_FOLDER}/{self.camera_name}_cal.npz")
+				else:
+					print(f"{self.camera_name}: Calibration failed")
 			elif k == ord('x'):
 				self.calibration.calibration_clear()
 			elif k == ord('f'):