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

refactored calibration

Browse Source
This commit is contained in:
jens
2025-11-25 19:16:12 +01:00
parent 0b88a47fa0
commit 8949c93800
1 changed files with 23 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cam_pose_charuco_board.py
+23 -21
View File
@@ -5,7 +5,6 @@ import argparse
from aruco_types import ARUCO_DICT from aruco_types import ARUCO_DICT
import math import math
ENTRY = {'charuco_corners': [], 'charuco_ids': [], 'obj_points': [], 'img_points': []}
CAM_CALIB_FILE = "results/cam_calib_charuco.npz" 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): 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) process_video(detector, board)
def process_video(detector: cv.aruco.CharucoDetector, board): def process_video(detector: cv.aruco.CharucoDetector, board):
global ENTRY
mtx = None mtx = None
dist = None dist = None
rvecs = None obj_points_list = []
tvecs = None img_points_list = []
try: try:
with np.load(CAM_CALIB_FILE) as X: with np.load(CAM_CALIB_FILE) as X:
mtx, dist, _, _ = [X[i] for i in ('mtx', 'dist', 'rvecs', 'tvecs')] mtx, dist, _, _ = [X[i] for i in ('mtx', 'dist', 'rvecs', 'tvecs')]
except FileNotFoundError: except FileNotFoundError:
pass pass
except KeyError:
pass
# Open camera # Open camera
video = cv.VideoCapture(0) video = cv.VideoCapture(0)
@@ -70,27 +69,29 @@ def process_video(detector: cv.aruco.CharucoDetector, board):
if k == ord('q'): if k == ord('q'):
break break
elif k == ord('s'): 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'): 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'): elif k == ord('x'):
ENTRY = {'charuco_corners': [], 'charuco_ids': [], 'obj_points': [], 'img_points': []} obj_points_list = []
img_points_list = []
else: 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): def store_frame(detector: cv.aruco.CharucoDetector, board: cv.aruco.Board, img):
global ENTRY img_size = None
img_size = 0 obj_points = None
img_points = None
gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY) gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
charuco_corners, charuco_ids, _, _ = detector.detectBoard(gray) charuco_corners, charuco_ids, _, _ = detector.detectBoard(gray)
if charuco_corners is not None: if charuco_corners is not None:
if len(charuco_ids) >= len(board.getIds()): if len(charuco_ids) >= len(board.getIds()):
obj_points, img_points = board.matchImagePoints(charuco_corners, charuco_ids) obj_points, img_points = board.matchImagePoints(charuco_corners, charuco_ids)
if len(obj_points) > 0 and len(img_points) > 0: 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 img_size = gray.shape
print("Frame captured") print("Frame captured")
else: else:
@@ -100,13 +101,14 @@ def store_frame(detector: cv.aruco.CharucoDetector, board: cv.aruco.Board, img):
else: else:
print("Point matching failed, try again") 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): def calibrate_camera(img_size, obj_points_list, img_points_list, mtx, dist, rvecs=None, tvecs=None):
if len(ENTRY['obj_points']) > 0 and len(ENTRY['img_points']) > 0: if len(obj_points_list) > 0 and len(img_points_list) > 0:
flags = cv.CALIB_USE_INTRINSIC_GUESS flags = 0
ret, mtx, dist, rvecs, tvecs = cv.calibrateCamera(ENTRY['obj_points'], ENTRY['img_points'], img_size, mtx, dist, rvecs=rvecs, tvecs=tvecs, flags=flags) if mtx is not None:
np.savez(f"{CAM_CALIB_FILE}", mtx=mtx, dist=dist, rvecs=rvecs, tvecs=tvecs) 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}") print(f"Camera calibration finished. Result = {ret}")
return mtx, dist, rvecs, tvecs return mtx, dist, rvecs, tvecs