def calibrate_camera_aruco(vid, board):
board_type = get_board_type(board)
video_params = get_video_params(vid)
someCorners, someIds = get_corners_aruco(vid, board)
allCorners = []
allIds = []
allCorners.extend(someCorners)
allIds.extend(someIds)
allCorners, allIds = trim_corners(allCorners, allIds, maxBoards=100)
allCornersConcat, allIdsConcat, markerCounter = reformat_corners(
allCorners, allIds)
expected_markers = get_expected_corners(board)
print("\nfound {} markers, {} boards, {} complete boards".format(
len(allCornersConcat), len(markerCounter),
np.sum(markerCounter == expected_markers)))
if board_type == 'charuco':
calib_params = calibrate_charuco(allCorners, allIds, board,
video_params)
else:
calib_params = calibrate_aruco(allCornersConcat, allIdsConcat,
markerCounter, board, video_params)
return calib_params
def calibrate_camera(vid, board):
board_type = get_board_type(board)
if board_type == 'checkerboard':
return calibrate_camera_checkerboard(vid, board)
else:
return calibrate_camera_aruco(vid, board)
def estimate_pose_aruco(gray, intrinsics, board):
detectedCorners, detectedIds = detect_aruco(gray, intrinsics, board)
if len(detectedIds) < 3:
return False, None
INTRINSICS_K = np.array(intrinsics['camera_mat'])
INTRINSICS_D = np.array(intrinsics['dist_coeff'])
board_type = get_board_type(board)
if board_type == 'charuco':
ret, rvec, tvec = cv2.aruco.estimatePoseCharucoBoard(
detectedCorners,
detectedIds,
board,
INTRINSICS_K,
INTRINSICS_D,
rvec=np.array([]),
tvec=np.array([]),
useExtrinsicGuess=False)
else:
ret, rvec, tvec = cv2.aruco.estimatePoseBoard(detectedCorners,
detectedIds,
board,
INTRINSICS_K,
INTRINSICS_D,
rvec=np.array([]),
tvec=np.array([]),
useExtrinsicGuess=False)
if not ret or rvec is None or tvec is None:
return False, None
return True, (detectedCorners, detectedIds, rvec, tvec)
def get_corners_aruco(vid, board, skip=20):
cap = cv2.VideoCapture(vid)
length = int(cap.get(cv2.CAP_PROP_FRAME_COUNT))
allCorners = []
allIds = []
go = int(skip / 2)
board_type = get_board_type(board)
max_size = get_expected_corners(board)
for framenum in trange(length, ncols=70):
ret, frame = cap.read()
if not ret:
break
if framenum % skip != 0 and go <= 0:
continue
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
params = cv2.aruco.DetectorParameters_create()
params.cornerRefinementMethod = cv2.aruco.CORNER_REFINE_CONTOUR
params.adaptiveThreshWinSizeMin = 100
params.adaptiveThreshWinSizeMax = 700
params.adaptiveThreshWinSizeStep = 50
params.adaptiveThreshConstant = 5
corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(
gray, board.dictionary, parameters=params)
if corners is None or len(corners) <= 2:
go = max(0, go - 1)
continue
detectedCorners, detectedIds, rejectedCorners, recoveredIdxs = \
cv2.aruco.refineDetectedMarkers(gray, board, corners, ids,
rejectedImgPoints, parameters=params)
if board_type == 'charuco' and len(detectedCorners) > 0:
ret, detectedCorners, detectedIds = cv2.aruco.interpolateCornersCharuco(
detectedCorners, detectedIds, gray, board)
if detectedCorners is not None and \
len(detectedCorners) >= 2 and len(detectedCorners) <= max_size:
allCorners.append(detectedCorners)
allIds.append(detectedIds)
go = int(skip / 2)
go = max(0, go - 1)
cap.release()
return allCorners, allIds
def fill_points(corners, ids, board):
board_type = get_board_type(board)
if board_type == 'checkerboard':
if corners is not None:
return corners.reshape(-1, 2)
else:
return np.copy(board.objPoints)[:, :2] * np.nan
elif board_type == 'aruco':
num_corners = get_expected_corners(board)
# N boxes with 4 corners each
out = np.zeros((num_corners * 4, 2))
out.fill(np.nan)
if corners is None or ids is None:
return out
for id_wrap, corner_wrap in zip(ids, corners):
ix = id_wrap[0]
corner = corner_wrap.flatten().reshape(4, 2)
if ix >= num_corners:
continue
out[ix * 4:(ix + 1) * 4, :] = corner
return out
elif board_type == 'charuco':
num_corners = get_expected_corners(board)
out = np.zeros((num_corners, 2))
out.fill(np.nan)
if ids is None or corners is None:
return out
corners = corners.reshape(-1, 2)
ids = ids.flatten()
for ix, corner in zip(ids, corners):
out[ix] = corner
return out
def detect_aruco(gray, intrinsics, board):
board_type = get_board_type(board)
gray = cv2.GaussianBlur(gray, (5, 5), 0)
params = cv2.aruco.DetectorParameters_create()
params.cornerRefinementMethod = cv2.aruco.CORNER_REFINE_CONTOUR
params.adaptiveThreshWinSizeMin = 100
params.adaptiveThreshWinSizeMax = 600
params.adaptiveThreshWinSizeStep = 50
params.adaptiveThreshConstant = 5
corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(
gray, board.dictionary, parameters=params)
if intrinsics is None:
INTRINSICS_K = INTRINSICS_D = None
else:
INTRINSICS_K = np.array(intrinsics['camera_mat'])
INTRINSICS_D = np.array(intrinsics['dist_coeff'])
if ids is None:
return [], []
elif len(ids) < 2:
return corners, ids
detectedCorners, detectedIds, rejectedCorners, recoveredIdxs = \
cv2.aruco.refineDetectedMarkers(gray, board, corners, ids,
rejectedImgPoints,
INTRINSICS_K, INTRINSICS_D,
parameters=params)
if board_type == 'charuco' and len(detectedCorners) > 0:
ret, detectedCorners, detectedIds = cv2.aruco.interpolateCornersCharuco(
detectedCorners, detectedIds, gray, board)
if detectedIds is None:
detectedCorners = detectedIds = []
return detectedCorners, detectedIds
def estimate_pose(gray, intrinsics, board):
board_type = get_board_type(board)
if board_type == 'checkerboard':
return estimate_pose_checkerboard(gray, intrinsics, board)
else:
return estimate_pose_aruco(gray, intrinsics, board)
