calibrate_fisheye

calibrate_fisheye

Fisheye Camera Calibration

Calibrates the equidistant fisheye model (r = f * theta) used by mqtt_bridge.py. Standard OpenCV fisheye calibration uses Kannala-Brandt with polynomial distortion coefficients, which does not fit this 200-degree spherical lens on a 16:9 sensor.

This script uses a custom bundle adjustment that directly fits the equidistant projection model to checkerboard observations.

Parameters calibrated

cx, cy : optical center in the preprocessed frame (pixels) focal_length : equidistant projection focal length (pixels) k1, k2 : optional radial correction terms (r = f(theta + k1theta^3 + k2*theta^5))

Usage

From a set of images showing a checkerboard:

python calibrate_fisheye.py --images calibration_frames/*.jpg

From a video (extracts frames at an interval):

python calibrate_fisheye.py --video calibration.avi --interval 30

With a known board size and square dimensions:

python calibrate_fisheye.py --images *.jpg --board-width 9 --board-height 6 --square-size 0.025

Enable radial correction terms for higher accuracy:

python calibrate_fisheye.py --images *.jpg --fit-distortion

Interactive mode: step through a video and press 's' to select frames:

python calibrate_fisheye.py --video calibration.avi --interactive

preprocess_frame

preprocess_frame(frame, pad_size=3840, crop_size=3500)

Pad to square and center-crop, matching the mqtt_bridge pipeline.

Source code in calibration\calibrate_fisheye.py

def preprocess_frame(frame, pad_size=3840, crop_size=3500):
    """Pad to square and center-crop, matching the mqtt_bridge pipeline."""
    h, w = frame.shape[:2]
    padded = np.zeros((pad_size, pad_size, 3), dtype=frame.dtype)

    if h > pad_size or w > pad_size:
        h_crop = min(h, pad_size)
        w_crop = min(w, pad_size)
        y_start = (h - h_crop) // 2
        x_start = (w - w_crop) // 2
        padded = frame[y_start:y_start + h_crop, x_start:x_start + w_crop].copy()
    else:
        y_offset = (pad_size - h) // 2
        x_offset = (pad_size - w) // 2
        padded[y_offset:y_offset + h, x_offset:x_offset + w] = frame

    center = pad_size // 2
    half = crop_size // 2
    y0 = max(0, center - half)
    y1 = min(pad_size, center + half)
    x0 = max(0, center - half)
    x1 = min(pad_size, center + half)

    return padded[y0:y1, x0:x1]

equidistant_project

equidistant_project(pts_3d, rvec, tvec, cx, cy, f, k1=0.0, k2=0.0)

Project 3D points through the equidistant fisheye model.

Model

theta = angle between ray and optical axis r = f * (theta + k1theta^3 + k2theta^5) u = cx + r * cos(phi) v = cy + r * sin(phi)

Camera convention: x-right, y-down, z-forward (optical axis).

Parameters:

Name	Type	Description	Default
pts_3d		(N, 3) object-frame coordinates.	required
rvec		(3,) Rodrigues rotation vector.	required
tvec		(3,) translation vector.	required
cx, cy		optical center (pixels).	required
f		equidistant focal length (pixels).	required
k1, k2		radial correction coefficients.	required

Returns:

Type	Description
	(N, 2) pixel coordinates [u, v].

Source code in calibration\calibrate_fisheye.py

def equidistant_project(pts_3d, rvec, tvec, cx, cy, f, k1=0.0, k2=0.0):
    """Project 3D points through the equidistant fisheye model.

    Model:
        theta  = angle between ray and optical axis
        r      = f * (theta + k1*theta^3 + k2*theta^5)
        u      = cx + r * cos(phi)
        v      = cy + r * sin(phi)

    Camera convention: x-right, y-down, z-forward (optical axis).

    Args:
        pts_3d:  (N, 3) object-frame coordinates.
        rvec:    (3,) Rodrigues rotation vector.
        tvec:    (3,) translation vector.
        cx, cy:  optical center (pixels).
        f:       equidistant focal length (pixels).
        k1, k2:  radial correction coefficients.

    Returns:
        (N, 2) pixel coordinates [u, v].
    """
    R = Rotation.from_rotvec(rvec).as_matrix()
    p_cam = (R @ pts_3d.T).T + tvec  # (N, 3) in camera frame

    x, y, z = p_cam[:, 0], p_cam[:, 1], p_cam[:, 2]
    norm = np.sqrt(x ** 2 + y ** 2 + z ** 2)
    norm = np.maximum(norm, 1e-12)

    theta = np.arccos(np.clip(z / norm, -1.0, 1.0))
    phi = np.arctan2(y, x)

    # Equidistant projection with optional polynomial correction
    r = f * (theta + k1 * theta ** 3 + k2 * theta ** 5)

    u = cx + r * np.cos(phi)
    v = cy + r * np.sin(phi)
    return np.column_stack([u, v])

build_perspective_remap

build_perspective_remap(fish_h, fish_w, cx, cy, focal_length, persp_size=1500, persp_fov_deg=160.0)

Build remap tables that convert a fisheye image to a perspective view.

findChessboardCorners requires straight lines between corners, which does not hold under fisheye distortion. Remapping to a perspective (rectilinear) view straightens those lines so detection works.

The remap is built from the approximate equidistant model — it does not need to be perfectly accurate, only good enough that the perspective image has approximately straight lines.

Returns:

Type	Description
	dict with map_x, map_y (for cv2.remap), f_persp, persp_size.

Source code in calibration\calibrate_fisheye.py

def build_perspective_remap(fish_h, fish_w, cx, cy, focal_length,
                            persp_size=1500, persp_fov_deg=160.0):
    """Build remap tables that convert a fisheye image to a perspective view.

    findChessboardCorners requires straight lines between corners, which
    does not hold under fisheye distortion.  Remapping to a perspective
    (rectilinear) view straightens those lines so detection works.

    The remap is built from the approximate equidistant model — it does not
    need to be perfectly accurate, only good enough that the perspective
    image has approximately straight lines.

    Returns:
        dict with map_x, map_y (for cv2.remap), f_persp, persp_size.
    """
    f_persp = persp_size / (2.0 * np.tan(np.deg2rad(persp_fov_deg / 2.0)))
    cx_p = (persp_size - 1) / 2.0
    cy_p = (persp_size - 1) / 2.0

    v_p, u_p = np.mgrid[0:persp_size, 0:persp_size].astype(np.float64)

    # Perspective pixel → ray direction (pinhole model)
    x_n = (u_p - cx_p) / f_persp
    y_n = (v_p - cy_p) / f_persp

    # Incidence angle and azimuth
    theta = np.arctan(np.sqrt(x_n ** 2 + y_n ** 2))
    phi = np.arctan2(y_n, x_n)

    # Equidistant projection: r = f * theta
    r_fish = focal_length * theta
    map_x = (cx + r_fish * np.cos(phi)).astype(np.float32)
    map_y = (cy + r_fish * np.sin(phi)).astype(np.float32)

    return {
        'map_x': map_x,
        'map_y': map_y,
        'f_persp': f_persp,
        'persp_size': persp_size,
        'cx_fish': cx,
        'cy_fish': cy,
        'f_fish': focal_length,
    }

detect_checkerboard

detect_checkerboard(image, board_size, remap)

Detect checkerboard corners in a fisheye image.

Pipeline

1. Remap fisheye → perspective (straightens curved lines)
2. Detect checkerboard in the perspective image
3. Map corners back to fisheye pixel coordinates
4. Refine with cornerSubPix on the original fisheye image

Parameters:

Name	Type	Description	Default
image		Full-resolution fisheye image (BGR or grayscale).	required
board_size		(cols, rows) inner corner count.	required
remap		Dict from build_perspective_remap().	required

Returns:

Type	Description
	(found, corners) where corners is (N, 1, 2) in fisheye pixels.

Source code in calibration\calibrate_fisheye.py

def detect_checkerboard(image, board_size, remap):
    """Detect checkerboard corners in a fisheye image.

    Pipeline:
      1. Remap fisheye → perspective (straightens curved lines)
      2. Detect checkerboard in the perspective image
      3. Map corners back to fisheye pixel coordinates
      4. Refine with cornerSubPix on the original fisheye image

    Args:
        image:       Full-resolution fisheye image (BGR or grayscale).
        board_size:  (cols, rows) inner corner count.
        remap:       Dict from build_perspective_remap().

    Returns:
        (found, corners)  where corners is (N, 1, 2) in fisheye pixels.
    """
    gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) if image.ndim == 3 else image

    # 1. Remap fisheye → perspective
    persp = cv2.remap(gray, remap['map_x'], remap['map_y'],
                      cv2.INTER_LINEAR, borderValue=0)

    # 2. Detect in perspective space
    found, corners = _detect_corners(persp, board_size)

    if not found:
        return False, None

    # 3. Map corners back to fisheye coordinates
    corners_fish = _persp_corners_to_fisheye(corners, remap)

    # 4. Subpixel refinement on the original fisheye image
    criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER,
                100, 0.001)
    corners_fish = cv2.cornerSubPix(gray, corners_fish, (11, 11),
                                    (-1, -1), criteria)

    return True, corners_fish

calibrate

calibrate(obj_pts_list, img_pts_list, image_size, initial_cx=None, initial_cy=None, initial_f=None, fit_distortion=False)

Run full calibration via two-stage bundle adjustment.

Stage 1 — Levenberg-Marquardt with linear loss (fast convergence). Stage 2 — Trust-region with soft_l1 loss (outlier robustness).

Parameters:

Name	Type	Description	Default
obj_pts_list		list of (N, 3) checkerboard 3D points per frame.	required
img_pts_list		list of (N, 2) detected pixel coordinates per frame.	required
image_size		(width, height) of preprocessed frame.	required
initial_cx/cy		initial optical-center guess (default: frame center).	required
initial_f		initial focal-length guess (default: from 200 deg FOV).	None
fit_distortion		if True, also fit k1, k2 correction coefficients.	False

Returns:

Type	Description
	dict with calibrated parameters, per-frame errors, and diagnostics.

Source code in calibration\calibrate_fisheye.py

def calibrate(obj_pts_list, img_pts_list, image_size,
              initial_cx=None, initial_cy=None, initial_f=None,
              fit_distortion=False):
    """Run full calibration via two-stage bundle adjustment.

    Stage 1 — Levenberg-Marquardt with linear loss (fast convergence).
    Stage 2 — Trust-region with soft_l1 loss (outlier robustness).

    Args:
        obj_pts_list:   list of (N, 3) checkerboard 3D points per frame.
        img_pts_list:   list of (N, 2) detected pixel coordinates per frame.
        image_size:     (width, height) of preprocessed frame.
        initial_cx/cy:  initial optical-center guess (default: frame center).
        initial_f:      initial focal-length guess (default: from 200 deg FOV).
        fit_distortion: if True, also fit k1, k2 correction coefficients.

    Returns:
        dict with calibrated parameters, per-frame errors, and diagnostics.
    """
    n_frames = len(obj_pts_list)
    w, h = image_size

    if initial_cx is None:
        initial_cx = (w - 1) / 2.0
    if initial_cy is None:
        initial_cy = (h - 1) / 2.0
    if initial_f is None:
        radius_guess = min(w, h) / 2.0
        initial_f = (2 * radius_guess) * 180.0 / (200.0 * np.pi)

    n_intrinsic = 5 if fit_distortion else 3
    k1_init, k2_init = 0.0, 0.0

    # --- Initialize per-frame extrinsics via PnP, then refine individually ---
    print("  Initializing per-frame extrinsics...")
    rvecs_init, tvecs_init = [], []
    for obj, img in zip(obj_pts_list, img_pts_list):
        rv, tv = _init_extrinsics_pnp(obj, img, initial_cx, initial_cy, initial_f)
        rv, tv = _refine_single_frame(obj, img, rv, tv, initial_cx, initial_cy, initial_f)
        rvecs_init.append(rv)
        tvecs_init.append(tv)

    # --- Pack parameter vector ---
    params0 = np.zeros(n_intrinsic + n_frames * 6)
    params0[0] = initial_cx
    params0[1] = initial_cy
    params0[2] = initial_f
    if fit_distortion:
        params0[3] = k1_init
        params0[4] = k2_init
    for i in range(n_frames):
        off = n_intrinsic + i * 6
        params0[off:off + 3] = rvecs_init[i]
        params0[off + 3:off + 6] = tvecs_init[i]

    pts_per_frame = [len(p) for p in img_pts_list]
    total_pts = sum(pts_per_frame)
    n_params = len(params0)
    jac_sparsity = _build_jac_sparsity(n_frames, pts_per_frame, fit_distortion)

    # --- Stage 1: Levenberg-Marquardt (linear loss, fast convergence) ---
    print(f"\n  Stage 1 — LM optimisation: {n_params} params "
          f"({n_frames} frames, {total_pts} corners)")

    result = least_squares(
        _build_residuals,
        params0,
        args=(obj_pts_list, img_pts_list, n_frames, fit_distortion),
        method='lm',
        ftol=1e-12,
        xtol=1e-12,
        gtol=1e-12,
        max_nfev=10000,
        verbose=1,
    )

    # --- Stage 2: Trust-region with soft_l1 for outlier robustness ---
    lower = np.full_like(result.x, -np.inf)
    upper = np.full_like(result.x, np.inf)
    lower[0], upper[0] = initial_cx - w * 0.2, initial_cx + w * 0.2
    lower[1], upper[1] = initial_cy - h * 0.2, initial_cy + h * 0.2
    lower[2], upper[2] = initial_f * 0.2, initial_f * 3.0
    if fit_distortion:
        lower[3], upper[3] = -2.0, 2.0
        lower[4], upper[4] = -2.0, 2.0

    print(f"  Stage 2 — TRF + soft_l1 refinement")

    result = least_squares(
        _build_residuals,
        result.x,
        args=(obj_pts_list, img_pts_list, n_frames, fit_distortion),
        method='trf',
        bounds=(lower, upper),
        jac_sparsity=jac_sparsity,
        loss='soft_l1',
        f_scale=1.0,
        ftol=1e-12,
        xtol=1e-12,
        gtol=1e-12,
        max_nfev=2000,
        verbose=1,
    )

    # --- Unpack results ---
    cx_opt, cy_opt, f_opt = result.x[0], result.x[1], result.x[2]
    k1_opt = result.x[3] if fit_distortion else 0.0
    k2_opt = result.x[4] if fit_distortion else 0.0

    opt_rvecs, opt_tvecs, per_frame_rms = [], [], []
    for i in range(n_frames):
        off = n_intrinsic + i * 6
        rv = result.x[off:off + 3]
        tv = result.x[off + 3:off + 6]
        opt_rvecs.append(rv)
        opt_tvecs.append(tv)
        proj = equidistant_project(obj_pts_list[i], rv, tv, cx_opt, cy_opt, f_opt, k1_opt, k2_opt)
        err = np.sqrt(np.mean((proj - img_pts_list[i]) ** 2))
        per_frame_rms.append(err)

    rms_total = np.sqrt(np.mean(result.fun ** 2))

    # Derive FOV from calibrated focal length
    radius_px = min(w, h) / 2.0
    fov_deg = (2 * radius_px) * 180.0 / (f_opt * np.pi)

    return {
        'cx': cx_opt,
        'cy': cy_opt,
        'focal_length': f_opt,
        'fov_deg': fov_deg,
        'radius_px': radius_px,
        'k1': k1_opt,
        'k2': k2_opt,
        'rms_error': rms_total,
        'per_frame_rms': per_frame_rms,
        'num_frames': n_frames,
        'num_points': total_pts,
        'rvecs': opt_rvecs,
        'tvecs': opt_tvecs,
        'optimizer_cost': result.cost,
        'optimizer_status': result.message,
    }

draw_reprojection

draw_reprojection(image, obj_pts, img_pts, rvec, tvec, cx, cy, f, k1=0.0, k2=0.0, label='')

Overlay detected (green) and reprojected (red) corners on an image.

Source code in calibration\calibrate_fisheye.py

def draw_reprojection(image, obj_pts, img_pts, rvec, tvec,
                      cx, cy, f, k1=0.0, k2=0.0, label=""):
    """Overlay detected (green) and reprojected (red) corners on an image."""
    vis = image.copy()
    proj = equidistant_project(obj_pts, rvec, tvec, cx, cy, f, k1, k2)

    for obs, rep in zip(img_pts, proj):
        ox, oy = int(round(obs[0])), int(round(obs[1]))
        rx, ry = int(round(rep[0])), int(round(rep[1]))
        cv2.circle(vis, (ox, oy), 6, (0, 255, 0), 2)
        cv2.drawMarker(vis, (rx, ry), (0, 0, 255), cv2.MARKER_CROSS, 12, 2)
        cv2.line(vis, (ox, oy), (rx, ry), (255, 128, 0), 1)

    if label:
        cv2.putText(vis, label, (20, 50),
                    cv2.FONT_HERSHEY_SIMPLEX, 1.2, (255, 255, 255), 2)
    return vis

draw_detection

draw_detection(image, corners, board_size)

Draw detected checkerboard corners on an image.

Source code in calibration\calibrate_fisheye.py

def draw_detection(image, corners, board_size):
    """Draw detected checkerboard corners on an image."""
    vis = image.copy()
    cv2.drawChessboardCorners(vis, board_size, corners, True)
    return vis

load_frames_from_images

load_frames_from_images(patterns)

Load frames from image file paths / glob patterns.

Source code in calibration\calibrate_fisheye.py

def load_frames_from_images(patterns):
    """Load frames from image file paths / glob patterns."""
    paths = []
    for pat in patterns:
        paths.extend(sorted(glob.glob(pat)))
    frames = []
    for p in paths:
        img = cv2.imread(p)
        if img is not None:
            frames.append((Path(p).name, img))
        else:
            print(f"  Warning: could not read {p}")
    return frames

load_frames_from_video

load_frames_from_video(video_path, interval, max_frames)

Extract frames from a video at a regular interval.

Source code in calibration\calibrate_fisheye.py

def load_frames_from_video(video_path, interval, max_frames):
    """Extract frames from a video at a regular interval."""
    cap = cv2.VideoCapture(video_path)
    if not cap.isOpened():
        print(f"Error: cannot open video {video_path}")
        sys.exit(1)
    total = int(cap.get(cv2.CAP_PROP_FRAME_COUNT))
    frames = []
    idx = 0
    while len(frames) < max_frames:
        ret, frame = cap.read()
        if not ret:
            break
        if idx % interval == 0:
            frames.append((f"frame_{idx:06d}", frame))
        idx += 1
    cap.release()
    print(f"Extracted {len(frames)} frames from video "
          f"({total} total, interval={interval})")
    return frames

interactive_select_frames

interactive_select_frames(video_path, pad_size, crop_size, board_size, remap)

Step through a video and let the user press 's' to select frames.

Controls

s - save current frame for calibration d / -> - skip forward 10 frames a / <- - skip backward 10 frames space - advance 1 frame q / Esc - done selecting

Source code in calibration\calibrate_fisheye.py

def interactive_select_frames(video_path, pad_size, crop_size, board_size,
                              remap):
    """Step through a video and let the user press 's' to select frames.

    Controls:
        s       - save current frame for calibration
        d / ->  - skip forward 10 frames
        a / <-  - skip backward 10 frames
        space   - advance 1 frame
        q / Esc - done selecting
    """
    cap = cv2.VideoCapture(video_path)
    if not cap.isOpened():
        print(f"Error: cannot open video {video_path}")
        sys.exit(1)

    total = int(cap.get(cv2.CAP_PROP_FRAME_COUNT))
    print(f"\nInteractive selection — {total} frames in video")
    print("  [s] save frame   [space] next   [d/→] +10   [a/←] -10   [q] done\n")

    selected = []
    idx = 0

    while True:
        ret, frame = cap.read()
        if not ret:
            break

        prep = preprocess_frame(frame, pad_size, crop_size)
        found, corners = detect_checkerboard(prep, board_size, remap)

        # Build display image
        disp = prep.copy()
        if found:
            cv2.drawChessboardCorners(disp, board_size, corners, True)
            status = "DETECTED"
            colour = (0, 255, 0)
        else:
            status = "not found"
            colour = (0, 0, 255)

        text = f"Frame {idx}/{total}  |  Board: {status}  |  Saved: {len(selected)}"
        # Scale for display
        scale = min(1.0, 1200.0 / max(disp.shape[:2]))
        small = cv2.resize(disp, None, fx=scale, fy=scale)
        cv2.putText(small, text, (15, 35),
                    cv2.FONT_HERSHEY_SIMPLEX, 0.8, colour, 2)
        cv2.imshow("Calibration - Select Frames", small)

        key = cv2.waitKey(0) & 0xFF
        if key == ord('q') or key == 27:
            break
        elif key == ord('s') and found:
            selected.append((f"frame_{idx:06d}", frame.copy()))
            print(f"  Saved frame {idx} ({len(selected)} total)")
        elif key == ord('d') or key == 83:  # right arrow
            skip = min(10, total - idx - 1)
            for _ in range(skip):
                cap.read()
                idx += 1
        elif key == ord('a') or key == 81:  # left arrow
            new_idx = max(0, idx - 10)
            cap.set(cv2.CAP_PROP_POS_FRAMES, new_idx)
            idx = new_idx - 1  # will be incremented below
        # space or any other key: advance one frame

        idx += 1

    cv2.destroyAllWindows()
    cap.release()
    print(f"\nSelected {len(selected)} frames")
    return selected