import os

import cv2

import numpy as np

import matplotlib.pyplot as plt

from sklearn.cluster import SpectralClustering

import torch

import torchvision

from torchvision.transforms import functional as F

from ultralytics import YOLO



# ==============================================================================

# MODIFIABLE PARAMETERS (SINGLE IMAGE)

# ==============================================================================

image_path = "bus.jpg"           # Main test image[cite: 58, 71, 73]

target_height = 400              # Standardized resizing height[cite: 14]



# Exp 1 & 2 Filter Parameters

kernel_size = (3, 3)             # Generic spatial filter size[cite: 1, 3, 20]

noise_prob = 0.05                # Noise ratio for median filter testing[cite: 15]

canny_low = 50                   # Canny low threshold[cite: 23, 54]

canny_high = 150                 # Canny high threshold[cite: 23, 54]



# Exp 5 Segmentation Parameters

k_clusters = 5                   # K-Means & Spectral cluster count[cite: 41, 48, 50]

meanshift_sp = 20                # Mean Shift spatial radius[cite: 50]

meanshift_sr = 30                # Mean Shift color radius[cite: 50]



# Exp 6 Feature Extraction Parameters

hough_threshold = 150            # Hough transform voting threshold[cite: 54]

harris_block_size = 2            # Harris corner window size[cite: 57, 58]

harris_ksize = 3                 # Harris Sobel kernel aperture[cite: 57, 58]

harris_k = 0.04                  # Harris detector free parameter[cite: 57, 58]



# Exp 8 Object Detection Parameters

conf_threshold = 0.5             # Deep learning detection cutoff[cite: 71, 73]



# Exp 10 Kalman Filter Parameters

dt = 0.1                         # Time step (seconds)[cite: 89, 90]

n_steps = 100                    # Total tracking steps[cite: 89, 90]

sigma_a = 0.4                    # Process noise (acceleration std)[cite: 89, 90]

sigma_z = 2.0                    # Measurement noise (position std)[cite: 89, 90]

# ==============================================================================



# --- Load and Preprocess Image ---

img = cv2.imread(image_path)[cite: 54, 57, 71]

if img is None:

    # Create a synthetic test canvas if local file is missing

    img = np.zeros((400, 600, 3), dtype=np.uint8)

    cv2.rectangle(img, (50, 50), (250, 350), (255, 255, 255), -1)

    cv2.circle(img, (400, 200), 80, (200, 200, 200), -1)



aspect_ratio = target_height / img.shape[0][cite: 14]

target_width = int(img.shape[1] * aspect_ratio)[cite: 14]

img = cv2.resize(img, (target_width, target_height))[cite: 15]

gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)[cite: 1, 3, 20, 38, 54]



# --- 1. EXPERIMENT 1 & 2: IMAGE ENHANCEMENT & FILTERS ---

min_filtered = cv2.erode(gray, np.ones(kernel_size, np.uint8))[cite: 1, 11]

mean_filtered = cv2.blur(gray, kernel_size)[cite: 3, 11]

stretched = cv2.normalize(gray, None, 0, 255, cv2.NORM_MINMAX)[cite: 5, 11]

equalized = cv2.equalizeHist(gray)[cite: 9, 11, 35]



# Median Filter on Noisy Image

noisy_gray = gray.copy()

num_salt = np.ceil(noise_prob * gray.size * 0.5)[cite: 15]

coords = [np.random.randint(0, i - 1, int(num_salt)) for i in gray.shape][cite: 15]

noisy_gray[tuple(coords)] = 255[cite: 15]

median_filtered = cv2.medianBlur(noisy_gray, kernel_size[0])[cite: 15]



# Summed Area Table (SAT) Box Filter

integral_img = cv2.integral(gray)[cite: 17]

sat_box = cv2.blur(gray, kernel_size) # SAT algorithm equivalent wrapper output[cite: 18]



# Edges: Sobel & Canny

sobel_x = cv2.Sobel(gray, cv2.CV_64F, 1, 0, ksize=3)[cite: 20]

sobel_y = cv2.Sobel(gray, cv2.CV_64F, 0, 1, ksize=3)[cite: 20]

sobel_combined = cv2.addWeighted(cv2.convertScaleAbs(sobel_x), 0.5, cv2.convertScaleAbs(sobel_y), 0.5, 0)[cite: 20]

canny_edges = cv2.Canny(gray, canny_low, canny_high)[cite: 23, 37, 38, 54]



# Display Enhancement Results

plt.figure(figsize=(12, 6))

plt.subplot(2, 4, 1); plt.imshow(gray, cmap="gray"); plt.title("Original")

plt.subplot(2, 4, 2); plt.imshow(min_filtered, cmap="gray"); plt.title("Minimum Filter")

plt.subplot(2, 4, 3); plt.imshow(mean_filtered, cmap="gray"); plt.title("Average Filter")

plt.subplot(2, 4, 4); plt.imshow(stretched, cmap="gray"); plt.title("Contrast Stretch")

plt.subplot(2, 4, 5); plt.imshow(equalized, cmap="gray"); plt.title("Equalized")

plt.subplot(2, 4, 6); plt.imshow(median_filtered, cmap="gray"); plt.title("Median Filter")

plt.subplot(2, 4, 7); plt.imshow(sobel_combined, cmap="gray"); plt.title("Sobel Edges")

plt.subplot(2, 4, 8); plt.imshow(canny_edges, cmap="gray"); plt.title("Canny Edges")

plt.tight_layout(); plt.show()



# --- 2. EXPERIMENT 3: MORPHOLOGICAL OPERATIONS & DISTANCE TRANSFORM ---

morph_kernel = np.ones(kernel_size, np.uint8)[cite: 25, 27, 28]

eroded = cv2.erode(gray, morph_kernel)[cite: 25]

dilated = cv2.dilate(gray, morph_kernel)[cite: 27]

opened = cv2.morphologyEx(gray, cv2.MORPH_OPEN, morph_kernel)[cite: 28]

closed = cv2.morphologyEx(gray, cv2.MORPH_CLOSE, morph_kernel)[cite: 28]



_, binary_thresh = cv2.threshold(gray, 127, 255, cv2.THRESH_BINARY)

dist_transform_l2 = cv2.distanceTransform(binary_thresh, cv2.DIST_L2, 5)[cite: 30]



plt.figure(figsize=(12, 4))

plt.subplot(1, 5, 1); plt.imshow(eroded, cmap="gray"); plt.title("Erosion")

plt.subplot(1, 5, 2); plt.imshow(dilated, cmap="gray"); plt.title("Dilation")

plt.subplot(1, 5, 3); plt.imshow(opened, cmap="gray"); plt.title("Opening")

plt.subplot(1, 5, 4); plt.imshow(closed, cmap="gray"); plt.title("Closing")

plt.subplot(1, 5, 5); plt.imshow(dist_transform_l2, cmap="jet"); plt.title("Distance Trans (L2)")

plt.tight_layout(); plt.show()



# --- 3. EXPERIMENT 4: FREQUENCY SPECTRUM (2D FFT) ---

f_transform = np.fft.fft2(gray)[cite: 36]

f_shift = np.fft.fftshift(f_transform)[cite: 36]

fft_spectrum = 20 * np.log(np.abs(f_shift) + 1)[cite: 36]



plt.figure(figsize=(6, 3))

plt.subplot(1, 2, 1); plt.imshow(gray, cmap='gray'); plt.title("Spatial Domain")

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

plt.tight_layout(); plt.show()



# --- 4. EXPERIMENT 5: COLOR SEGMENTATION ---

# K-Means

pixels = np.float32(img.reshape((-1, 3)))[cite: 41, 50]

criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 0.2)[cite: 41, 50]

_, labels, centers = cv2.kmeans(pixels, k_clusters, None, criteria, 10, cv2.KMEANS_RANDOM_CENTERS)[cite: 41, 50]

kmeans_img = cv2.cvtColor(np.uint8(centers)[labels.flatten()].reshape(img.shape), cv2.COLOR_BGR2RGB)[cite: 41, 50]



# Mean Shift

meanshift_img = cv2.cvtColor(cv2.pyrMeanShiftFiltering(img, meanshift_sp, meanshift_sr), cv2.COLOR_BGR2RGB)[cite: 43, 50]



# Spectral Clustering (Downsampled)

small_img = cv2.resize(cv2.cvtColor(img, cv2.COLOR_BGR2RGB), (40, 40))[cite: 45, 46, 50]

h_s, w_s, _ = small_img.shape[cite: 45, 46, 50]

spectral_data = [[small_img[y, x, 0]/255., small_img[y, x, 1]/255., small_img[y, x, 2]/255., x/w_s, y/h_s] for y in range(h_s) for x in range(w_s)][cite: 45, 46, 50]

spectral_model = SpectralClustering(n_clusters=k_clusters, affinity="nearest_neighbors", n_neighbors=10, random_state=42)[cite: 45, 48, 50]

spectral_labels = spectral_model.fit_predict(np.array(spectral_data))[cite: 45, 46, 50]

spectral_img = np.zeros_like(small_img)[cite: 50]

for i in range(k_clusters):

    pts = small_img.reshape(-1, 3)[spectral_labels == i][cite: 50]

    if len(pts) > 0: spectral_img.reshape(-1, 3)[spectral_labels == i] = np.mean(pts, axis=0).astype(np.uint8)[cite: 50]



plt.figure(figsize=(10, 3))

plt.subplot(1, 3, 1); plt.imshow(kmeans_img); plt.title("K-Means")

plt.subplot(1, 3, 2); plt.imshow(meanshift_img); plt.title("Mean Shift")

plt.subplot(1, 3, 3); plt.imshow(spectral_img); plt.title("Spectral Clustering")

plt.tight_layout(); plt.show()



# --- 5. EXPERIMENT 6: HOUGH LINES & HARRIS CORNERS ---

# Hough Lines

hough_img = cv2.cvtColor(img.copy(), cv2.COLOR_BGR2RGB)[cite: 54]

blurred = cv2.GaussianBlur(gray, (5, 5), 0)[cite: 54]

edges_hough = cv2.Canny(blurred, canny_low, canny_high)[cite: 54]

lines = cv2.HoughLines(edges_hough, 1, np.pi / 180, hough_threshold)[cite: 54]

if lines is not None:

    for line in lines:

        rho, theta = line[0][cite: 54]

        a, b = np.cos(theta), np.sin(theta)[cite: 54]

        x0, y0 = a * rho, b * rho[cite: 54]

        cv2.line(hough_img, (int(x0 + 1000 * (-b)), int(y0 + 1000 * a)), (int(x0 - 1000 * (-b)), int(y0 - 1000 * a)), (255, 0, 0), 2)[cite: 54]



# Harris Corners

harris_img = cv2.cvtColor(img.copy(), cv2.COLOR_BGR2RGB)[cite: 57]

dst_corner = cv2.cornerHarris(np.float32(gray), harris_block_size, harris_ksize, harris_k)[cite: 57]

harris_img[cv2.dilate(dst_corner, None) > 0.01 * dst_corner.max()] = [255, 0, 0][cite: 57]



plt.figure(figsize=(8, 4))

plt.subplot(1, 2, 1); plt.imshow(hough_img); plt.title("Hough Lines")

plt.subplot(1, 2, 2); plt.imshow(harris_img); plt.title("Harris Corners")

plt.tight_layout(); plt.show()



# --- 6. EXPERIMENT 7 & 8: SIFT & DEEP LEARNING DETECTION ---

# SIFT Keypoints

sift = cv2.SIFT_create()[cite: 61, 63, 82]

kp, _ = sift.detectAndCompute(gray, None)[cite: 61, 82]

sift_img = cv2.drawKeypoints(img, kp, None, flags=cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)[cite: 61]



# Faster R-CNN (PyTorch)

frcnn_weights = torchvision.models.detection.FasterRCNN_ResNet50_FPN_Weights.DEFAULT[cite: 71, 75]

frcnn_model = torchvision.models.detection.fasterrcnn_resnet50_fpn(weights=frcnn_weights).eval()[cite: 71, 75]

frcnn_classes = frcnn_weights.meta["categories"][cite: 71]

img_tensor = F.to_tensor(cv2.cvtColor(img, cv2.COLOR_BGR2RGB))[cite: 71, 75]

with torch.no_grad(): frcnn_preds = frcnn_model([img_tensor])[0][cite: 71, 75]



frcnn_out = cv2.cvtColor(img.copy(), cv2.COLOR_BGR2RGB)

for box, label, score in zip(frcnn_preds['boxes'], frcnn_preds['labels'], frcnn_preds['scores']):

    if score > conf_threshold:

        x1, y1, x2, y2 = map(int, box.tolist())[cite: 71, 75]

        cv2.rectangle(frcnn_out, (x1, y1), (x2, y2), (0, 255, 0), 2)[cite: 71, 75]

        cv2.putText(frcnn_out, f"{frcnn_classes[label]}: {score*100:.0f}%", (x1, max(y1-10, 15)), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)[cite: 71]



# YOLO Detection

yolo_model = YOLO("yolov8n.pt")[cite: 73, 75, 78]

yolo_results = yolo_model(img, conf=conf_threshold)[0][cite: 73, 76, 79]

yolo_out = cv2.cvtColor(img.copy(), cv2.COLOR_BGR2RGB)

for box in yolo_results.boxes:

    x1, y1, x2, y2 = map(int, box.xyxy[0])[cite: 73, 76, 79]

    cls_name = yolo_model.names[int(box.cls[0])][cite: 73, 79]

    cv2.rectangle(yolo_out, (x1, y1), (x2, y2), (255, 0, 0), 2)[cite: 73, 76]

    cv2.putText(yolo_out, f"{cls_name}", (x1, max(y1-10, 15)), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 2)[cite: 73]



plt.figure(figsize=(12, 4))

plt.subplot(1, 3, 1); plt.imshow(cv2.cvtColor(sift_img, cv2.COLOR_BGR2RGB)); plt.title(f"SIFT ({len(kp)} Keypoints)")

plt.subplot(1, 3, 2); plt.imshow(frcnn_out); plt.title("Faster R-CNN")

plt.subplot(1, 3, 3); plt.imshow(yolo_out); plt.title("YOLOv8")

plt.tight_layout(); plt.show()



# --- 7. EXPERIMENT 10: DISCRETE-TIME KALMAN FILTER ---

F_mat = np.array([[1, dt], [0, 1]])[cite: 89, 90]

Gamma_mat = np.array([[0.5 * dt**2], [dt]])[cite: 89, 90]

H_mat = np.array([[1, 0]])[cite: 89, 90]



x_true = np.zeros((n_steps, 2))[cite: 90]

x_true[0] = [0.0, 3.0][cite: 89, 90]

for k in range(1, n_steps):

    x_true[k] = F_mat @ x_true[k - 1] + (Gamma_mat * np.random.normal(0, sigma_a)).ravel()[cite: 89, 90]



z_meas = x_true[:, 0] + np.random.normal(0, sigma_z, size=n_steps)[cite: 89, 90]



Q_mat = (sigma_a ** 2) * (Gamma_mat @ Gamma_mat.T)[cite: 89, 90]

R_mat = np.array([[sigma_z ** 2]])[cite: 89, 90]

x_est = np.array([z_meas[0], 0.0])[cite: 90]

P_est = np.diag([sigma_z**2, 25.0])[cite: 90]

x_hat = np.zeros((n_steps, 2))[cite: 90]

x_hat[0] = x_est[cite: 90]



for k in range(1, n_steps):

    x_pred = F_mat @ x_est[cite: 90]

    P_pred = F_mat @ P_est @ F_mat.T + Q_mat[cite: 90]

    y_resid = z_meas[k] - (H_mat @ x_pred)[0][cite: 90]

    S_mat = H_mat @ P_pred @ H_mat.T + R_mat[cite: 90]

    K_gain = P_pred @ H_mat.T @ np.linalg.inv(S_mat)[cite: 90]

    x_est = x_pred + (K_gain.ravel() * y_resid)[cite: 90]

    P_est = (np.eye(2) - K_gain @ H_mat) @ P_pred[cite: 90]

    x_hat[k] = x_est[cite: 90]



plt.figure(figsize=(10, 3))

plt.plot(x_true[:, 0], 'k-', label="True Position")

plt.plot(z_meas, 'r.', alpha=0.4, label="Noisy Measurements")

plt.plot(x_hat[:, 0], 'g-', label="Kalman Filter Estimate")

plt.title("Kalman Filter Tracking (Exp 10)"); plt.legend(); plt.tight_layout(); plt.show()




import cv2

import numpy as np

import matplotlib.pyplot as plt

import time

from scipy.optimize import least_squares

import torch

import torchvision

from torchvision.transforms import functional as F

from ultralytics import YOLO



# ==============================================================================

# MODIFIABLE PARAMETERS (MULTI-IMAGE)

# ==============================================================================

# Exp 7 Matching Paths

image1_path = "obj1.webp"        # Object image 1 path[cite: 63]

image2_path = "obj2.webp"        # Object image 2 path[cite: 63]

ratio_thresh = 0.75              # Lowe's ratio test threshold[cite: 63, 82]

match_count_thresh = 20          # Threshold for decision[cite: 64]



# Exp 8 Benchmark Path

benchmark_image_path = "bus.jpg" # Image used for dual-model comparison[cite: 71, 73, 75]

conf_threshold = 0.5             # Bounding box confidence cutoff[cite: 71, 73, 75]



# Exp 9 Bundle Adjustment Paths & Intrinsics

view1_path = "view1.png"         # Reconstruction View 1[cite: 82]

view2_path = "view2.png"         # Reconstruction View 2[cite: 82]

max_depth = 50.0                 # Max valid depth for triangulated points[cite: 83]

max_iterations = 200             # Max evaluations for bundle adjustment[cite: 85]



# Camera Intrinsic Matrix K

K_matrix = np.array([[800, 0, 320],

                     [0, 800, 240],

                     [0, 0, 1]], dtype=np.float64)[cite: 83]

# ==============================================================================



# --- 1. EXPERIMENT 7: SIFT FEATURE MATCHING ACROSS 2 IMAGES ---

img1_sift = cv2.imread(image1_path)[cite: 63]

img2_sift = cv2.imread(image2_path)[cite: 63]



if img1_sift is None or img2_sift is None:

    # Synthetic images if files are unavailable

    img1_sift = np.zeros((300, 300, 3), dtype=np.uint8)

    img2_sift = np.zeros((300, 300, 3), dtype=np.uint8)

    cv2.rectangle(img1_sift, (50, 50), (200, 200), (255, 255, 255), -1)

    cv2.rectangle(img2_sift, (70, 70), (220, 220), (255, 255, 255), -1)



gray1_sift = cv2.cvtColor(img1_sift, cv2.COLOR_BGR2GRAY)[cite: 63]

gray2_sift = cv2.cvtColor(img2_sift, cv2.COLOR_BGR2GRAY)[cite: 63]



sift = cv2.SIFT_create()[cite: 61, 63, 82]

kp1, des1 = sift.detectAndCompute(gray1_sift, None)[cite: 63, 82]

kp2, des2 = sift.detectAndCompute(gray2_sift, None)[cite: 63, 82]



bf = cv2.BFMatcher()[cite: 63, 82]

raw_matches = bf.knnMatch(des1, des2, k=2)[cite: 63, 82]



good_matches = []

for m, n in raw_matches:

    if m.distance < ratio_thresh * n.distance:[cite: 63, 82]

        good_matches.append(m)[cite: 63]



matching_result = cv2.drawMatches(

    img1_sift, kp1, img2_sift, kp2, good_matches, None,

    flags=cv2.DrawMatchesFlags_NOT_DRAW_SINGLE_POINTS

)[cite: 64]



plt.figure(figsize=(10, 4))

plt.imshow(cv2.cvtColor(matching_result, cv2.COLOR_BGR2RGB))

plt.title(f"SIFT Matching ({len(good_matches)} Good Matches Passed Ratio Test)")

plt.axis("off")

plt.show()





# --- 2. EXPERIMENT 8: DUAL-MODEL DETECTION BENCHMARK ---

img_bench = cv2.imread(benchmark_image_path)[cite: 71, 73, 75]

if img_bench is None:

    img_bench = np.zeros((400, 600, 3), dtype=np.uint8)

    cv2.rectangle(img_bench, (100, 100), (300, 300), (0, 255, 0), -1)



img_frcnn = cv2.cvtColor(img_bench.copy(), cv2.COLOR_BGR2RGB)

img_yolo = cv2.cvtColor(img_bench.copy(), cv2.COLOR_BGR2RGB)



# Faster R-CNN Execution

frcnn_weights = torchvision.models.detection.FasterRCNN_ResNet50_FPN_Weights.DEFAULT[cite: 71, 75]

frcnn_model = torchvision.models.detection.fasterrcnn_resnet50_fpn(weights=frcnn_weights).eval()[cite: 71, 75]

start_t = time.time()

with torch.no_grad():

    frcnn_preds = frcnn_model([F.to_tensor(img_frcnn)])[0][cite: 71, 75]

frcnn_time = time.time() - start_t



for box, score in zip(frcnn_preds['boxes'], frcnn_preds['scores']):

    if score > conf_threshold:

        x1, y1, x2, y2 = map(int, box.tolist())[cite: 71, 75]

        cv2.rectangle(img_frcnn, (x1, y1), (x2, y2), (0, 255, 0), 2)[cite: 71, 75]



# YOLO Execution

yolo_model = YOLO("yolov8n.pt")[cite: 73, 75, 78]

start_t = time.time()

yolo_results = yolo_model(img_bench, conf=conf_threshold)[0][cite: 73, 76, 79]

yolo_time = time.time() - start_t



for box in yolo_results.boxes:

    x1, y1, x2, y2 = map(int, box.xyxy[0])[cite: 73, 76, 79]

    cv2.rectangle(img_yolo, (x1, y1), (x2, y2), (255, 0, 0), 2)[cite: 73, 76]



plt.figure(figsize=(10, 4))

plt.subplot(1, 2, 1); plt.imshow(img_frcnn); plt.title(f"Faster R-CNN ({frcnn_time:.2f}s)")

plt.subplot(1, 2, 2); plt.imshow(img_yolo); plt.title(f"YOLOv8 ({yolo_time:.2f}s)")

plt.tight_layout(); plt.show()





# --- 3. EXPERIMENT 9: BUNDLE ADJUSTMENT FOR 3D RECONSTRUCTION ---

v1_img = cv2.imread(view1_path)[cite: 82]

v2_img = cv2.imread(view2_path)[cite: 82]



if v1_img is None or v2_img is None:

    # Use synthetic images if local stereo pair is missing

    v1_img, v2_img = img1_sift.copy(), img2_sift.copy()



v1_gray = cv2.cvtColor(v1_img, cv2.COLOR_BGR2GRAY)[cite: 82]

v2_gray = cv2.cvtColor(v2_img, cv2.COLOR_BGR2GRAY)[cite: 82]



kp1_3d, des1_3d = sift.detectAndCompute(v1_gray, None)[cite: 82]

kp2_3d, des2_3d = sift.detectAndCompute(v2_gray, None)[cite: 82]



matches_3d = bf.knnMatch(des1_3d, des2_3d, k=2)[cite: 82]

good_3d = [m for m, n in matches_3d if m.distance < ratio_thresh * n.distance][cite: 82]



pts1_3d = np.float32([kp1_3d[m.queryIdx].pt for m in good_3d])[cite: 82]

pts2_3d = np.float32([kp2_3d[m.trainIdx].pt for m in good_3d])[cite: 82]



# Essential Matrix & Pose Recovery

E_mat, mask_e = cv2.findEssentialMat(pts1_3d, pts2_3d, K_matrix, method=cv2.RANSAC, prob=0.999, threshold=1.0)[cite: 83]

inliers = mask_e.ravel().astype(bool)[cite: 83]

pts1_in, pts2_in = pts1_3d[inliers], pts2_3d[inliers][cite: 83]



R_rec, t_rec, _ = cv2.recoverPose(E_mat, pts1_in, pts2_in, K_matrix)[cite: 83]



# Triangulation

P1 = K_matrix @ np.hstack([np.eye(3), np.zeros((3, 1))])[cite: 83]

P2 = K_matrix @ np.hstack([R_rec, t_rec])[cite: 83]



pts4d = cv2.triangulatePoints(P1, P2, pts1_in.T, pts2_in.T)[cite: 83]

pts3d = (pts4d[:3] / pts4d[3]).T[cite: 83]



depth1 = pts3d[:, 2][cite: 83]

depth2 = (R_rec @ pts3d.T + t_rec).T[:, 2][cite: 83]

valid = (depth1 > 0) & (depth1 < max_depth) & (depth2 > 0) & (depth2 < max_depth)[cite: 83]



pts3d_valid = pts3d[valid][cite: 83]

pts1_v, pts2_v = pts1_in[valid], pts2_in[valid][cite: 83]



# Reprojection Error Calculation Function

def calculate_reprojection_rmse(R1, t1, R2, t2, X, obs1, obs2, K):

    proj1, _ = cv2.projectPoints(X, cv2.Rodrigues(R1)[0], t1, K, None)[cite: 84, 85]

    proj2, _ = cv2.projectPoints(X, cv2.Rodrigues(R2)[0], t2, K, None)[cite: 84, 85]

    err1 = np.linalg.norm(proj1.reshape(-1, 2) - obs1, axis=1)[cite: 84]

    err2 = np.linalg.norm(proj2.reshape(-1, 2) - obs2, axis=1)[cite: 84]

    return np.sqrt(np.mean(np.concatenate([err1, err2]) ** 2))[cite: 84, 85]



rmse_before = calculate_reprojection_rmse(np.eye(3), np.zeros((3, 1)), R_rec, t_rec, pts3d_valid, pts1_v, pts2_v, K_matrix)[cite: 84]



# Bundle Adjustment Residuals Optimization

def ba_residuals(params, n_pts, obs1, obs2, K):

    rvec2 = params[0:3][cite: 85]

    tvec2 = params[3:6][cite: 85]

    X = params[6:].reshape(n_pts, 3)[cite: 85]

    proj1, _ = cv2.projectPoints(X, np.zeros(3), np.zeros(3), K, None)[cite: 85]

    proj2, _ = cv2.projectPoints(X, rvec2, tvec2, K, None)[cite: 85]

    return np.concatenate([(proj1.reshape(-1, 2) - obs1).ravel(), (proj2.reshape(-1, 2) - obs2).ravel()])[cite: 85]



n_pts = pts3d_valid.shape[0][cite: 85]

initial_params = np.concatenate([cv2.Rodrigues(R_rec)[0].ravel(), t_rec.ravel(), pts3d_valid.ravel()])[cite: 85]

opt_result = least_squares(ba_residuals, initial_params, method="lm", max_nfev=max_iterations, args=(n_pts, pts1_v, pts2_v, K_matrix))[cite: 85]



opt_pts3d = opt_result.x[6:].reshape(n_pts, 3)[cite: 85]

opt_R2 = cv2.Rodrigues(opt_result.x[0:3])[0][cite: 85]

opt_t2 = opt_result.x[3:6].reshape(3, 1)[cite: 85]



rmse_after = calculate_reprojection_rmse(np.eye(3), np.zeros((3, 1)), opt_R2, opt_t2, opt_pts3d, pts1_v, pts2_v, K_matrix)[cite: 85]



print("\n--- Experiment 9: 3D Bundle Adjustment Metrics ---")

print(f"Points Triangulated : {n_pts}")[cite: 83]

print(f"RMSE Before Optimization : {rmse_before:.4f} px")[cite: 84, 85]

print(f"RMSE After Optimization  : {rmse_after:.4f} px")[cite: 85]



# 3D Point Cloud Plotting

fig = plt.figure(figsize=(10, 4))

ax1 = fig.add_subplot(1, 2, 1, projection='3d')

ax1.scatter(pts3d_valid[:, 0], pts3d_valid[:, 1], pts3d_valid[:, 2], c='r', marker='.', s=10)[cite: 86]

ax1.set_title(f"Initial 3D Points (RMSE: {rmse_before:.2f}px)")[cite: 86]



ax2 = fig.add_subplot(1, 2, 2, projection='3d')

ax2.scatter(opt_pts3d[:, 0], opt_pts3d[:, 1], opt_pts3d[:, 2], c='g', marker='.', s=10)[cite: 86]

ax2.set_title(f"Refined 3D Points (RMSE: {rmse_after:.2f}px)")[cite: 86]

plt.tight_layout(); plt.show()