Day 11 — Camera Geometry and Calibration

  World point P = (X, Y, Z)
         *
          \
           \     light ray
            \
             \
              \
    ───────────O───────────── optical axis (Z_c)
               │\             (optical center)
               │ \
               │  * image point p = (u, v)
               │
          Image Plane (at focal length f from O)

Resolution:   3280 x 2464
Sensor size:  3.68 x 2.76 mm
Focal length: 3.04 mm
Pixel size:   3.68 / 3280 = 0.00112 mm/px

f_x = 3.04 / 0.00112 = 2714 px
f_y = 3.04 / 0.00112 = 2714 px
c_x = 3280 / 2 = 1640 px
c_y = 2464 / 2 = 1232 px

K = [2714    0   1640]
    [   0  2714  1232]
    [   0     0     1]

FOV_x = 2 * arctan(3280 / (2*2714)) = 62.2 degrees

  World Frame (W)              Camera Frame (C)

  Z_w (up)                     Z_c (forward / optical axis)
   |                            |
   |                            |
   |______ X_w (east)           |______ X_c (right in image)
  /                            /
 Y_w (north)                  Y_c (down in image)

  Barrel                No distortion          Pincushion
  (k1 < 0)                                    (k1 > 0)

  ┌──────────┐          ┌──────────┐          ┌──────────┐
  │ (      ) │          │ |      | │          │ )      ( │
  │(        )│          │ |      | │          │)        (│
  │(        )│          │ |      | │          │)        (│
  │ (      ) │          │ |      | │          │ )      ( │
  └──────────┘          └──────────┘          └──────────┘
  Straight lines        Straight lines        Straight lines
  bow outward           remain straight       bow inward

  Front camera view:              Bird's Eye View:

      ╲          ╱                 |              |
       ╲        ╱                  |              |
        ╲      ╱                   |              |
         ╲    ╱                    |              |
          ╲  ╱                     |              |
           ╲╱   <-- vanishing     |              |
           ╱╲       point          |              |
          ╱  ╲                     |              |
         ╱    ╲                    |              |
        ╱      ╲                   |              |
       ╱________╲                  |______________|

  Lanes converge                  Lanes are parallel
  (hard to measure width)        (easy to measure width)

  Camera Image:                    BEV Image:

  (src[0])──────────(src[1])       (dst[0])────────(dst[1])
      \                /            |                    |
       \              /             |                    |
        \            /              |                    |
         \          /               |                    |
  (src[3])──(src[2])               (dst[3])────────(dst[2])

"""
capture_calibration.py
Capture checkerboard images for camera calibration.
Press 's' to save when pattern is detected, 'q' to quit.
"""

import cv2
import os
import time


def capture_calibration_images(camera_index=0, save_dir='calibration_images',
                                pattern_size=(9, 6)):
    """
    Interactive tool for capturing calibration images.

    Shows live preview with checkerboard detection overlay.
    Only allows saving when the pattern is successfully found.
    """
    os.makedirs(save_dir, exist_ok=True)

    cap = cv2.VideoCapture(camera_index)
    cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
    cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)

    if not cap.isOpened():
        print("Error: cannot open camera")
        return

    count = 0
    last_save = 0

    print("=" * 60)
    print("Camera Calibration Image Capture")
    print("=" * 60)
    print(f"Pattern size: {pattern_size[0]}x{pattern_size[1]} inner corners")
    print(f"Save directory: {save_dir}")
    print()
    print("Instructions:")
    print("  1. Hold checkerboard in front of camera")
    print("  2. When green overlay appears, press 's' to save")
    print("  3. Move board to different angle/distance, repeat")
    print("  4. Aim for 15-25 images from varied poses")
    print("  5. Press 'q' to quit")
    print()
    print("Tips for good calibration:")
    print("  - Cover all regions of the image (corners too!)")
    print("  - Include tilted views (30-45 degree angles)")
    print("  - Vary the distance (close, medium, far)")
    print("  - Keep the board flat and still when saving")
    print("=" * 60)

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

        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        found, corners = cv2.findChessboardCorners(
            gray, pattern_size,
            cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_NORMALIZE_IMAGE
        )

        display = frame.copy()

        if found:
            criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER,
                        30, 0.001)
            corners_refined = cv2.cornerSubPix(
                gray, corners, (11, 11), (-1, -1), criteria
            )
            cv2.drawChessboardCorners(display, pattern_size,
                                      corners_refined, found)
            cv2.putText(display, "PATTERN FOUND - press 's' to save",
                        (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7,
                        (0, 255, 0), 2)
        else:
            cv2.putText(display, "Pattern not detected...",
                        (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7,
                        (0, 0, 255), 2)

        cv2.putText(display, f"Images saved: {count}",
                    (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.7,
                    (255, 255, 0), 2)
        cv2.imshow('Calibration Capture', display)

        key = cv2.waitKey(1) & 0xFF

        if key == ord('s') and found and (time.time() - last_save > 0.5):
            filename = os.path.join(save_dir, f'calib_{count:03d}.png')
            cv2.imwrite(filename, frame)
            print(f"  Saved {filename}")
            count += 1
            last_save = time.time()
        elif key == ord('q'):
            break

    cap.release()
    cv2.destroyAllWindows()
    print(f"\nTotal images saved: {count}")
    if count < 10:
        print("WARNING: At least 10 images recommended for good calibration.")


if __name__ == "__main__":
    capture_calibration_images()

"""
calibrate_camera.py
Calibrate camera from checkerboard images using Zhang's method (via OpenCV).
Reports per-image errors and overall quality metrics.
"""

import cv2
import numpy as np
import glob
import os
import yaml


def calibrate(image_dir='calibration_images',
              pattern_size=(9, 6),
              square_size_mm=25.0):
    """
    Calibrate camera from checkerboard images.

    Args:
        image_dir:     directory containing calibration PNG images
        pattern_size:  (columns, rows) of inner checkerboard corners
        square_size_mm: physical size of each square in mm

    Returns:
        ret:    RMS reprojection error
        K:      3x3 intrinsic matrix
        D:      1x5 distortion coefficients
        rvecs:  list of rotation vectors (one per image)
        tvecs:  list of translation vectors (one per image)
    """
    # --- Prepare 3D object points ---
    objp = np.zeros((pattern_size[0] * pattern_size[1], 3), np.float32)
    objp[:, :2] = np.mgrid[0:pattern_size[0],
                            0:pattern_size[1]].T.reshape(-1, 2)
    objp *= square_size_mm

    obj_points = []
    img_points = []
    img_shape = None

    # --- Process each image ---
    images = sorted(glob.glob(os.path.join(image_dir, '*.png')))
    if not images:
        images = sorted(glob.glob(os.path.join(image_dir, '*.jpg')))
    print(f"Found {len(images)} images in {image_dir}/")

    criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)

    for fname in images:
        img = cv2.imread(fname)
        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

        if img_shape is None:
            img_shape = gray.shape[::-1]

        ret, corners = cv2.findChessboardCorners(
            gray, pattern_size,
            cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_NORMALIZE_IMAGE
        )

        if ret:
            corners_refined = cv2.cornerSubPix(
                gray, corners, (11, 11), (-1, -1), criteria
            )
            obj_points.append(objp)
            img_points.append(corners_refined)
            print(f"  [OK]   {os.path.basename(fname)}: "
                  f"{len(corners_refined)} corners detected")
        else:
            print(f"  [SKIP] {os.path.basename(fname)}: pattern not found")

    if len(obj_points) < 3:
        raise ValueError(
            f"Need at least 3 valid images, found only {len(obj_points)}"
        )

    print(f"\nCalibrating with {len(obj_points)} valid images...")

    # --- Run calibration ---
    ret, K, D, rvecs, tvecs = cv2.calibrateCamera(
        obj_points, img_points, img_shape, None, None
    )

    # --- Report results ---
    print(f"\n{'='*60}")
    print(f"CALIBRATION RESULTS")
    print(f"{'='*60}")
    print(f"\nRMS Reprojection Error: {ret:.4f} pixels")

    if ret < 0.3:
        quality = "EXCELLENT"
    elif ret < 0.5:
        quality = "GOOD"
    elif ret < 1.0:
        quality = "ACCEPTABLE"
    else:
        quality = "POOR - check pattern and images"
    print(f"Quality: {quality}")

    print(f"\nIntrinsic Matrix K:")
    print(f"  fx = {K[0, 0]:.2f} px")
    print(f"  fy = {K[1, 1]:.2f} px")
    print(f"  cx = {K[0, 2]:.2f} px")
    print(f"  cy = {K[1, 2]:.2f} px")

    w, h = img_shape
    fov_x = 2 * np.arctan(w / (2 * K[0, 0])) * 180 / np.pi
    fov_y = 2 * np.arctan(h / (2 * K[1, 1])) * 180 / np.pi
    print(f"\nField of View:")
    print(f"  Horizontal: {fov_x:.1f} degrees")
    print(f"  Vertical:   {fov_y:.1f} degrees")

    print(f"\nDistortion Coefficients D:")
    print(f"  k1 = {D[0, 0]:+.6f}  (radial)")
    print(f"  k2 = {D[0, 1]:+.6f}  (radial)")
    print(f"  p1 = {D[0, 2]:+.6f}  (tangential)")
    print(f"  p2 = {D[0, 3]:+.6f}  (tangential)")
    print(f"  k3 = {D[0, 4]:+.6f}  (radial)")

    # --- Per-image reprojection errors ---
    print(f"\nPer-image reprojection errors:")
    errors = []
    for i, (rvec, tvec) in enumerate(zip(rvecs, tvecs)):
        reproj, _ = cv2.projectPoints(obj_points[i], rvec, tvec, K, D)
        error = cv2.norm(img_points[i], reproj, cv2.NORM_L2) / len(reproj)
        errors.append(error)
        marker = " ***" if error > 1.0 else ""
        print(f"  Image {i:3d}: {error:.4f} px{marker}")

    print(f"\n  Mean:   {np.mean(errors):.4f} px")
    print(f"  Median: {np.median(errors):.4f} px")
    print(f"  Max:    {np.max(errors):.4f} px")

    return ret, K, D, rvecs, tvecs, img_shape


if __name__ == "__main__":
    ret, K, D, rvecs, tvecs, img_shape = calibrate()

"""
undistort_demo.py
Apply distortion correction and compare different alpha values.
"""

import cv2
import numpy as np
import matplotlib.pyplot as plt


def undistort_comparison(image_path, K, D, img_shape):
    """
    Show original vs undistorted with different alpha values.

    alpha controls the tradeoff:
      alpha=0: all output pixels are valid (some FOV is lost)
      alpha=1: all source pixels retained (black borders appear)
    """
    img = cv2.imread(image_path)
    h, w = img.shape[:2]

    # Simple undistort
    undistorted_simple = cv2.undistort(img, K, D)

    # Optimal alpha=0 (no black borders)
    new_K_0, roi_0 = cv2.getOptimalNewCameraMatrix(K, D, (w, h), alpha=0)
    undistorted_a0 = cv2.undistort(img, K, D, None, new_K_0)

    # Optimal alpha=0.5 (balanced)
    new_K_05, roi_05 = cv2.getOptimalNewCameraMatrix(K, D, (w, h), alpha=0.5)
    undistorted_a05 = cv2.undistort(img, K, D, None, new_K_05)

    # Optimal alpha=1 (keep all pixels)
    new_K_1, roi_1 = cv2.getOptimalNewCameraMatrix(K, D, (w, h), alpha=1)
    undistorted_a1 = cv2.undistort(img, K, D, None, new_K_1)

    # Using remap (faster for video — compute maps once, apply many times)
    map1, map2 = cv2.initUndistortRectifyMap(
        K, D, None, new_K_05, (w, h), cv2.CV_32FC1
    )
    undistorted_remap = cv2.remap(img, map1, map2, cv2.INTER_LINEAR)

    # --- Display ---
    fig, axes = plt.subplots(2, 3, figsize=(18, 10))

    images_list = [
        (img, 'Original (distorted)'),
        (undistorted_simple, 'Simple undistort'),
        (undistorted_a0, 'alpha=0 (no black borders)'),
        (undistorted_a05, 'alpha=0.5 (balanced)'),
        (undistorted_a1, 'alpha=1 (all source pixels)'),
        (undistorted_remap, 'Remap (same as alpha=0.5)'),
    ]

    for ax, (im, title) in zip(axes.flat, images_list):
        ax.imshow(cv2.cvtColor(im, cv2.COLOR_BGR2RGB))
        ax.set_title(title)
        ax.axis('off')

    plt.suptitle('Distortion Correction Comparison', fontsize=14)
    plt.tight_layout()
    plt.savefig('undistortion_comparison.png', dpi=150)
    plt.show()

    print("\nPerformance note:")
    print("  cv2.undistort():  recomputes mapping every call")
    print("  cv2.remap():      uses precomputed maps (10x faster for video)")
    print("  For real-time: compute map1/map2 once, then remap every frame")

"""
bev_transform.py
Compute and apply Bird's Eye View (BEV) perspective transform.
"""

import cv2
import numpy as np
import matplotlib.pyplot as plt


def compute_bev_transform(img_shape=(480, 640)):
    """Define source/destination points for BEV homography."""
    h, w = img_shape

    # Source: trapezoid on road (tune for your camera mount!)
    src = np.float32([
        [w * 0.40, h * 0.65],   # top-left
        [w * 0.60, h * 0.65],   # top-right
        [w * 0.85, h * 0.95],   # bottom-right
        [w * 0.15, h * 0.95],   # bottom-left
    ])

    # Destination: rectangle in BEV
    margin = w * 0.25
    dst = np.float32([
        [margin, 0],
        [w - margin, 0],
        [w - margin, h],
        [margin, h],
    ])

    H_bev = cv2.getPerspectiveTransform(src, dst)
    H_bev_inv = cv2.getPerspectiveTransform(dst, src)

    return H_bev, H_bev_inv, src, dst


def demo_bev():
    """Create synthetic road image and demonstrate BEV transform."""
    h, w = 480, 640

    # Create synthetic road
    img = np.zeros((h, w, 3), dtype=np.uint8)
    img[:] = [80, 80, 80]
    img[:int(h * 0.55)] = [200, 180, 150]  # sky

    vanish_x, vanish_y = w // 2, int(h * 0.55)

    # Lane lines
    cv2.line(img, (int(w * 0.15), h), (vanish_x - 10, vanish_y),
             (0, 255, 255), 3)
    cv2.line(img, (int(w * 0.85), h), (vanish_x + 10, vanish_y),
             (0, 255, 255), 3)

    # Dashed center line
    for i in range(8):
        frac1 = 0.55 + i * 0.055
        frac2 = frac1 + 0.025
        if frac2 > 1.0:
            break
        y1, y2 = int(h * frac1), int(h * frac2)
        x1 = int(vanish_x + (w * 0.5 - vanish_x) * (frac1 - 0.55) / 0.45)
        x2 = int(vanish_x + (w * 0.5 - vanish_x) * (frac2 - 0.55) / 0.45)
        cv2.line(img, (x1, y1), (x2, y2), (255, 255, 255), 2)

    # BEV transform
    H_bev, H_bev_inv, src, dst = compute_bev_transform((h, w))
    bev = cv2.warpPerspective(img, H_bev, (w, h))

    # Annotate
    img_annotated = img.copy()
    cv2.polylines(img_annotated, [src.astype(np.int32)], True, (0, 0, 255), 2)

    # Display
    fig, axes = plt.subplots(1, 2, figsize=(14, 6))
    axes[0].imshow(cv2.cvtColor(img_annotated, cv2.COLOR_BGR2RGB))
    axes[0].set_title('Camera View (source trapezoid in red)')
    axes[1].imshow(cv2.cvtColor(bev, cv2.COLOR_BGR2RGB))
    axes[1].set_title("Bird's Eye View (lanes are now parallel)")
    for ax in axes:
        ax.axis('off')
    plt.tight_layout()
    plt.savefig('bev_transform.png', dpi=150)
    plt.show()

    print("BEV Homography Matrix H:")
    np.set_printoptions(precision=4, suppress=True)
    print(H_bev)


demo_bev()

"""
save_calibration_ros2.py
Save camera calibration in ROS2 camera_info YAML format.
"""

import numpy as np
import yaml
import os


def save_calibration_yaml(filename, K, D, img_shape,
                          camera_name='autonomous_car_camera',
                          distortion_model='plumb_bob'):
    """
    Save calibration in ROS2 camera_calibration format.

    This YAML file can be loaded by:
    - image_proc nodes for rectification
    - camera_info_manager for publishing CameraInfo
    - Any ROS2 node subscribing to sensor_msgs/CameraInfo
    """
    w, h = img_shape

    R = np.eye(3)
    P = np.zeros((3, 4))
    P[:3, :3] = K

    calibration = {
        'image_width': int(w),
        'image_height': int(h),
        'camera_name': camera_name,
        'camera_matrix': {
            'rows': 3, 'cols': 3,
            'data': [float(x) for x in K.flatten()]
        },
        'distortion_model': distortion_model,
        'distortion_coefficients': {
            'rows': 1, 'cols': 5,
            'data': [float(x) for x in D.flatten()]
        },
        'rectification_matrix': {
            'rows': 3, 'cols': 3,
            'data': [float(x) for x in R.flatten()]
        },
        'projection_matrix': {
            'rows': 3, 'cols': 4,
            'data': [float(x) for x in P.flatten()]
        }
    }

    with open(filename, 'w') as f:
        yaml.dump(calibration, f, default_flow_style=False)

    print(f"Calibration saved to {filename}")
    print(f"\nTo use in ROS2 launch file:")
    print(f"  camera_info_url: 'file://{os.path.abspath(filename)}'")


# Example
if __name__ == "__main__":
    K = np.array([[615.0, 0, 320.0],
                  [0, 615.0, 240.0],
                  [0, 0, 1.0]])
    D = np.array([[-0.05, 0.12, 0.001, -0.002, -0.08]])
    save_calibration_yaml('camera_calibration.yaml', K, D, (640, 480))

image_width: 640
image_height: 480
camera_name: autonomous_car_camera
camera_matrix:
  rows: 3
  cols: 3
  data: [615.0, 0.0, 320.0, 0.0, 615.0, 240.0, 0.0, 0.0, 1.0]
distortion_model: plumb_bob
distortion_coefficients:
  rows: 1
  cols: 5
  data: [-0.05, 0.12, 0.001, -0.002, -0.08]
rectification_matrix:
  rows: 3
  cols: 3
  data: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
projection_matrix:
  rows: 3
  cols: 4
  data: [615.0, 0.0, 320.0, 0.0, 0.0, 615.0, 240.0, 0.0, 0.0, 0.0, 1.0, 0.0]