From 7350c941cdbc5d253b67bf2399096d0ed496d1fd Mon Sep 17 00:00:00 2001 From: Jens Ahrensfeld Date: Tue, 25 Nov 2025 17:20:55 +0100 Subject: [PATCH] don't use rvecs and tvecs as input for solvePnP --- cam_pose_charuco_board.py | 28 ++++++++++++++++------------ 1 file changed, 16 insertions(+), 12 deletions(-) diff --git a/cam_pose_charuco_board.py b/cam_pose_charuco_board.py index f8c3b5c..3ba6d58 100644 --- a/cam_pose_charuco_board.py +++ b/cam_pose_charuco_board.py @@ -45,8 +45,12 @@ def process_video(detector: cv.aruco.CharucoDetector, board): dist = None rvecs = None tvecs = None - with np.load(CAM_CALIB_FILE) as X: - mtx, dist, rvecs, tvecs = [X[i] for i in ('mtx', 'dist', 'rvecs', 'tvecs')] + + try: + with np.load(CAM_CALIB_FILE) as X: + mtx, dist, _, _ = [X[i] for i in ('mtx', 'dist', 'rvecs', 'tvecs')] + except FileNotFoundError: + pass # Open camera video = cv.VideoCapture(0) @@ -66,7 +70,7 @@ def process_video(detector: cv.aruco.CharucoDetector, board): elif k == ord('s'): img_size = store_frame(detector, board, img) elif k == ord('c'): - mtx, dist, rvecs, tvecs = calibrate_camera(mtx, dist, rvecs, tvecs, img_size) + mtx, dist, _, _ = calibrate_camera(img_size, mtx, dist, None, None) else: process(detector, board, img, mtx, dist, rvecs, tvecs) @@ -78,7 +82,6 @@ def store_frame(detector: cv.aruco.CharucoDetector, board: cv.aruco.Board, img): 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) @@ -95,15 +98,17 @@ def store_frame(detector: cv.aruco.CharucoDetector, board: cv.aruco.Board, img): return img_size -def calibrate_camera(mtx, dist, rvecs, tvecs, img_size): +def calibrate_camera(img_size, mtx, dist, rvecs=None, tvecs=None): global ENTRY - ret, mtx, dist, rvecs, tvecs = cv.calibrateCamera(ENTRY['obj_points'], ENTRY['img_points'], img_size, mtx, dist, rvecs, tvecs) - np.savez(f"{CAM_CALIB_FILE}", mtx=mtx, dist=dist, rvecs=rvecs, tvecs=tvecs) - ENTRY = {'charuco_corners': [], 'charuco_ids': [], 'obj_points': [], 'img_points': []} + if len(ENTRY['obj_points']) > 0 and len(ENTRY['img_points']) > 0: + ret, mtx, dist, rvecs, tvecs = cv.calibrateCamera(ENTRY['obj_points'], ENTRY['img_points'], img_size, mtx, dist, rvecs=rvecs, tvecs=tvecs) + np.savez(f"{CAM_CALIB_FILE}", mtx=mtx, dist=dist, rvecs=rvecs, tvecs=tvecs) + ENTRY = {'charuco_corners': [], 'charuco_ids': [], 'obj_points': [], 'img_points': []} + print(f"Camera calibration finished. Result = {ret}") return mtx, dist, rvecs, tvecs -def process(detector: cv.aruco.CharucoDetector, board: cv.aruco.Board, img, mtx, dist, rvecs, tvecs): # Load previously saved data +def process(detector: cv.aruco.CharucoDetector, board: cv.aruco.Board, img, mtx, dist, rvecs=None, tvecs=None): # Load previously saved data gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY) charuco_corners, charuco_ids, marker_corners, marker_ids = detector.detectBoard(gray) if charuco_corners is not None: @@ -116,12 +121,11 @@ def process(detector: cv.aruco.CharucoDetector, board: cv.aruco.Board, img, mtx, if len(charuco_ids) >= len(board.getIds()): obj_points, img_points = board.matchImagePoints(charuco_corners, charuco_ids) - if mtx is not None and dist is not None: - pose, rvec, tvec = cv.solvePnP(obj_points, img_points, mtx, dist, rvecs, tvecs) + pose, rvec, tvec = cv.solvePnP(obj_points, img_points, mtx, dist, rvec=rvecs, tvec=tvecs) if pose: # Draw Axis - cv.drawFrameAxes(img, mtx, dist, rvec, tvec, 20) + cv.drawFrameAxes(img, mtx, dist, rvec, tvec, 10) cv.imshow('img', img)