Fisheye Stereo ChAruco calibration

room2
Tue, 18 Mar 2025 21:24:12 +0000
Python

Hello,

I am attempting to calibrate a wide angle lens stereo camera (125º) with the method in the title, but I am not able to make the code work. My initial attempt was to use a normal chessboard to obtain the calibration images with the fisheye model, although this works, I am unable to achieve a low enough error (around 0.1). After making some research I adapted my code to do the following:

1. Remove images very close to the edge (the reasoning behind this is exposed in my previous post https://forum.opencv.org/t/problems-with-fisheye-stereo-calibration/20201)

2. Remove images with potentially fipped points or corners

3. Monocularly calibrate each camera as initial guesses for the stereoCalibration

After attempting to take more and better images without success, I decided to implement a charuco calibration since I suspected that it was the (1) step that was avoiding my model to get enough information around the borders. But I get the following error in the stereo calibration function:

retL, K_L, D_L, rvecsL, tvecsL = cv2.fisheye.calibrate(
                                     ^^^^^^^^^^^^^^^^^^^^^^
cv2.error: OpenCV(4.11.0) D:\bld\libopencv_1739279452246\work\modules\calib3d\src\fisheye.cpp:710: error: (-215:Assertion failed) !objectPoints.empty() && !imagePoints.empty() && objectPoints.total() == imagePoints.total() in function 'cv::fisheye::calibrate'

Below is my complete code (including charuco and simple chessboard).

Thanks in advanced.

import cv2
import numpy as np
import os, sys, glob

REPROJ_ERROR_THRESHOLD = 2 # px
BLACK_PIXEL_FRACTION_THRESHOLD = 0.8
GRAY_PIXEL_FRACTION_THRESHOLD = 0.8

# https://github.com/jamiemilsom/Fisheye_ChArUco_Calibration
# https://github.com/pettod/charuco-corner-detection

def image_selector(calibration_images, frame_size, chessboard_size, square_size, debug_dirs):
    imagesL, imagesR = calibration_images
    debug_corners_dir, debug_rect_dir, debug_rejected = debug_dirs

    objpoints = []
    imgpoints_L = []
    imgpoints_R = []

    valid_imgs_L = []
    valid_imgs_R = []

    objp = np.zeros((chessboard_size[0] * chessboard_size[1], 1, 3), np.float32)
    objp[:, 0, :2] = np.mgrid[0:chessboard_size[0], 0:chessboard_size[1]].T.reshape(-1, 2) * square_size
    loadedX, loadedY = cv2.imread(imagesL[0], cv2.IMREAD_GRAYSCALE).shape[::-1]

    for i, (imgL_path, imgR_path) in enumerate(zip(imagesL, imagesR)):
        imgL = cv2.imread(imgL_path)
        imgR = cv2.imread(imgR_path)

        grayL = cv2.cvtColor(imgL, cv2.COLOR_BGR2GRAY)
        grayR = cv2.cvtColor(imgR, cv2.COLOR_BGR2GRAY)

        retL, cornersL = cv2.findChessboardCorners(grayL, chessboard_size, None)
        retR, cornersR = cv2.findChessboardCorners(grayR, chessboard_size, None)

        if not retL or not retR:
            print(f"No corners found for {imgL_path}. Skipping.")
            continue  # Skip this pair and continue with the next one

        imgL_debug = imgL.copy()
        imgR_debug = imgR.copy()

        cv2.drawChessboardCorners(imgL_debug, chessboard_size, cornersL, retL)
        cv2.drawChessboardCorners(imgR_debug, chessboard_size, cornersR, retR)
        debug_corner_img = np.hstack((imgL_debug, imgR_debug))

        cornersL = np.float32(cornersL)
        cornersR = np.float32(cornersR)

        minRx, maxRx = np.min(cornersR[:, :, 0]), np.max(cornersR[:, :, 0])
        minRy, maxRy = np.min(cornersR[:, :, 1]), np.max(cornersR[:, :, 1])
        minLx, maxLx = np.min(cornersL[:, :, 0]), np.max(cornersL[:, :, 0])
        minLy, maxLy = np.min(cornersL[:, :, 1]), np.max(cornersL[:, :, 1])

        border_threshold_x = loadedX / 10
        border_threshold_y = loadedY / 10

        # if minRx < border_threshold_x or minLx < border_threshold_x or \
        #    minRy < border_threshold_y or minLy < border_threshold_y:
        #     print(f"Chessboard too close to the border in {imgL_path}. Skipping.")
        #     cv2.imwrite(f"{debug_rejected}/cutted_rejected_{i}.png", debug_corner_img)
        #     continue  # Skip this pair

        cv2.imwrite(f"{debug_corners_dir}/detected_{i}.png", debug_corner_img)

        criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.1)
        winsize = (11, 11)
        cornersL = cv2.cornerSubPix(grayL, cornersL, winsize, (-1, -1), criteria)
        cornersR = cv2.cornerSubPix(grayR, cornersR, winsize, (-1, -1), criteria)

        diff = cornersL - cornersR
        lengths = np.linalg.norm(diff, axis=2)
        total_diff = np.sum(lengths)

        if total_diff > 5000.0:
            print(f"Broken stereo pair detected for {imgL_path}. Diff: {total_diff}. Skipping.")
            cv2.imwrite(f"{debug_rejected}/diff_rejected_{i}.png", debug_corner_img)
            continue  # Skip this pair

        # Save valid data
        objpoints.append(objp)
        imgpoints_L.append(cornersL)
        imgpoints_R.append(cornersR)
        valid_imgs_L.append(imgL_path)
        valid_imgs_R.append(imgR_path)

    return valid_imgs_L, valid_imgs_R, objpoints, imgpoints_L, imgpoints_R

def charucho_image_selector(calibration_images, aruco_board, aruco_dict, aruco_params, debug_dir):
    imagesL, imagesR = calibration_images

    valid_imgs_L = []
    valid_imgs_R = []

    objpoints = []
    imgpoints_L = []
    imgpoints_R = []

    for i, (imgL_path, imgR_path) in enumerate(zip(imagesL, imagesR)):
        imgL = cv2.imread(imgL_path)
        imgR = cv2.imread(imgR_path)

        grayL = cv2.cvtColor(imgL, cv2.COLOR_BGR2GRAY)
        grayR = cv2.cvtColor(imgR, cv2.COLOR_BGR2GRAY)

        corners_L, ids_L, rc_L = cv2.aruco.detectMarkers(grayL, aruco_dict, parameters=aruco_params)
        corners_R, ids_R, rc_R = cv2.aruco.detectMarkers(grayR, aruco_dict, parameters=aruco_params)

        if ids_L is None or ids_R is None:
            print(f"No markers found for {imgL_path}. Skipping.")
            continue  # Skip this pair and continue with the next one

        # Refine the markers
        corners_L_ref, ids_L_ref, rc_L_ref, rec_idx_L = cv2.aruco.refineDetectedMarkers(
            grayL, aruco_board, corners_L, ids_L, rc_L, parameters=aruco_params
        )
        corners_R_ref, ids_R_ref, rc_R_ref, rec_idx_R = cv2.aruco.refineDetectedMarkers(
            grayR, aruco_board, corners_R, ids_R, rc_R, parameters=aruco_params
        )

        # Use markers to find corners
        if ids_L_ref is None or ids_R_ref is None and \
            len(ids_L_ref) <= 0 and len(ids_R_ref) <= 0:
            print(f"No markers found for {imgL_path}. Skipping.")
            continue

        charuco_retL, charuco_corners_L, charuco_ids_L = cv2.aruco.interpolateCornersCharuco(
            corners_L_ref, ids_L_ref, grayL, aruco_board
        )
        charuco_retR, charuco_corners_R, charuco_ids_R = cv2.aruco.interpolateCornersCharuco(
            corners_R_ref, ids_R_ref, grayR, aruco_board
        )

        if len(charuco_corners_L) <= 4 or len(charuco_corners_R) <= 4:
            print(f"Less than 4 charuco corners found for {imgL_path}. Skipping.")
            continue

        # Save valid data
        valid_imgs_L.append(imgL_path)
        valid_imgs_R.append(imgR_path)

        # objpoints.append(aruco_board.getChessboardCorners()[charuco_ids_R])
        # imgpoints_L.append(charuco_corners_L)
        # imgpoints_R.append(charuco_corners_R)

        # Since with charuco we dont need detected points to be the same
        # we have to take the common ones manually
        common_ids = np.intersect1d(charuco_ids_L.flatten(), charuco_ids_R.flatten())
        common_corners_L = []
        common_corners_R = []
        common_objpoints = []

        board_obj_corners = aruco_board.getChessboardCorners()  # shape (N, 3)
        # For each common ID, extract its image coordinates (from left and right)
        # and the corresponding 3D object point from the board.
        for cid in common_ids:
            idxL = np.where(charuco_ids_L.flatten() == cid)[0][0]
            idxR = np.where(charuco_ids_R.flatten() == cid)[0][0]
            common_corners_L.append(charuco_corners_L[idxL][0])  # charuco_corners shape: (n,1,2)
            common_corners_R.append(charuco_corners_R[idxR][0])
            # Use the board's ordering. The board's getChessboardCorners() returns the
            # 3D coordinates in the board coordinate system. We use the detected ID as index.
            common_objpoints.append(board_obj_corners[cid])

        common_corners_L = np.array(common_corners_L, dtype=np.float32)
        common_corners_R = np.array(common_corners_R, dtype=np.float32)
        common_objpoints = np.array(common_objpoints, dtype=np.float32)
        # Expand dimensions if your calibration function expects each view's points in shape (n, 1, ?)

        # For example, fisheye calibration in OpenCV expects object points as (n, 1, 3)
        common_corners_L = np.expand_dims(common_corners_L, axis=1)  # shape: (n, 1, 2)
        common_corners_R = np.expand_dims(common_corners_R, axis=1)  # shape: (n, 1, 2)
        common_objpoints   = np.expand_dims(common_objpoints, axis=1)    # shape: (n, 1, 3)

        objpoints.append(common_objpoints)
        imgpoints_L.append(common_corners_L)
        imgpoints_R.append(common_corners_R)

        # Draw markers and charuco corners
        imgL_debug = imgL.copy()
        imgR_debug = imgR.copy()

        cv2.aruco.drawDetectedMarkers(imgL_debug, corners_L_ref, ids_L_ref)
        cv2.aruco.drawDetectedMarkers(imgR_debug, corners_R_ref, ids_R_ref)

        cv2.aruco.drawDetectedCornersCharuco(imgL_debug, charuco_corners_L, charuco_ids_L)
        cv2.aruco.drawDetectedCornersCharuco(imgR_debug, charuco_corners_R, charuco_ids_R)
        img_debug = np.hstack((imgL_debug, imgR_debug))

        cv2.imwrite(f"{debug_dir}/markers_{i}.png", img_debug)

    return valid_imgs_L, valid_imgs_R, objpoints, imgpoints_L, imgpoints_R

def mono_calibration(calibration_images, frame_size, debug_dir,
                            objpoints, imgpoints_L, imgpoints_R, output_dir):

    # Create output and debug directories
    output_dir = f"{output_dir}/independent"
    os.makedirs(output_dir, exist_ok=True)

    debug_dir = f"{debug_dir}/independent"
    os.makedirs(debug_dir, exist_ok=True)

    # Camera calibration: Independent Fisheye for Left and Right cameras
    K_L = np.zeros((3, 3), dtype=np.float64)
    D_L = np.zeros((4, 1), dtype=np.float64)
    K_R = np.zeros((3, 3), dtype=np.float64)
    D_R = np.zeros((4, 1), dtype=np.float64)

    # Calibration flags
    flags = (
        cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC +
        cv2.fisheye.CALIB_CHECK_COND +
        cv2.fisheye.CALIB_FIX_SKEW
    )

    # Independent calibration for the left camera
    retL, K_L, D_L, rvecsL, tvecsL = cv2.fisheye.calibrate(
        objpoints, imgpoints_L, frame_size, K_L, D_L, flags=flags
    )

    # Independent calibration for the right camera
    retR, K_R, D_R, rvecsR, tvecsR = cv2.fisheye.calibrate(
        objpoints, imgpoints_R, frame_size, K_R, D_R, flags=flags
    )

    print(f"Left camera calibration error: {retL}")
    print(f"Right camera calibration error: {retR}")

    # Save parameters for both cameras
    np.save(os.path.join(output_dir, "K_L.npy"), K_L, allow_pickle=False)
    np.save(os.path.join(output_dir, "D_L.npy"), D_L, allow_pickle=False)
    np.save(os.path.join(output_dir, "K_R.npy"), K_R, allow_pickle=False)
    np.save(os.path.join(output_dir, "D_R.npy"), D_R, allow_pickle=False)

    # Rectify images and save debug results
    imagesL, imagesR = calibration_images
    for i, (imgL_path, imgR_path) in enumerate(zip(imagesL, imagesR)):
        imgL = cv2.imread(imgL_path)
        imgR = cv2.imread(imgR_path)

        # Undistort images independently
        map1_L, map2_L = cv2.fisheye.initUndistortRectifyMap(K_L, D_L, np.eye(3), K_L, frame_size, cv2.CV_16SC2)
        map1_R, map2_R = cv2.fisheye.initUndistortRectifyMap(K_R, D_R, np.eye(3), K_R, frame_size, cv2.CV_16SC2)

        rectifiedL = cv2.remap(imgL, map1_L, map2_L, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT)
        rectifiedR = cv2.remap(imgR, map1_R, map2_R, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT)

        # Combine left and right rectified images for visualization (not required, but useful for debugging)
        combined_rect = np.hstack((rectifiedL, rectifiedR))
        cv2.imwrite(os.path.join(debug_dir, f"independent_rectified_{i}.png"), combined_rect)

    return K_L, D_L, K_R, D_R

def stereo_calibration(calibration_images, frame_size, debug_dir,
                     objpoints, imgpoints_L, imgpoints_R, output_dir,
                     K_L, D_L, K_R, D_R):

    output_dir = f"{output_dir}/stereo"
    os.makedirs(output_dir, exist_ok=True)

    imagesL, imagesR = calibration_images

    # calibration_flags = (
        # cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC
        # cv2.fisheye.CALIB_CHECK_COND +
        # cv2.fisheye.CALIB_FIX_SKEW
    # )

    # K_L = np.zeros((3, 3), dtype=np.float64)
    # D_L = np.zeros((4, 1), dtype=np.float64)
    # K_R = np.zeros((3, 3), dtype=np.float64)
    # D_R = np.zeros((4, 1), dtype=np.float64)
    R = np.eye(3, dtype=np.float64)
    T = np.zeros((3, 1), dtype=np.float64)

    print("objpoints", len(objpoints), len(objpoints[0]), objpoints[0].shape)
    print("imgpoints_L", len(imgpoints_L), len(imgpoints_L[0]), imgpoints_L[0].shape)
    print("imgpoints_R", len(imgpoints_R), len(imgpoints_R[0]), imgpoints_R[0].shape)

    ret, K_L, D_L, K_R, D_R, R, T, E, F = cv2.fisheye.stereoCalibrate(
        objpoints, imgpoints_L, imgpoints_R,
        K_L, D_L, K_R, D_R,
        frame_size, R, T,
        # flags=calibration_flags,
        criteria=(cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 1e-6)
    )

    print(f"Stereo calibration error: {ret}")

    # Save parameters
    np.save(os.path.join(output_dir, "K_L.npy"), K_L, allow_pickle=False)
    np.save(os.path.join(output_dir, "D_L.npy"), D_L, allow_pickle=False)
    np.save(os.path.join(output_dir, "K_R.npy"), K_R, allow_pickle=False)
    np.save(os.path.join(output_dir, "D_R.npy"), D_R, allow_pickle=False)
    np.save(os.path.join(output_dir, "R.npy"), R, allow_pickle=False)
    np.save(os.path.join(output_dir, "T.npy"), T, allow_pickle=False)
    np.save(os.path.join(output_dir, "E.npy"), E, allow_pickle=False)
    np.save(os.path.join(output_dir, "F.npy"), F, allow_pickle=False)

    # Stereo rectification
    R_L, R_R, P_L, P_R, Q = cv2.fisheye.stereoRectify(
        K_L, D_L, K_R, D_R, frame_size, R, T,
        flags=cv2.fisheye.CALIB_ZERO_DISPARITY
    )

    xmapL, ymapL = cv2.fisheye.initUndistortRectifyMap(
        K_L, D_L, R_L, P_L, frame_size, cv2.CV_16SC2
    )
    xmapR, ymapR = cv2.fisheye.initUndistortRectifyMap(
        K_R, D_R, R_R, P_R, frame_size, cv2.CV_16SC2
    )

    for i, (imgL_path, imgR_path) in enumerate(zip(imagesL, imagesR)):
        imgL = cv2.imread(imgL_path)
        imgR = cv2.imread(imgR_path)

        rectifiedL = cv2.remap(imgL, xmapL, ymapL, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT)
        rectifiedR = cv2.remap(imgR, xmapR, ymapR, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT)

        combined_rect = np.hstack((rectifiedL, rectifiedR))
        cv2.imwrite(os.path.join(debug_dir, f"stereo_rectified_{i}.png"), combined_rect)


if __name__ == "__main__":

    # Parameters
    chessboard_size = (6, 7)
    square_size = 42.5

    # charuco_board_size = (5, 7)
    # square_length = 28.5
    # marker_length = 14.6
    # square_length = 30
    # marker_length = 15

    # aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_5X5_100)
    # board = cv2.aruco.CharucoBoard(charuco_board_size, square_length, marker_length, aruco_dict)
    # aruco_params = cv2.aruco.DetectorParameters()

    frame_size = (1280, 720)

    # Input and output dirs
    # images_dir = f"calibration_images/chessboard/big_rect/cb_big_rect"
    # images_dir = f"calibration_images/charuco/opencv/charuco"
    images_dir = "calibration_images/handpicked"
    image_dir_L = os.path.join(images_dir, "img_separated_L")
    image_dir_R = os.path.join(images_dir, "img_separated_R")
    output_dir = "calibration_matrices"
    os.makedirs(output_dir, exist_ok=True)

    # Debug directories
    debug_dir = "debug"
    debug_corners = os.path.join(debug_dir, "corners")
    debug_rectified = os.path.join(debug_dir, "rectified")
    debug_rejected = os.path.join(debug_dir, "rejected")
    debug_charuco = os.path.join(debug_dir, "charuco")
    for d in [debug_corners, debug_rectified, debug_rejected, debug_charuco]:
        os.makedirs(d, exist_ok=True)
    debug_dirs = (debug_corners, debug_rectified, debug_rejected)

    stereo_debug_dir = os.path.join(debug_dir, "stereo")
    os.makedirs(stereo_debug_dir, exist_ok=True)

    # Get image file paths (assumes matching sorted order)
    imagesL = sorted(glob.glob(os.path.join(image_dir_L, "*.png")))
    imagesR = sorted(glob.glob(os.path.join(image_dir_R, "*.png")))

    if len(imagesL) != len(imagesR) or len(imagesL) == 0:
        print("Error: Mismatch in number of left and right images or no images found.")
        sys.exit(1)

    valid_imgs_L = imagesL.copy()
    valid_imgs_R = imagesR.copy()
    recalibrate = True

    prev_L_length = len(valid_imgs_L)
    prev_R_length = len(valid_imgs_R)
    print(f"Number of valid images: {len(valid_imgs_L)}")
    print(f"Number of valid images: {len(valid_imgs_R)}")

    while recalibrate:
        calibration_images = (valid_imgs_L, valid_imgs_R)
        valid_imgs_L, valid_imgs_R, objpoints, imgpoints_L, imgpoints_R = image_selector(
            calibration_images, frame_size, chessboard_size, square_size, debug_dirs
        )
        # valid_imgs_L, valid_imgs_R, objpoints, imgpoints_L, imgpoints_R = charucho_image_selector(
        #     calibration_images, board, aruco_dict, aruco_params, debug_charuco
        # )

        if (len(valid_imgs_L) != prev_L_length or len(valid_imgs_R) != prev_R_length) and \
        (len(valid_imgs_L) != 0 and len(valid_imgs_R) != 0):
            prev_L_length = len(valid_imgs_L)
            prev_R_length = len(valid_imgs_R)
            print(f"Number of valid images: {len(valid_imgs_L)}")
            print(f"Number of valid images: {len(valid_imgs_R)}")

            print("Recalibrating...")
        else:
            recalibrate = False

    print(f"Accepted {len(valid_imgs_L)} images.")

    print("Running independent calibration...")
    K_L, D_L, K_R, D_R = mono_calibration(
        calibration_images, frame_size, debug_dir, objpoints,
        imgpoints_L, imgpoints_R, output_dir
    )

    print("Running stereo calibration...")
    calibration_images = (valid_imgs_L, valid_imgs_R)
    stereo_calibration(
        calibration_images, frame_size, stereo_debug_dir, objpoints,
        imgpoints_L, imgpoints_R, output_dir, K_L, D_L, K_R, D_R
    )

3 posts - 2 participants