# ==============================================================================
# EXPERIMENT 1: IMAGE FILTERS AND HISTOGRAM ENHANCEMENT
# ==============================================================================

# --- Q1: Minimum Filter ---
import cv2
import numpy as np
import matplotlib.pyplot as plt

# Modifiable Parameters
image_path = "images.jpeg"   # Path to input image
kernel_size = (3, 3)         # Size of structuring element for erosion (minimum filter)

img = cv2.imread(image_path)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

kernel = np.ones(kernel_size, np.uint8)
minimum = cv2.erode(gray, kernel)

plt.figure(figsize=(10, 5))
plt.subplot(1, 2, 1)
plt.imshow(gray, cmap="gray")
plt.title("Original")
plt.axis("off")

plt.subplot(1, 2, 2)
plt.imshow(minimum, cmap="gray")
plt.title(f"Minimum Filter ({kernel_size[0]}x{kernel_size[1]})")
plt.axis("off")
plt.show()


# --- Q2: Average (Mean) Filter ---
import cv2
import matplotlib.pyplot as plt

# Modifiable Parameters
image_path = "images1.jpeg"  # Path to input image
kernel_3 = (3, 3)            # Small mean filter kernel
kernel_5 = (5, 5)            # Medium mean filter kernel
kernel_7 = (7, 7)            # Large mean filter kernel

img = cv2.imread(image_path)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

mean3 = cv2.blur(gray, kernel_3)
mean5 = cv2.blur(gray, kernel_5)
mean7 = cv2.blur(gray, kernel_7)

plt.figure(figsize=(10, 8))
plt.subplot(2, 2, 1)
plt.imshow(gray, cmap="gray")
plt.title("Original")
plt.axis("off")

plt.subplot(2, 2, 2)
plt.imshow(mean3, cmap="gray")
plt.title(f"{kernel_3[0]}x{kernel_3[1]} Mean Filter")
plt.axis("off")

plt.subplot(2, 2, 3)
plt.imshow(mean5, cmap="gray")
plt.title(f"{kernel_5[0]}x{kernel_5[1]} Mean Filter")
plt.axis("off")

plt.subplot(2, 2, 4)
plt.imshow(mean7, cmap="gray")
plt.title(f"{kernel_7[0]}x{kernel_7[1]} Mean Filter")
plt.axis("off")

plt.show()


# --- Q3: Contrast Stretching ---
import cv2
import matplotlib.pyplot as plt

# Modifiable Parameters
image_path = "images2.jpeg"  # Path to input image
norm_min = 0                 # Minimum target intensity
norm_max = 255               # Maximum target intensity
bins = 256                   # Number of histogram bins
hist_range = (0, 256)        # Range of pixel values

img = cv2.imread(image_path)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

stretched = cv2.normalize(gray, None, norm_min, norm_max, cv2.NORM_MINMAX)

plt.figure(figsize=(10, 8))
plt.subplot(2, 2, 1)
plt.imshow(gray, cmap="gray")
plt.title("Original")
plt.axis("off")

plt.subplot(2, 2, 2)
plt.imshow(stretched, cmap="gray")
plt.title("Contrast Stretching")
plt.axis("off")

plt.subplot(2, 2, 3)
plt.hist(gray.ravel(), bins=bins, range=hist_range)
plt.title("Original Histogram")

plt.subplot(2, 2, 4)
plt.hist(stretched.ravel(), bins=bins, range=hist_range)
plt.title("Stretched Histogram")

plt.show()


# --- Q4: Histogram Equalization ---
import cv2
import matplotlib.pyplot as plt

# Modifiable Parameters
image_path = "images4.jpeg"  # Path to input image
bins = 256                   # Number of histogram bins
hist_range = (0, 256)        # Range of pixel values

img = cv2.imread(image_path)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

equalized = cv2.equalizeHist(gray)

plt.figure(figsize=(10, 8))
plt.subplot(2, 2, 1)
plt.imshow(gray, cmap="gray")
plt.title("Original")
plt.axis("off")

plt.subplot(2, 2, 2)
plt.imshow(equalized, cmap="gray")
plt.title("Histogram Equalization")
plt.axis("off")

plt.subplot(2, 2, 3)
plt.hist(gray.ravel(), bins=bins, range=hist_range)
plt.title("Original Histogram")

plt.subplot(2, 2, 4)
plt.hist(equalized.ravel(), bins=bins, range=hist_range)
plt.title("Equalized Histogram")

plt.show()


# --- Q5: Comparative Analysis ---
import cv2
import numpy as np
import matplotlib.pyplot as plt

# Modifiable Parameters
image_path = "images4.jpeg"  # Path to input image
filter_size = (3, 3)         # Kernel dimensions for minimum and average filters
norm_min = 0                 # Min intensity for contrast stretching
norm_max = 255               # Max intensity for contrast stretching

img = cv2.imread(image_path)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

minimum = cv2.erode(gray, np.ones(filter_size, np.uint8))
average = cv2.blur(gray, filter_size)
stretch = cv2.normalize(gray, None, norm_min, norm_max, cv2.NORM_MINMAX)
equalized = cv2.equalizeHist(gray)

plt.figure(figsize=(15, 8))
plt.subplot(2, 3, 1)
plt.imshow(gray, cmap="gray")
plt.title("Original")
plt.axis("off")

plt.subplot(2, 3, 2)
plt.imshow(minimum, cmap="gray")
plt.title("Minimum Filter")
plt.axis("off")

plt.subplot(2, 3, 3)
plt.imshow(average, cmap="gray")
plt.title("Average Filter")
plt.axis("off")

plt.subplot(2, 3, 4)
plt.imshow(stretch, cmap="gray")
plt.title("Contrast Stretching")
plt.axis("off")

plt.subplot(2, 3, 5)
plt.imshow(equalized, cmap="gray")
plt.title("Histogram Equalization")
plt.axis("off")

plt.show()


# ==============================================================================
# EXPERIMENT 2: NOISE FILTERING AND EDGE DETECTION
# ==============================================================================

# --- Q1: Median Filter ---
import os
import cv2
import numpy as np
import requests

# Modifiable Parameters
image_url = "https://images.unsplash.com/photo-1543466835-00a7907e9de1?w=800&q=80"
image_name = "dog_test_image.jpg"
target_height = 400            # Standardized display height
noise_prob = 0.05              # Salt and pepper noise ratio
kernel_sizes = [3, 5, 7, 9]    # Median filter sizes to compare

downloads_path = os.path.expanduser("~/Downloads")
image_path = os.path.join(downloads_path, image_name)

if not os.path.exists(image_path):
    response = requests.get(image_url, headers={'User-Agent': 'Mozilla/5.0'})
    with open(image_path, "wb") as f:
        f.write(response.content)

gray = cv2.imread(image_path, cv2.IMREAD_GRAYSCALE)
aspect_ratio = target_height / gray.shape[0]
target_width = int(gray.shape[1] * aspect_ratio)
gray = cv2.resize(gray, (target_width, target_height))

def add_salt_and_pepper_noise(image, prob):
    noisy = image.copy()
    num_salt = np.ceil(prob * image.size * 0.5)
    coords = [np.random.randint(0, i - 1, int(num_salt)) for i in image.shape]
    noisy[tuple(coords)] = 255
    num_pepper = np.ceil(prob * image.size * 0.5)
    coords = [np.random.randint(0, i - 1, int(num_pepper)) for i in image.shape]
    noisy[tuple(coords)] = 0
    return noisy

noisy_img = add_salt_and_pepper_noise(gray, prob=noise_prob)
filtered_images = [noisy_img]

for k in kernel_sizes:
    filtered = cv2.medianBlur(noisy_img, k)
    filtered_images.append(filtered)

labeled_images = []
labels = ["Noisy"] + [f"{k}x{k}" for k in kernel_sizes]
for img, label in zip(filtered_images, labels):
    img_bgr = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
    cv2.putText(img_bgr, label, (15, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 0, 255), 2, cv2.LINE_AA)
    labeled_images.append(img_bgr)

combined = np.hstack(labeled_images)
cv2.imshow("Q1: Median Filter Comparison", combined)
cv2.waitKey(0)
cv2.destroyAllWindows()


# --- Q2: Summed Area Table (SAT) Box Filter ---
import os
import cv2
import numpy as np
import requests

# Modifiable Parameters
image_url = "https://images.unsplash.com/photo-1543466835-00a7907e9de1?w=800&q=80"
image_name = "dog_test_image.jpg"
target_height = 400            # Standardized display height
kernel_sizes = [3, 5, 7]       # Kernel sizes for box filter

downloads_path = os.path.expanduser("~/Downloads")
image_path = os.path.join(downloads_path, image_name)

if not os.path.exists(image_path):
    response = requests.get(image_url, headers={'User-Agent': 'Mozilla/5.0'})
    with open(image_path, "wb") as f:
        f.write(response.content)

gray = cv2.imread(image_path, cv2.IMREAD_GRAYSCALE)
aspect_ratio = target_height / gray.shape[0]
target_width = int(gray.shape[1] * aspect_ratio)
gray = cv2.resize(gray, (target_width, target_height))

def box_filter_with_sat(image, kernel_size):
    integral_img = cv2.integral(image)
    h, w = image.shape
    pad = kernel_size // 2
    output = np.zeros_like(image, dtype=np.float32)
    
    for y in range(h):
        for x in range(w):
            x1 = max(0, x - pad)
            y1 = max(0, y - pad)
            x2 = min(w, x + pad + 1)
            y2 = min(h, y + pad + 1)
            
            area_factor = (x2 - x1) * (y2 - y1)
            sum_val = (integral_img[y2, x2] - integral_img[y1, x2] - integral_img[y2, x1] + integral_img[y1, x1])
            output[y, x] = sum_val / area_factor
            
    return np.uint8(np.clip(output, 0, 255))

box_outputs = [gray]
for k in kernel_sizes:
    box_outputs.append(box_filter_with_sat(gray, k))

labeled_images = []
labels = ["Original"] + [f"Box {k}x{k}" for k in kernel_sizes]
for img, label in zip(box_outputs, labels):
    img_bgr = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
    cv2.putText(img_bgr, label, (15, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 0, 255), 2, cv2.LINE_AA)
    labeled_images.append(img_bgr)

combined = np.hstack(labeled_images)
cv2.imshow("Q2: SAT Box Filter Comparison", combined)
cv2.waitKey(0)
cv2.destroyAllWindows()


# --- Q3: Sobel Edge Detection ---
import os
import cv2
import numpy as np
import requests

# Modifiable Parameters
image_url = "https://images.unsplash.com/photo-1543466835-00a7907e9de1?w=800&q=80"
image_name = "dog_test_image.jpg"
target_height = 400            # Standardized display height
sobel_ksize = 3                # Kernel size for Sobel filter
weight_x = 0.5                 # Weight for horizontal gradient
weight_y = 0.5                 # Weight for vertical gradient

downloads_path = os.path.expanduser("~/Downloads")
image_path = os.path.join(downloads_path, image_name)

if not os.path.exists(image_path):
    response = requests.get(image_url, headers={'User-Agent': 'Mozilla/5.0'})
    with open(image_path, "wb") as f:
        f.write(response.content)

gray = cv2.imread(image_path, cv2.IMREAD_GRAYSCALE)
aspect_ratio = target_height / gray.shape[0]
target_width = int(gray.shape[1] * aspect_ratio)
gray = cv2.resize(gray, (target_width, target_height))

sobel_x = cv2.Sobel(gray, cv2.CV_64F, 1, 0, ksize=sobel_ksize)
sobel_y = cv2.Sobel(gray, cv2.CV_64F, 0, 1, ksize=sobel_ksize)

abs_sobel_x = cv2.convertScaleAbs(sobel_x)
abs_sobel_y = cv2.convertScaleAbs(sobel_y)
sobel_combined = cv2.addWeighted(abs_sobel_x, weight_x, abs_sobel_y, weight_y, 0)

sobel_outputs = [gray, abs_sobel_x, abs_sobel_y, sobel_combined]
labels = ["Original", "Sobel-X", "Sobel-Y", "Combined"]
labeled_images = []

for img, label in zip(sobel_outputs, labels):
    img_bgr = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
    cv2.putText(img_bgr, label, (15, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 0, 255), 2, cv2.LINE_AA)
    labeled_images.append(img_bgr)

combined = np.hstack(labeled_images)
cv2.imshow("Q3: Sobel Edge Detection", combined)
cv2.waitKey(0)
cv2.destroyAllWindows()


# --- Q4: Canny Edge Detection ---
import os
import cv2
import numpy as np
import requests

# Modifiable Parameters
image_url = "https://images.unsplash.com/photo-1543466835-00a7907e9de1?w=800&q=80"
image_name = "dog_test_image.jpg"
target_height = 400            # Standardized display height
threshold_pairs = [(50, 150), (100, 200), (150, 250)]  # (Low, High) threshold pairs

downloads_path = os.path.expanduser("~/Downloads")
image_path = os.path.join(downloads_path, image_name)

if not os.path.exists(image_path):
    response = requests.get(image_url, headers={'User-Agent': 'Mozilla/5.0'})
    with open(image_path, "wb") as f:
        f.write(response.content)

gray = cv2.imread(image_path, cv2.IMREAD_GRAYSCALE)
aspect_ratio = target_height / gray.shape[0]
target_width = int(gray.shape[1] * aspect_ratio)
gray = cv2.resize(gray, (target_width, target_height))

canny_outputs = [gray]
labels = ["Original"] + [f"Canny {low}-{high}" for low, high in threshold_pairs]

for low, high in threshold_pairs:
    canny_outputs.append(cv2.Canny(gray, low, high))

labeled_images = []
for img, label in zip(canny_outputs, labels):
    img_bgr = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
    cv2.putText(img_bgr, label, (15, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 0, 255), 2, cv2.LINE_AA)
    labeled_images.append(img_bgr)

combined = np.hstack(labeled_images)
cv2.imshow("Q4: Canny Edge Detection", combined)
cv2.waitKey(0)
cv2.destroyAllWindows()


# ==============================================================================
# EXPERIMENT 3: MORPHOLOGICAL OPERATIONS
# ==============================================================================

# --- Q1: Morphological Erosion ---
import cv2
import numpy as np
import matplotlib.pyplot as plt

# Modifiable Parameters
canvas_size = (200, 200)       # Image resolution
rect_start = (60, 60)          # Rectangle start coordinate
rect_end = (150, 150)          # Rectangle end coordinate
noise_threshold = 0.999        # Noise density control
kernel_size = (3, 3)          # Structuring element size

img = np.zeros(canvas_size, np.uint8)
cv2.rectangle(img, rect_start, rect_end, 255, -1)
noise = np.random.rand(*canvas_size) > noise_threshold
img[noise] = 255

kernel = np.ones(kernel_size, np.uint8)
eroded = cv2.erode(img, kernel)

plt.figure(figsize=(8, 4))
plt.subplot(1, 2, 1)
plt.imshow(img, cmap="gray")
plt.title("Original")

plt.subplot(1, 2, 2)
plt.imshow(eroded, cmap="gray")
plt.title(f"Eroded {kernel_size[0]}x{kernel_size[1]}")

plt.show()


# --- Q2: Morphological Dilation ---
import cv2
import numpy as np
import matplotlib.pyplot as plt

# Modifiable Parameters
canvas_size = (200, 200)       # Image resolution
circle_center = (100, 100)     # Circle center coordinate
circle_radius = 50             # Circle radius
circle_thickness = 6           # Line thickness of circle
gap_start = (95, 45)           # Rectangle gap cut start
gap_end = (105, 55)            # Rectangle gap cut end
kernel_size = (3, 3)          # Structuring element size
iterations = 1                 # Number of dilation passes

img = np.zeros(canvas_size, np.uint8)
cv2.circle(img, circle_center, circle_radius, 255, circle_thickness)
cv2.rectangle(img, gap_start, gap_end, 0, -1)

kernel = np.ones(kernel_size, np.uint8)
dilated = cv2.dilate(img, kernel, iterations=iterations)

plt.figure(figsize=(8, 4))
plt.subplot(1, 2, 1)
plt.imshow(img, cmap="gray")
plt.title("Broken 'O'")

plt.subplot(1, 2, 2)
plt.imshow(dilated, cmap="gray")
plt.title(f"Dilated {kernel_size[0]}x{kernel_size[1]}")

plt.show()


# --- Q3: Morphological Opening vs. Closing ---
import cv2
import numpy as np
import matplotlib.pyplot as plt

# Modifiable Parameters
canvas_size = (150, 150)       # Image resolution
rect_start = (40, 40)          # Rectangle start coordinate
rect_end = (110, 110)          # Rectangle end coordinate
noise_threshold = 0.995        # Noise density threshold
kernel_size = (3, 3)          # Structuring element size

A = np.zeros(canvas_size, np.uint8)
cv2.rectangle(A, rect_start, rect_end, 255, -1)
salt = np.random.rand(*canvas_size) > noise_threshold
A[salt] = 255

B = np.zeros(canvas_size, np.uint8)
cv2.rectangle(B, rect_start, rect_end, 255, -1)
pepper = np.random.rand(*canvas_size) > noise_threshold
B[pepper & (B == 255)] = 0

kernel = np.ones(kernel_size, np.uint8)
opened = cv2.morphologyEx(A, cv2.MORPH_OPEN, kernel)
closed = cv2.morphologyEx(B, cv2.MORPH_CLOSE, kernel)

plt.figure(figsize=(10, 8))
plt.subplot(2, 2, 1)
plt.imshow(A, cmap="gray")
plt.title("A: Salt Noise")

plt.subplot(2, 2, 2)
plt.imshow(opened, cmap="gray")
plt.title("A: Opened")

plt.subplot(2, 2, 3)
plt.imshow(B, cmap="gray")
plt.title("B: Pepper Noise")

plt.subplot(2, 2, 4)
plt.imshow(closed, cmap="gray")
plt.title("B: Closed")

plt.show()


# --- Q4: Distance Transform Properties ---
import cv2
import numpy as np
import matplotlib.pyplot as plt

# Modifiable Parameters
canvas_size = (300, 300)       # Image resolution
rect_start = (100, 100)        # Square start coordinate
rect_end = (200, 200)          # Square end coordinate
mask_size = 5                  # Distance transform mask size
colormap = "jet"               # Visualization colormap

sq = np.zeros(canvas_size, np.uint8)
cv2.rectangle(sq, rect_start, rect_end, 255, -1)

dist_l2 = cv2.distanceTransform(sq, cv2.DIST_L2, mask_size)
dist_l1 = cv2.distanceTransform(sq, cv2.DIST_L1, mask_size)

plt.figure(figsize=(10, 4))
plt.subplot(1, 2, 1)
plt.imshow(dist_l2, cmap=colormap)
plt.title("Distance Transform (L2)")

plt.subplot(1, 2, 2)
plt.imshow(dist_l1, cmap=colormap)
plt.title("Distance Transform (L1)")

plt.show()

print("Max L2 value:", dist_l2.max())
print("Max L1 value:", dist_l1.max())


# ==============================================================================
# EXPERIMENT 4: ENHANCEMENT, FREQUENCY SPECTRUM, AND CANNY EDGES
# ==============================================================================

# --- Task 1: Histogram Equalization on MRI Image ---
import cv2
import matplotlib.pyplot as plt

# Modifiable Parameters
image_path = "mri.jpg"        # Path to MRI scan

img = cv2.imread(image_path, 0)
equalized = cv2.equalizeHist(img)

plt.subplot(1, 2, 1)
plt.imshow(img, cmap='gray')
plt.title("Original Image")

plt.subplot(1, 2, 2)
plt.imshow(equalized, cmap='gray')
plt.title("Histogram Equalized Image")

plt.show()


# --- Task 2: Frequency Spectrum using 2D FFT ---
import cv2
import numpy as np
import matplotlib.pyplot as plt

# Modifiable Parameters
image_path = "lena.jpg"       # Path to input image
scale_factor = 20             # Spectrum scaling factor

img = cv2.imread(image_path, 0)
f = np.fft.fft2(img)
fshift = np.fft.fftshift(f)
magnitude = scale_factor * np.log(np.abs(fshift) + 1)

plt.subplot(1, 2, 1)
plt.imshow(img, cmap='gray')
plt.title("Original Image")

plt.subplot(1, 2, 2)
plt.imshow(magnitude, cmap='gray')
plt.title("FFT Magnitude Spectrum")

plt.show()


# --- Task 3: Edge Detection using Canny (Grayscale) ---
import cv2
import matplotlib.pyplot as plt

# Modifiable Parameters
image_path = "lena.jpg"       # Path to input image
canny_low = 100               # Lower threshold
canny_high = 200              # Upper threshold

img = cv2.imread(image_path, 0)
edges = cv2.Canny(img, canny_low, canny_high)

plt.subplot(1, 2, 1)
plt.imshow(img, cmap='gray')
plt.title("Original Image")

plt.subplot(1, 2, 2)
plt.imshow(edges, cmap='gray')
plt.title("Edge Image")

plt.show()


# --- Task 4: Edge Detection on a Color Image ---
import cv2
import matplotlib.pyplot as plt

# Modifiable Parameters
image_path = "workspace.jpg"  # Path to color input image
canny_low = 100               # Lower threshold
canny_high = 200              # Upper threshold

img = cv2.imread(image_path)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
edges = cv2.Canny(gray, canny_low, canny_high)

plt.subplot(1, 2, 1)
plt.imshow(cv2.cvtColor(img, cv2.COLOR_BGR2RGB))
plt.title("Original Image")

plt.subplot(1, 2, 2)
plt.imshow(edges, cmap='gray')
plt.title("Detected Edges")

plt.show()


# ==============================================================================
# EXPERIMENT 5: COLOR IMAGE SEGMENTATION
# ==============================================================================

# --- Task 1: Image Segmentation Using K-Means Clustering ---
import cv2
import numpy as np
import matplotlib.pyplot as plt

# Modifiable Parameters
image_path = "flower.jpeg"     # Input image path
k_values = [2, 3, 5, 8]        # Number of clusters to evaluate
max_iter = 100                 # Maximum iterations for K-Means
epsilon = 0.2                  # Convergence epsilon
attempts = 10                  # K-Means execution attempts

img = cv2.imread(image_path)
img_rgb = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
pixels = np.float32(img.reshape((-1, 3)))

segmented_images = []
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, max_iter, epsilon)

for K in k_values:
    _, labels, centers = cv2.kmeans(pixels, K, None, criteria, attempts, cv2.KMEANS_RANDOM_CENTERS)
    centers = np.uint8(centers)
    segmented = centers[labels.flatten()].reshape(img.shape)
    segmented_images.append(cv2.cvtColor(segmented, cv2.COLOR_BGR2RGB))

plt.figure(figsize=(12, 8))
plt.subplot(2, 3, 1)
plt.imshow(img_rgb)
plt.title("Original Image")
plt.axis("off")

for i, K in enumerate(k_values):
    plt.subplot(2, 3, i + 2)
    plt.imshow(segmented_images[i])
    plt.title(f"K = {K}")
    plt.axis("off")

plt.tight_layout()
plt.show()


# --- Task 2: Image Segmentation Using Mean Shift ---
import cv2
import matplotlib.pyplot as plt

# Modifiable Parameters
image_path = "flower.jpeg"     # Path to input image
sp_sr_pairs = [(10, 20), (20, 30), (30, 40), (40, 60)]  # (spatial_radius, color_radius) pairs

img = cv2.imread(image_path)
img_rgb = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)

plt.figure(figsize=(12, 8))
plt.subplot(2, 3, 1)
plt.imshow(img_rgb)
plt.title("Original")
plt.axis("off")

for i, (sp, sr) in enumerate(sp_sr_pairs):
    result = cv2.pyrMeanShiftFiltering(img, sp, sr)
    result = cv2.cvtColor(result, cv2.COLOR_BGR2RGB)
    plt.subplot(2, 3, i + 2)
    plt.imshow(result)
    plt.title(f"sp={sp}, sr={sr}")
    plt.axis("off")

plt.tight_layout()
plt.show()


# --- Task 3: Image Segmentation Using Graph-Based Spectral Clustering ---
import cv2
import numpy as np
import matplotlib.pyplot as plt
from sklearn.cluster import SpectralClustering

# Modifiable Parameters
image_path = "flower.jpeg"     # Path to input image
resize_dim = (50, 50)          # Downsampled resolution
n_clusters = 6                 # Number of clusters

ogimg = cv2.imread(image_path)
img = cv2.cvtColor(ogimg, cv2.COLOR_BGR2RGB)
img = cv2.resize(img, resize_dim)
h, w, c = img.shape

data = []
for y in range(h):
    for x in range(w):
        r, g, b = img[y, x]
        data.append([float(r)/255.0, float(g)/255.0, float(b)/255.0, float(x)/w, float(y)/h])

data = np.array(data, dtype=np.float32)

model = SpectralClustering(n_clusters=n_clusters, affinity="nearest_neighbors", n_neighbors=10, random_state=42)
labels = model.fit_predict(data)

segmented = np.zeros_like(img)
for i in range(n_clusters):
    pix = img.reshape(-1, 3)[labels == i]
    if len(pix) > 0:
        segmented.reshape(-1, 3)[labels == i] = np.mean(pix, axis=0).astype(np.uint8)

plt.figure(figsize=(10, 5))
plt.subplot(1, 2, 1)
plt.imshow(cv2.cvtColor(ogimg, cv2.COLOR_BGR2RGB))
plt.title("Original Image")

plt.subplot(1, 2, 2)
plt.imshow(segmented)
plt.title(f"Spectral Clustering (K={n_clusters})")

plt.show()


# --- Task 4: Comparative Analysis of Image Segmentation Techniques ---
import cv2
import numpy as np
import matplotlib.pyplot as plt
from sklearn.cluster import SpectralClustering
import time

# Modifiable Parameters
image_path = "flower.jpeg"     # Path to input image
k_clusters = 5                 # Cluster count for algorithms
sp_radius = 20                 # Mean Shift spatial radius
sr_radius = 30                 # Mean Shift color radius
spectral_size = (60, 60)       # Image resize dimensions for Spectral Clustering

img = cv2.imread(image_path)
original = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)

# 1. K-Means
start = time.time()
pixels = np.float32(img.reshape((-1, 3)))
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 0.2)
_, labels, centers = cv2.kmeans(pixels, k_clusters, None, criteria, 10, cv2.KMEANS_RANDOM_CENTERS)
kmeans_img = np.uint8(centers)[labels.flatten()].reshape(img.shape)
kmeans_time = time.time() - start

# 2. Mean Shift
start = time.time()
meanshift_img = cv2.pyrMeanShiftFiltering(img, sp_radius, sr_radius)
meanshift_time = time.time() - start

# 3. Spectral Clustering
start = time.time()
small = cv2.resize(original, spectral_size)
h, w, c = small.shape
data = []
for y in range(h):
    for x in range(w):
        r, g, b = small[y, x]
        data.append([r / 255.0, g / 255.0, b / 255.0, x / w, y / h])
data = np.array(data)

model = SpectralClustering(n_clusters=k_clusters, affinity="nearest_neighbors", n_neighbors=10, random_state=42)
labels = model.fit_predict(data)
graph_img = np.zeros_like(small)
for i in range(k_clusters):
    pix = small.reshape(-1, 3)[labels == i]
    if len(pix) > 0:
        graph_img.reshape(-1, 3)[labels == i] = np.mean(pix, axis=0).astype(np.uint8)
graph_time = time.time() - start

plt.figure(figsize=(12, 8))
plt.subplot(2, 2, 1); plt.imshow(original); plt.title("Original"); plt.axis("off")
plt.subplot(2, 2, 2); plt.imshow(cv2.cvtColor(kmeans_img, cv2.COLOR_BGR2RGB)); plt.title("K-Means"); plt.axis("off")
plt.subplot(2, 2, 3); plt.imshow(cv2.cvtColor(meanshift_img, cv2.COLOR_BGR2RGB)); plt.title("Mean Shift"); plt.axis("off")
plt.subplot(2, 2, 4); plt.imshow(graph_img); plt.title("Graph Based"); plt.axis("off")
plt.tight_layout()
plt.show()

print("Execution Time:")
print("K-Means:", round(kmeans_time, 4), "seconds")
print("Mean Shift:", round(meanshift_time, 4), "seconds")
print("Graph Based:", round(graph_time, 4), "seconds")


# ==============================================================================
# EXPERIMENT 6: STRUCTURAL BOUNDARIES AND INTEREST POINTS
# ==============================================================================

# --- Task 1: Hough Transform Line Detection ---
import cv2
import numpy as np
import matplotlib.pyplot as plt

# Modifiable Parameters
image_path = 'robot_corridor.jpg'  # Input image path
threshold_val = 150                # Hough accumulator threshold
canny_low = 50                     # Canny low threshold
canny_high = 150                   # Canny high threshold

def hough_line_detection(image_path, threshold_val=150):
    img = cv2.imread(image_path)
    if img is None:
        return
    img_rgb = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    blurred = cv2.GaussianBlur(gray, (5, 5), 0)
    edges = cv2.Canny(blurred, canny_low, canny_high, apertureSize=3)
    
    lines = cv2.HoughLines(edges, rho=1, theta=np.pi/180, threshold=threshold_val)
    output_img = img_rgb.copy()
    
    if lines is not None:
        for line in lines:
            rho, theta = line[0]
            a, b = np.cos(theta), np.sin(theta)
            x0, y0 = a * rho, b * rho
            x1, y1 = int(x0 + 1000 * (-b)), int(y0 + 1000 * (a))
            x2, y2 = int(x0 - 1000 * (-b)), int(y0 - 1000 * (a))
            cv2.line(output_img, (x1, y1), (x2, y2), (255, 0, 0), 2)
            
    fig, axes = plt.subplots(1, 3, figsize=(18, 6))
    axes[0].imshow(gray, cmap='gray'); axes[0].set_title('Grayscale Image'); axes[0].axis('off')
    axes[1].imshow(edges, cmap='gray'); axes[1].set_title('Canny Edge Map'); axes[1].axis('off')
    axes[2].imshow(output_img); axes[2].set_title(f'Hough Lines (Threshold = {threshold_val})'); axes[2].axis('off')
    plt.tight_layout()
    plt.show()

# Execution
hough_line_detection(image_path, threshold_val=threshold_val)


# --- Task 2: Harris Corner Detection ---
import cv2
import numpy as np
import matplotlib.pyplot as plt

# Modifiable Parameters
image_path = 'bus.jpg'             # Input image path
block_size = 2                     # Neighborhood size
ksize = 3                          # Sobel filter aperture
k = 0.04                           # Harris free parameter
threshold_ratio = 0.01             # Response threshold ratio

def harris_corner_detection(image_path, block_size=2, ksize=3, k=0.04, threshold_ratio=0.01):
    img = cv2.imread(image_path)
    if img is None:
        return
    img_rgb = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    
    dst = cv2.cornerHarris(np.float32(gray), blockSize=block_size, ksize=ksize, k=k)
    dst_dilated = cv2.dilate(dst, None)
    
    output_img = img_rgb.copy()
    output_img[dst_dilated > threshold_ratio * dst_dilated.max()] = [255, 0, 0]
    
    fig, axes = plt.subplots(1, 2, figsize=(12, 6))
    axes[0].imshow(gray, cmap='gray')
    axes[0].set_title('Original Grayscale')
    axes[0].axis('off')
    
    axes[1].imshow(output_img)
    axes[1].set_title(f'Harris Corners (blockSize={block_size}, ksize={ksize}, k={k})')
    axes[1].axis('off')
    
    plt.tight_layout()
    plt.show()

# Execution
harris_corner_detection(image_path, block_size=block_size, ksize=ksize, k=k, threshold_ratio=threshold_ratio)


# ==============================================================================
# EXPERIMENT 7: FEATURE DETECTION, MATCHING, AND DEEP LEARNING DETECTION
# ==============================================================================

# --- Task 1: SIFT Feature Detection ---
import cv2

# Modifiable Parameters
image_path = "input.webp"     # Input image path

image = cv2.imread(image_path)
if image is not None:
    gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
    sift = cv2.SIFT_create()
    keypoints, descriptors = sift.detectAndCompute(gray, None)

    result = cv2.drawKeypoints(image, keypoints, None, flags=cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)

    print("Total keypoints detected:", len(keypoints))
    cv2.imshow("SIFT Keypoints", result)
    cv2.waitKey(0)
    cv2.destroyAllWindows()


# --- Task 2: Object Matching using SIFT ---
import cv2

# Modifiable Parameters
image1_path = "obj1.webp"      # Image 1 path
image2_path = "obj2.webp"      # Image 2 path
ratio_thresh = 0.75            # Lowe's ratio test threshold
match_threshold = 20           # Match threshold to consider objects identical

image1 = cv2.imread(image1_path)
image2 = cv2.imread(image2_path)

if image1 is not None and image2 is not None:
    gray1 = cv2.cvtColor(image1, cv2.COLOR_BGR2GRAY)
    gray2 = cv2.cvtColor(image2, cv2.COLOR_BGR2GRAY)

    sift = cv2.SIFT_create()
    keypoints1, descriptors1 = sift.detectAndCompute(gray1, None)
    keypoints2, descriptors2 = sift.detectAndCompute(gray2, None)

    bf = cv2.BFMatcher()
    matches = bf.knnMatch(descriptors1, descriptors2, k=2)

    good_matches = []
    for m, n in matches:
        if m.distance < ratio_thresh * n.distance:
            good_matches.append(m)

    result = cv2.drawMatches(image1, keypoints1, image2, keypoints2, good_matches, None, flags=cv2.DrawMatchesFlags_NOT_DRAW_SINGLE_POINTS)

    print("Number of good matches:", len(good_matches))
    if len(good_matches) >= match_threshold:
        print("The images likely contain the same object.")
    else:
        print("The images probably contain different objects.")

    cv2.imshow("SIFT Object Matching", result)
    cv2.waitKey(0)
    cv2.destroyAllWindows()


# --- Task 3: Object Detection Using Faster R-CNN ---
import cv2
import os
import urllib.request

# Modifiable Parameters
image_path = "input.jpg"       # Input image path
conf_threshold = 0.5           # Minimum confidence score

model_url = "https://github.com/opencv/opencv_extra/raw/4.x/testdata/dnn/faster_rcnn_inception_v2_coco_2018_01_28.pb"
config_url = "https://raw.githubusercontent.com/opencv/opencv_extra/4.x/testdata/dnn/faster_rcnn_inception_v2_coco_2018_01_28.pbtxt"
model_file = "faster_rcnn.pb"
config_file = "faster_rcnn.pbtxt"

if not os.path.exists(model_file):
    urllib.request.urlretrieve(model_url, model_file)
if not os.path.exists(config_file):
    urllib.request.urlretrieve(config_url, config_file)

image = cv2.imread(image_path)
if image is not None:
    net = cv2.dnn.readNetFromTensorflow(model_file, config_file)
    blob = cv2.dnn.blobFromImage(image, size=(800, 800), swapRB=True, crop=False)
    net.setInput(blob)
    detections = net.forward()

    classes = [
        "background", "person", "bicycle", "car", "motorcycle", "airplane", "bus", "train", "truck", "boat", 
        "traffic light", "fire hydrant", "stop sign", "parking meter", "bench", "bird", "cat", "dog", "horse", 
        "sheep", "cow", "elephant", "bear", "zebra", "giraffe", "backpack", "umbrella", "handbag", "tie", 
        "suitcase", "frisbee", "skis", "snowboard", "sports ball", "kite", "baseball bat", "baseball glove", 
        "skateboard", "surfboard", "tennis racket", "bottle", "wine glass", "cup", "fork", "knife", "spoon", 
        "bowl", "banana", "apple", "sandwich", "orange", "broccoli", "carrot", "hot dog", "pizza", "donut", 
        "cake", "chair", "couch", "potted plant", "bed", "dining table", "toilet", "TV", "laptop", "mouse", 
        "remote", "keyboard", "cell phone", "microwave", "oven", "toaster", "sink", "refrigerator", "book", 
        "clock", "vase", "scissors", "teddy bear", "hair drier", "toothbrush"
    ]

    h, w = image.shape[:2]
    for i in range(detections.shape[2]):
        confidence = detections[0, 0, i, 2]
        if confidence < conf_threshold:
            continue
        class_id = int(detections[0, 0, i, 1])
        box = detections[0, 0, i, 3:7] * [w, h, w, h]
        x1, y1, x2, y2 = box.astype(int)
        
        cv2.rectangle(image, (x1, y1), (x2, y2), (0, 255, 0), 2)
        label = f"{classes[class_id]}: {confidence:.2f}"
        cv2.putText(image, label, (x1, y1 - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2)
        print(label)

    cv2.imshow("Faster R-CNN Detection", image)
    cv2.waitKey(0)
    cv2.destroyAllWindows()


# ==============================================================================
# EXPERIMENT 8: OBJECT DETECTION WITH OPENCV AND ULTRALYTICS YOLO
# ==============================================================================

# --- Task 1: Faster R-CNN using Torchvision ---
import cv2
import torch
import torchvision
from torchvision.transforms import functional as F

# Modifiable Parameters
image_path = "bus.jpg"         # Input image path
conf_threshold = 0.5           # Min score cutoff

weights = torchvision.models.detection.FasterRCNN_ResNet50_FPN_Weights.DEFAULT
model = torchvision.models.detection.fasterrcnn_resnet50_fpn(weights=weights).eval()
categories = weights.meta["categories"]

image = cv2.imread(image_path)
if image is not None:
    image_rgb = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
    img_tensor = F.to_tensor(image_rgb)

    with torch.no_grad():
        predictions = model([img_tensor])[0]

    for box, label, score in zip(predictions['boxes'], predictions['labels'], predictions['scores']):
        if score > conf_threshold:
            x1, y1, x2, y2 = map(int, box.tolist())
            class_name = categories[label]
            cv2.rectangle(image, (x1, y1), (x2, y2), (0, 255, 0), 2)
            text = f"{class_name}: {score * 100:.1f}%"
            cv2.putText(image, text, (x1, max(y1 - 10, 15)), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)

    cv2.imshow("Faster R-CNN (Auto Weights)", image)
    cv2.waitKey(0)
    cv2.destroyAllWindows()


# --- Task 2: YOLO Object Detection ---
import cv2
from ultralytics import YOLO

# Modifiable Parameters
model_path = "yolov8n.pt"      # YOLO model variant
image_path = "bus.jpg"         # Test image path
conf_threshold = 0.5           # Confidence threshold

model = YOLO(model_path)
image = cv2.imread(image_path)

if image is not None:
    results = model(image, conf=conf_threshold)[0]

    for box in results.boxes:
        x1, y1, x2, y2 = map(int, box.xyxy[0])
        confidence = float(box.conf[0])
        class_id = int(box.cls[0])
        label = model.names[class_id]
        
        cv2.rectangle(image, (x1, y1), (x2, y2), (255, 0, 0), 2)
        text = f"{label}: {confidence * 100:.1f}%"
        cv2.putText(image, text, (x1, max(y1 - 10, 15)), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 2)

    cv2.imshow("YOLO Detection (Auto Weights)", image)
    cv2.waitKey(0)
    cv2.destroyAllWindows()


# --- Task 3: Comparative Analysis of Faster R-CNN and YOLO ---
import cv2
import time
import torch
import torchvision
import numpy as np
from torchvision.transforms import functional as F
from ultralytics import YOLO

# Modifiable Parameters
image_path = "bus.jpg"         # Input image path
conf_threshold = 0.5           # Detection threshold

frcnn_weights = torchvision.models.detection.FasterRCNN_ResNet50_FPN_Weights.DEFAULT
frcnn_model = torchvision.models.detection.fasterrcnn_resnet50_fpn(weights=frcnn_weights).eval()
yolo_model = YOLO("yolov8n.pt")

img_frcnn = cv2.imread(image_path)
if img_frcnn is not None:
    img_yolo = img_frcnn.copy()

    # Benchmark Faster R-CNN
    start_time = time.time()
    img_tensor = F.to_tensor(cv2.cvtColor(img_frcnn, cv2.COLOR_BGR2RGB))
    with torch.no_grad():
        frcnn_preds = frcnn_model([img_tensor])[0]
    frcnn_time = time.time() - start_time

    frcnn_count = 0
    for box, score in zip(frcnn_preds['boxes'], frcnn_preds['scores']):
        if score > conf_threshold:
            frcnn_count += 1
            x1, y1, x2, y2 = map(int, box.tolist())
            cv2.rectangle(img_frcnn, (x1, y1), (x2, y2), (0, 255, 0), 2)

    # Benchmark YOLO
    start_time = time.time()
    yolo_results = yolo_model(img_yolo, conf=conf_threshold)[0]
    yolo_time = time.time() - start_time

    yolo_count = len(yolo_results.boxes)
    for box in yolo_results.boxes:
        x1, y1, x2, y2 = map(int, box.xyxy[0])
        cv2.rectangle(img_yolo, (x1, y1), (x2, y2), (255, 0, 0), 2)

    print("=== COMPARATIVE METRICS ===")
    print(f"Faster R-CNN Inference: {frcnn_time:.4f}s | Detections: {frcnn_count}")
    print(f"YOLOv8 Inference       : {yolo_time:.4f}s | Detections: {yolo_count}")

    combined = np.hstack((img_frcnn, img_yolo))
    cv2.imshow("Left: Faster R-CNN | Right: YOLO", combined)
    cv2.waitKey(0)
    cv2.destroyAllWindows()


# --- Task 4: Real-Time Robot Object Detection using YOLO ---
import cv2
import time
from ultralytics import YOLO

# Modifiable Parameters
model_path = "yolov8n.pt"      # YOLO weight file
camera_id = 0                  # Web camera index
frame_width = 640              # Stream frame width
frame_height = 480             # Stream frame height
conf_threshold = 0.5           # Detection threshold

model = YOLO(model_path)
cap = cv2.VideoCapture(camera_id)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, frame_width)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, frame_height)

prev_time = 0

while cap.isOpened():
    ret, frame = cap.read()
    if not ret:
        break
        
    curr_time = time.time()
    fps = 1 / (curr_time - prev_time) if (curr_time - prev_time) > 0 else 0
    prev_time = curr_time
    
    results = model(frame, conf=conf_threshold)[0]
    for box in results.boxes:
        x1, y1, x2, y2 = map(int, box.xyxy[0])
        conf = float(box.conf[0])
        cls_id = int(box.cls[0])
        label = model.names[cls_id]
        
        cv2.rectangle(frame, (x1, y1), (x2, y2), (0, 255, 0), 2)
        cv2.putText(frame, f"{label} {conf:.2f}", (x1, max(y1 - 10, 15)), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)
        
    cv2.putText(frame, f"FPS: {int(fps)}", (20, 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
    cv2.imshow("Robot Real-Time Detection", frame)
    
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break

cap.release()
cv2.destroyAllWindows()


# ==============================================================================
# EXPERIMENT 9: BUNDLE ADJUSTMENT FOR 3D RECONSTRUCTION
# ==============================================================================

import cv2
import numpy as np
import matplotlib.pyplot as plt
from scipy.optimize import least_squares

# Modifiable Parameters
view1_path = "view1.png"                 # First view image path
view2_path = "view2.png"                 # Second view image path
ratio_thresh = 0.75                      # Ratio test threshold
ransac_thresh = 1.0                      # RANSAC distance threshold
max_depth = 50.0                         # Maximum depth threshold
max_iterations = 200                     # Bundle Adjustment maximum evaluations

# Camera Intrinsic Matrix K
K = np.array([[800, 0, 320],
              [0, 800, 240],
              [0, 0, 1]], dtype=np.float64)

# 1. Feature Extraction & Matching
img1 = cv2.imread(view1_path)
img2 = cv2.imread(view2_path)

if img1 is not None and img2 is not None:
    gray1 = cv2.cvtColor(img1, cv2.COLOR_BGR2GRAY)
    gray2 = cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY)

    sift = cv2.SIFT_create()
    kp1, des1 = sift.detectAndCompute(gray1, None)
    kp2, des2 = sift.detectAndCompute(gray2, None)

    bf = cv2.BFMatcher()
    raw_matches = bf.knnMatch(des1, des2, k=2)
    good = [m for m, n in raw_matches if m.distance < ratio_thresh * n.distance]

    pts1 = np.float32([kp1[m.queryIdx].pt for m in good])
    pts2 = np.float32([kp2[m.trainIdx].pt for m in good])
    print("Keypoints:", len(kp1), len(kp2), "Good matches:", len(good))

    # 2. Essential Matrix & Pose Recovery
    E, mask_e = cv2.findEssentialMat(pts1, pts2, K, method=cv2.RANSAC, prob=0.999, threshold=ransac_thresh)
    inliers = mask_e.ravel().astype(bool)
    pts1_in, pts2_in = pts1[inliers], pts2[inliers]
    print("Essential matrix inliers:", inliers.sum(), "/", len(good))

    R_rec, t_rec, _ = cv2.recoverPose(E, pts1_in, pts2_in, K)

    # 3. Triangulation
    P1 = K @ np.hstack([np.eye(3), np.zeros((3, 1))])
    P2 = K @ np.hstack([R_rec, t_rec])

    pts4d = cv2.triangulatePoints(P1, P2, pts1_in.T, pts2_in.T)
    pts3d = (pts4d[:3] / pts4d[3]).T

    depth1 = pts3d[:, 2]
    depth2 = (R_rec @ pts3d.T + t_rec).T[:, 2]
    valid = (depth1 > 0) & (depth1 < max_depth) & (depth2 > 0) & (depth2 < max_depth)

    pts3d, pts1_v, pts2_v = pts3d[valid], pts1_in[valid], pts2_in[valid]
    print("Valid triangulated points:", pts3d.shape[0])

    # 4. Error Calculation
    def reprojection_errors(R1, t1, R2, t2, X, obs1, obs2, K):
        proj1, _ = cv2.projectPoints(X, cv2.Rodrigues(R1)[0], t1, K, None)
        proj2, _ = cv2.projectPoints(X, cv2.Rodrigues(R2)[0], t2, K, None)
        err1 = np.linalg.norm(proj1.reshape(-1, 2) - obs1, axis=1)
        err2 = np.linalg.norm(proj2.reshape(-1, 2) - obs2, axis=1)
        return err1, err2

    R1_init, t1_init = np.eye(3), np.zeros((3, 1))
    e1, e2 = reprojection_errors(R1_init, t1_init, R_rec, t_rec, pts3d, pts1_v, pts2_v, K)
    rmse_before = np.sqrt(np.mean(np.concatenate([e1, e2]) ** 2))
    print("RMSE before bundle adjustment: %.4f px" % rmse_before)

    # 5. Non-Linear Optimization (Bundle Adjustment)
    def residuals(params, n_pts, obs1, obs2, K):
        rvec2 = params[0:3]
        tvec2 = params[3:6]
        X = params[6:].reshape(n_pts, 3)
        proj1, _ = cv2.projectPoints(X, np.zeros(3), np.zeros(3), K, None)
        proj2, _ = cv2.projectPoints(X, rvec2, tvec2, K, None)
        res1 = (proj1.reshape(-1, 2) - obs1).ravel()
        res2 = (proj2.reshape(-1, 2) - obs2).ravel()
        return np.concatenate([res1, res2])

    n_pts = pts3d.shape[0]
    x0 = np.concatenate([cv2.Rodrigues(R_rec)[0].ravel(), t_rec.ravel(), pts3d.ravel()])

    result = least_squares(residuals, x0, method="lm", max_nfev=max_iterations, args=(n_pts, pts1_v, pts2_v, K))

    rvec2_opt = result.x[0:3]
    tvec2_opt = result.x[3:6]
    X_opt = result.x[6:].reshape(n_pts, 3)
    R2_opt = cv2.Rodrigues(rvec2_opt)[0]
    t2_opt = tvec2_opt.reshape(3, 1)

    e1o, e2o = reprojection_errors(R1_init, t1_init, R2_opt, t2_opt, X_opt, pts1_v, pts2_v, K)
    rmse_after = np.sqrt(np.mean(np.concatenate([e1o, e2o]) ** 2))
    print("RMSE after bundle adjustment: %.4f px" % rmse_after)


# ==============================================================================
# EXPERIMENT 10: DISCRETE-TIME KALMAN FILTER
# ==============================================================================

import numpy as np
import matplotlib.pyplot as plt

# Modifiable Parameters
dt = 0.1                       # Time step (seconds)
n_steps = 100                  # Total duration steps
sigma_a = 0.4                  # Process noise standard deviation
sigma_z = 2.0                  # Measurement noise standard deviation
init_pos = 0.0                 # Simulated initial position
init_vel = 3.0                 # Simulated initial velocity
init_vel_cov = 25.0            # Initial velocity error variance

# 1. State Space Matrix Setup
F = np.array([[1, dt], [0, 1]])
Gamma = np.array([[0.5 * dt**2], [dt]])
H = np.array([[1, 0]])

x_true = np.zeros((n_steps, 2))
x_true[0] = [init_pos, init_vel]

for k in range(1, n_steps):
    a_k = np.random.normal(0, sigma_a)
    x_true[k] = F @ x_true[k - 1] + (Gamma * a_k).ravel()

z = x_true[:, 0] + np.random.normal(0, sigma_z, size=n_steps)

# 2. Filter Initial Conditions
Q = (sigma_a ** 2) * (Gamma @ Gamma.T)
R = np.array([[sigma_z ** 2]])

x_est = np.array([z[0], 0.0])
P_est = np.diag([sigma_z**2, init_vel_cov])

x_hat = np.zeros((n_steps, 2))
x_hat[0] = x_est
I2 = np.eye(2)

# 3. Kalman Filter Predict & Update Loop
for k in range(1, n_steps):
    # Predict
    x_pred = F @ x_est
    P_pred = F @ P_est @ F.T + Q
    
    # Update
    y_resid = z[k] - (H @ x_pred)[0]
    S = H @ P_pred @ H.T + R
    K_gain = P_pred @ H.T @ np.linalg.inv(S)
    
    x_est = x_pred + (K_gain.ravel() * y_resid)
    P_est = (I2 - K_gain @ H) @ P_pred
    x_hat[k] = x_est

# Performance Metric Evaluation
pos_rmse_meas = np.sqrt(np.mean((z - x_true[:, 0])**2))
pos_rmse_kf = np.sqrt(np.mean((x_hat[:, 0] - x_true[:, 0])**2))
vel_rmse_kf = np.sqrt(np.mean((x_hat[:, 1] - x_true[:, 1])**2))

print("Position RMSE, raw noisy measurements:", pos_rmse_meas)
print("Position RMSE, Kalman filter estimate:", pos_rmse_kf)
print("Velocity RMSE, Kalman filter estimate:", vel_rmse_kf)

# Visualization
plt.figure(figsize=(12, 8))

plt.subplot(2, 2, 1)
plt.plot(x_true[:, 0], 'k-', label="True position")
plt.plot(z, 'r.', alpha=0.5, label="Noisy measurements")
plt.plot(x_hat[:, 0], 'g-', label="Kalman filter estimate")
plt.title("Position: True vs Measured vs Filtered")
plt.xlabel("Time (s)")
plt.ylabel("Position (m)")
plt.legend()

plt.subplot(2, 2, 2)
plt.plot(x_true[:, 1], 'k-', label="True velocity")
plt.plot(x_hat[:, 1], 'b-', label="Kalman filter estimate")
plt.title("Velocity: True vs Filtered (not directly observed)")
plt.xlabel("Time (s)")
plt.ylabel("Velocity (m/s)")
plt.legend()

plt.subplot(2, 1, 2)
pos_error = x_hat[:, 0] - x_true[:, 0]
plt.plot(pos_error, label="Position error (estimate - true)")
plt.axhline(0, color='k', linestyle='--')
plt.title("Kalman Filter Estimation Error")
plt.xlabel("Time (s)")
plt.ylabel("Position error (m)")
plt.legend()

plt.tight_layout()
plt.show()