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)