""" Filename: part_B.py Description: Second part of processing Author: Tyler de Zeeuw License: GPL-3.0 """ # Built-in imports import os import re import sys import time import argparse import subprocess # External library imports import cv2 import h5py import numpy as np import pyvista as pv from scipy.spatial.distance import cdist from scipy.optimize import linear_sum_assignment from concurrent.futures import ThreadPoolExecutor #TODO: Most of this file is very hard coded. Needs to be user-defined parameters print("\n--- DEBUG: Inside Part B ---") print(f"Current System Working Directory: {os.getcwd()}") print(f"Directory of this script file: {os.path.dirname(os.path.abspath(__file__))}") Kp = np.load("phone_matrix.npy") Dp = np.load("phone_dist.npy") K1 = np.load("camera1_matrix.npy") D1 = np.load("dist1_coeffs.npy") K2 = np.load("camera2_matrix.npy") D2 = np.load("dist2_coeffs.npy") R_p1 = np.load("R_cam1_to_phone.npy") t_p1 = np.load("t_cam1_to_phone.npy").reshape(3, 1) R_p2 = np.load("R_cam2_to_phone.npy") t_p2 = np.load("t_cam2_to_phone.npy").reshape(3, 1) phone_pos = np.zeros(3) cam1_pos = t_p1.flatten() cam2_pos = t_p2.flatten() GREEN_STRICT_LOW = np.array([36, 128, 64]) GREEN_STRICT_HIGH = np.array([90, 255, 255]) GREEN_LOOSE_LOW = np.array([32, 108, 52]) GREEN_LOOSE_HIGH = np.array([95, 255, 255]) PINK_STRICT_LOW = np.array([150, 100, 80]) PINK_STRICT_HIGH = np.array([175, 255, 255]) PINK_LOOSE_LOW = np.array([145, 60, 60]) PINK_LOOSE_HIGH = np.array([175, 255, 255]) RED_STRICT_LOW = np.array([0, 170, 160]) RED_STRICT_HIGH = np.array([10, 255, 255]) RED_LOOSE_LOW = np.array([0, 120, 100]) RED_LOOSE_HIGH = np.array([14, 255, 255]) PALE_STRICT_LOW = np.array([0, 110, 180]) PALE_STRICT_HIGH = np.array([4, 145, 255]) PALE_LOOSE_LOW = np.array([0, 95, 150]) PALE_LOOSE_HIGH = np.array([6, 155, 255]) TOLERANCE_METERS = 0.020 # Maximum allowed ray miss RANK_PENALTY = 0.03 # High penalty to ruthlessly suppress "X" cross-overs TOLERANCE_METERS2 = 0.015 # Maximum allowed ray miss RANK_PENALTY2 = 0.05 # High penalty to ruthlessly suppress "X" cross-overs def print_elapsed(label="Timestamp"): """Helper function to print time elapsed since the script started.""" elapsed = time.perf_counter() - APP_START_TIME print(f"[{label}] {elapsed:.3f}s total elapsed time") # ---------------------------------------------------------------------- # CAP + CORE + SURROUND EXTRACTION # ---------------------------------------------------------------------- def find_cap_mask(image, downscale=4): """Good method (now runs the expensive morphology/contour steps on a downsampled copy, then upsamples the final mask back to full resolution). The cap boundary is a coarse blob, so this loses no meaningful precision while cutting cvtColor/inRange/morphologyEx/dilate/findContours cost by ~downscale^2.""" h, w = image.shape[:2] if downscale > 1: small = cv2.resize(image, (max(1, w // downscale), max(1, h // downscale)), interpolation=cv2.INTER_AREA) else: small = image sh, sw = small.shape[:2] center = (sw // 2, sh // 2) hsv = cv2.cvtColor(small, cv2.COLOR_BGR2HSV) dark_mask = cv2.inRange(hsv, (0, 0, 0), (180, 120, 80)) k = max(3, round(15 / downscale)) kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (k, k)) closed = cv2.morphologyEx(dark_mask, cv2.MORPH_CLOSE, kernel) contours, _ = cv2.findContours(closed, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) if not contours: return np.ones((h, w), dtype=np.uint8) * 255 best_cnt = None for cnt in contours: if cv2.pointPolygonTest(cnt, (float(center[0]), float(center[1])), False) >= 0: best_cnt = cnt break if best_cnt is None: best_dist = float('inf') for cnt in contours: M = cv2.moments(cnt) if M["m00"] != 0: cx = int(M["m10"] / M["m00"]) cy = int(M["m01"] / M["m00"]) dist = np.hypot(cx - center[0], cy - center[1]) if dist < best_dist: best_dist = dist best_cnt = cnt if best_cnt is None and contours: best_cnt = max(contours, key=cv2.contourArea) mask = np.zeros_like(dark_mask) if best_cnt is not None: cv2.drawContours(mask, [best_cnt], -1, 255, thickness=cv2.FILLED) else: mask[:] = 255 dil_k = max(3, round(30 / downscale)) dil_kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (dil_k, dil_k)) dilated_small = cv2.dilate(mask, dil_kernel, iterations=1) if downscale > 1: return cv2.resize(dilated_small, (w, h), interpolation=cv2.INTER_NEAREST) return dilated_small _SURROUND_DILATE_KERNEL = np.ones((7, 7), np.uint8) def extract_core_surround(hsv_image, strict_low, strict_high, loose_low, loose_high, roi_mask): """Good method. roi_mask may be passed pre-converted to bool (recommended, since callers processing both green and pink share the same cap_mask and otherwise redundantly re-run the full-image astype(bool) conversion).""" strict = cv2.inRange(hsv_image, strict_low, strict_high).astype(bool) loose = cv2.inRange(hsv_image, loose_low, loose_high).astype(bool) roi = roi_mask if roi_mask.dtype == bool else roi_mask.astype(bool) strict = strict & roi loose = loose & roi core = strict core_dilated = cv2.dilate(core.astype(np.uint8), _SURROUND_DILATE_KERNEL, iterations=2).astype(bool) surround = loose & core_dilated & (~core) return core, surround def analyse_components(core_mask, surround_mask, min_total_area=100, x_offset=0, y_offset=0): # core_mask/surround_mask are always boolean arrays produced by # extract_core_surround's boolean (&) ops, so the conversion to a 0/255 # uint8 mask is unconditional -- no need to check np.max() first (that # was an extra full-image scan for a result that's always true here). core_uint8 = core_mask.astype(np.uint8) * 255 surround_uint8 = surround_mask.astype(np.uint8) * 255 # Combine using safe uint8 arrays combined = cv2.bitwise_or(core_uint8, surround_uint8) num_labels, labels, stats, centroids = cv2.connectedComponentsWithStats(combined, connectivity=8) results = [] for i in range(1, num_labels): total_px = int(stats[i, cv2.CC_STAT_AREA]) if total_px < min_total_area: continue x = stats[i, cv2.CC_STAT_LEFT] y = stats[i, cv2.CC_STAT_TOP] w = stats[i, cv2.CC_STAT_WIDTH] h = stats[i, cv2.CC_STAT_HEIGHT] # Crop out the region of interest from our uint8 core mask roi_core = core_uint8[y:y+h, x:x+w] roi_labels = labels[y:y+h, x:x+w] # CRITICAL FIX 2: Build a uint8 mask that perfectly matches the ROI shape and type component_mask = (roi_labels == i).astype(np.uint8) * 255 # Safe bitwise operation now that types and dimensions match perfectly core_px = int(cv2.countNonZero(cv2.bitwise_and(roi_core, component_mask))) if core_px < min_total_area: continue surround_px = total_px - core_px # Find contours directly inside the tiny component mask crop contours, _ = cv2.findContours(component_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) cx, cy = None, None if contours: cnt = max(contours, key=cv2.contourArea) if len(cnt) >= 5: try: ellipse = cv2.fitEllipse(cnt) (cx_ell, cy_ell), _, _ = ellipse cx = int(round(cx_ell + x)) cy = int(round(cy_ell + y)) except cv2.error: pass # Fall back to centroid if math fails if cx is None: cx = int(centroids[i][0]) cy = int(centroids[i][1]) # Translate back into full-image pixel coordinates if this was # called on a cropped ROI (see extract_optode_data). results.append((core_px, surround_px, total_px, cx + x_offset, cy + y_offset)) return results def extract_optode_data(image_path): """Good method""" img = cv2.imread(image_path) if img is None: print(f"Error: Could not load image {image_path}") return [], [] hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) cap_mask = find_cap_mask(img) cap_mask_bool = cap_mask.astype(bool) # convert once, reuse for green + pink green_core, green_surround = extract_core_surround( hsv, GREEN_STRICT_LOW, GREEN_STRICT_HIGH, GREEN_LOOSE_LOW, GREEN_LOOSE_HIGH, cap_mask_bool) pink_core, pink_surround = extract_core_surround( hsv, PINK_STRICT_LOW, PINK_STRICT_HIGH, PINK_LOOSE_LOW, PINK_LOOSE_HIGH, cap_mask_bool) red_core, red_surround = extract_core_surround( hsv, RED_STRICT_LOW, RED_STRICT_HIGH, RED_LOOSE_LOW, RED_LOOSE_HIGH, cap_mask_bool) pale_core, pale_surround = extract_core_surround( hsv, PALE_STRICT_LOW, PALE_STRICT_HIGH, PALE_LOOSE_LOW, PALE_LOOSE_HIGH, cap_mask_bool) print_elapsed("b") # cv2.connectedComponentsWithStats scans every pixel of what it's given. # core/surround can only ever be non-zero inside the cap ROI, so running # it on the full-resolution frame wastes most of the scan on background. # Crop to the ROI's bounding box first (with a safety margin, since the # 7x7/2-iteration dilation used to build `surround` can reach a few # pixels beyond the exact cap_mask boundary) and translate coordinates # back afterward via analyse_components' x_offset/y_offset. h, w = cap_mask_bool.shape ys, xs = np.where(cap_mask_bool) pad = 32 if ys.size and xs.size: y0 = max(0, int(ys.min()) - pad) y1 = min(h, int(ys.max()) + 1 + pad) x0 = max(0, int(xs.min()) - pad) x1 = min(w, int(xs.max()) + 1 + pad) else: y0, y1, x0, x1 = 0, h, 0, w green_data = analyse_components( green_core[y0:y1, x0:x1], green_surround[y0:y1, x0:x1], x_offset=x0, y_offset=y0) pink_data = analyse_components( pink_core[y0:y1, x0:x1], pink_surround[y0:y1, x0:x1], x_offset=x0, y_offset=y0) red_data = analyse_components( red_core[y0:y1, x0:x1], red_surround[y0:y1, x0:x1], x_offset=x0, y_offset=y0) pale_data = analyse_components( pale_core[y0:y1, x0:x1], pale_surround[y0:y1, x0:x1], x_offset=x0, y_offset=y0) print_elapsed("a") red_data += pale_data return green_data, pink_data, red_data # ====================================================================== # 1. RAY GEOMETRY ENGINE (WITH CONSOLE DIAGNOSTICS) # ====================================================================== def pixel_to_ray_corrected(u, v, K, D, R_to_phone=None, t_to_phone=None): """Undistorst 2D pixels and casts 3D rays.""" pt = np.array([[[float(u), float(v)]]], dtype=np.float32) undistorted = cv2.undistortPoints(pt, K, D, P=None) xn, yn = undistorted[0][0] dir_local = np.array([xn, yn, 1.0]) if R_to_phone is None and t_to_phone is None: ray_origin = np.zeros(3) ray_dir = dir_local / np.linalg.norm(dir_local) else: ray_origin = t_to_phone.flatten() # DIAGNOSTIC REVERSAL CHECK: If rays look backward in the plot, # change the line below to: ray_dir = R_to_phone.T @ dir_local ray_dir = R_to_phone @ dir_local ray_dir /= np.linalg.norm(ray_dir) return ray_origin, ray_dir def intersect_rays(ray_origins, ray_dirs): """Finds closest 3D intersection point for N rays.""" I = np.eye(3) sum_A = np.zeros((3, 3)) sum_b = np.zeros(3) for p, d in zip(ray_origins, ray_dirs): A = I - np.outer(d, d) sum_A += A sum_b += A @ p try: point_3d = np.linalg.solve(sum_A, sum_b) except np.linalg.LinAlgError: return None, float('inf') errors = [] for p, d in zip(ray_origins, ray_dirs): vec = point_3d - p perpend_dist = np.linalg.norm(vec - np.dot(vec, d) * d) errors.append(perpend_dist) return point_3d, max(errors) # ====================================================================== # 2. LABELED 2D DEBUG IMAGE GENERATOR # ====================================================================== def save_labeled_debug_image(image_path, green_pts, pink_pts, output_name): img = cv2.imread(image_path) if img is None: print(f" [ERROR] Can't open {image_path}") return print(f" [IMAGE DEBUG] Writing tracking labels onto {output_name}...") for idx, pt in enumerate(green_pts): cv2.circle(img, (pt[3], pt[4]), 10, (0, 255, 0), 2) cv2.putText(img, f"G_{idx}", (pt[3]+12, pt[4]-4), cv2.FONT_HERSHEY_PLAIN, 0.5, (0, 255, 0), 1) for idx, pt in enumerate(pink_pts): cv2.circle(img, (pt[3], pt[4]), 10, (180, 105, 255), 2) cv2.putText(img, f"P_{idx}", (pt[3]+12, pt[4]-4), cv2.FONT_HERSHEY_PLAIN, 0.5, (180, 105, 255), 1) cv2.imwrite(output_name, img) # ====================================================================== # 3. VERBOSITY MATCHING LOOP # ====================================================================== def match_and_reconstruct_2d_ordered(color_label, phone_pts, cam2_pts, dist_threshold=0.015, rank_penalty_weight=0.02): """This is for any camera that is comparing against a camera that is 45 degrees to the LEFT.""" print(f"\n" + "="*80) print(f" STARTING 2D TOPOLOGY-ORDERED GLOBAL ASSIGNMENT FOR {color_label}") print(f"="*80) # 1. Sort both point sets by their 2D X-coordinate sorted_phone_pts = sorted(phone_pts, key=lambda pt: pt[3]) sorted_cam2_pts = sorted(cam2_pts, key=lambda pt: pt[3]) print(f"[STEP 1] Sorted 2D Inputs (Front-to-Back order) -> Phone: {len(sorted_phone_pts)} points | Cam2: {len(sorted_cam2_pts)} points") # 2. Generate rays from the ordered 2D sequences rays_p = [] for i, pt in enumerate(sorted_phone_pts): origin, direction = pixel_to_ray_corrected(pt[3], pt[4], Kp, Dp) rays_p.append({'origin': origin, 'dir': direction, 'rank': i, 'orig_id': pt[0] if len(pt) > 0 else i}) rays_2 = [] for j, pt in enumerate(sorted_cam2_pts): origin, direction = pixel_to_ray_corrected(pt[3], pt[4], K2, D2, R_p2, t_p2) rays_2.append({'origin': origin, 'dir': direction, 'rank': j, 'orig_id': pt[0] if len(pt) > 0 else j}) num_p, num_2 = len(rays_p), len(rays_2) if num_p == 0 or num_2 == 0: print("[WARN] Aborting match: One or both camera arrays are completely empty.") return [], (rays_p, [], rays_2) # 3. Build the Topology-Aware Cost Matrix print(f"\n[STEP 3] Evaluating all {num_p}x{num_2} ({num_p * num_2}) structural ray combinations...") cost_matrix = np.zeros((num_p, num_2)) d_points_matrix = {} for i, rp in enumerate(rays_p): for j, r2 in enumerate(rays_2): pt_3d, err = intersect_rays([rp['origin'], r2['origin']], [rp['dir'], r2['dir']]) d_points_matrix[(i, j)] = (pt_3d, err) if err > dist_threshold: cost_matrix[i, j] = 999.0 # Hard threshold limit else: # Calculate Rank Divergence rank_diff = abs(i - j) penalty = rank_diff * rank_penalty_weight # Total Cost = Ray Intersection Error + 2D Order Violation Penalty cost_matrix[i, j] = err + penalty # 4. Solve globally across the grid simultaneously print("\n[STEP 4] Executing global linear sum assignment solver...") row_ind, col_ind = linear_sum_assignment(cost_matrix) reconstructed_list = [] match_idx = 0 print("\n[STEP 5] Parsing global optimization results:") print(f" {'Label':<8} | {'Phone Rank':<10} -> {'Cam2 Rank':<10} | {'Ray Miss':<12} | {'Rank Delta':<10} | {'Total Cost':<10}") print(" " + "-"*75) for r, c in zip(row_ind, col_ind): total_cost = cost_matrix[r, c] if total_cost < 999.0: pt_3d, ray_miss_err = d_points_matrix[(r, c)] lbl = f"{color_label}_{match_idx}" rank_delta = abs(r - c) print(f" {lbl:<8} | {r:<10} -> {c:<10} | {ray_miss_err*1000:7.2f} mm | {rank_delta:<10} | {total_cost:.4f}") reconstructed_list.append({ 'point': pt_3d, 'label': lbl, 'err': ray_miss_err, 'phone_key': (sorted_phone_pts[r][3], sorted_phone_pts[r][4]), 'cam1_key': None, 'cam2_key': (sorted_cam2_pts[c][3], sorted_cam2_pts[c][4]), 'rays': [rays_p[r], rays_2[c]] }) match_idx += 1 else: print(f" [SKIP] | Phone Rank {r:<4} or Cam2 Rank {c:<4} left unmatched (Violated distance or rank threshold).") print("\n" + "="*80) print(f"[SUMMARY] Reconstructed {match_idx} optodes. 2D Order Violations successfully blocked.") print("="*80) return reconstructed_list, (rays_p, [], rays_2) def match_and_reconstruct_cam1_right_anchored(color_label, phone_pts, cam1_pts, dist_threshold=0.015, rank_penalty_weight=0.04): """This is for any camera that is comparing against a camera that is 45 degrees to the RIGHT.""" print(f"\n" + "="*80) print(f" RIGHT-ANCHORED CAM1 GLOBAL ASSIGNMENT FOR {color_label}") print(f"="*80) # 1. Sort both sets left-to-right based on u-pixel column sorted_phone_pts = sorted(phone_pts, key=lambda pt: pt[3]) sorted_cam1_pts = sorted(cam1_pts, key=lambda pt: pt[3]) num_p = len(sorted_phone_pts) num_1 = len(sorted_cam1_pts) print(f"[STEP 1] Sorted Inputs -> Phone: {num_p} points | Cam1: {num_1} points") if num_p == 0 or num_1 == 0: return [], ([], [], []) # 2. Build rays (single call per point; previously called # pixel_to_ray_corrected twice per point just to split origin/dir out) rays_p = [] for pt in sorted_phone_pts: origin, direction = pixel_to_ray_corrected(pt[3], pt[4], Kp, Dp) rays_p.append({'origin': origin, 'dir': direction}) rays_1 = [] for pt in sorted_cam1_pts: origin, direction = pixel_to_ray_corrected(pt[3], pt[4], K1, D1, R_p1, t_p1) rays_1.append({'origin': origin, 'dir': direction}) # 3. Cost Matrix with Right-Side Alignment Priority cost_matrix = np.zeros((num_p, num_1)) d_points_matrix = {} for i in range(num_p): # Distance from the RIGHT edge for Phone phone_right_rank = num_p - 1 - i for j in range(num_1): # Distance from the RIGHT edge for Cam1 cam1_right_rank = num_1 - 1 - j pt_3d, err = intersect_rays([rays_p[i]['origin'], rays_1[j]['origin']], [rays_p[i]['dir'], rays_1[j]['dir']]) d_points_matrix[(i, j)] = (pt_3d, err) if err > dist_threshold: cost_matrix[i, j] = 999.0 else: # Penalty is based on how mismatched they are from the RIGHT side rank_diff = abs(phone_right_rank - cam1_right_rank) cost_matrix[i, j] = err + (rank_diff * rank_penalty_weight) # 4. Solve Globally row_ind, col_ind = linear_sum_assignment(cost_matrix) reconstructed_list = [] match_idx = 0 print("\n[STEP 5] Parsing Right-Anchored Alignment:") for r, c in zip(row_ind, col_ind): total_cost = cost_matrix[r, c] if total_cost < 999.0: pt_3d, ray_miss_err = d_points_matrix[(r, c)] lbl = f"{color_label}_C1_{match_idx}" print(f" {lbl:<8} | Phone Item {r} -> Cam1 Item {c} (Aligned from Right) | Ray Miss: {ray_miss_err*1000:6.2f} mm") reconstructed_list.append({ 'point': pt_3d, 'label': lbl, 'err': ray_miss_err, 'phone_key': (sorted_phone_pts[r][3], sorted_phone_pts[r][4]), 'cam1_key': (sorted_cam1_pts[c][3], sorted_cam1_pts[c][4]), 'cam2_key': None, 'rays': [rays_p[r], rays_1[c]] }) match_idx += 1 else: print(f" [CLEAN SKIP] Phone index {r} (Western flank) successfully left unmatched.") return reconstructed_list, (rays_p, [], rays_1) # def match_90_deg_spatial_with_right_bias(color_label, cam1_pts, cam2_pts, max_ray_miss_mm=1.5, rank_weight=0.030): # """This is for any camera that is comparing against a camera that is 90 degrees to the right/left? I don't know.""" # print(f"\n" + "="*80) # print(f" SPATIAL INTERSECTION WITH RIGHT-SIDE PRIORITIZATION FOR {color_label}") # print(f"="*80) # # 1. Enumerate original indices BEFORE sorting so they match the debug image stamps # indexed_cam1 = list(enumerate(cam1_pts)) # indexed_cam2 = list(enumerate(cam2_pts)) # # Sort left-to-right based on pixel x-coordinate (pt[3]) # sorted_cam1 = sorted(indexed_cam1, key=lambda x: x[1][3]) # sorted_cam2 = sorted(indexed_cam2, key=lambda x: x[1][3]) # num_1 = len(sorted_cam1) # num_2 = len(sorted_cam2) # print(f"[INFO] Cam1 visible cluster: {num_1} dots | Cam2 visible cluster: {num_2} dots") # if num_1 == 0 or num_2 == 0: # return [], ([], [], []) # # 2. Build world rays using the sorted configurations (single call per # # point; previously called pixel_to_ray_corrected twice per point just # # to split origin/dir out) # rays_1 = [] # for _, pt in sorted_cam1: # origin, direction = pixel_to_ray_corrected(pt[3], pt[4], K1, D1, R_p1, t_p1) # rays_1.append({'origin': origin, 'dir': direction}) # rays_2 = [] # for _, pt in sorted_cam2: # origin, direction = pixel_to_ray_corrected(pt[3], pt[4], K2, D2, R_p2, t_p2) # rays_2.append({'origin': origin, 'dir': direction}) # cost_matrix = np.full((num_1, num_2), 999.0) # pts_3d_matrix = {} # max_ray_miss_m = max_ray_miss_mm / 1000.0 # mm to meters # expected_shift = max(0, num_2 - num_1) # # 3. Populate Cost Matrix # for i in range(num_1): # for j in range(num_2): # pt_3d, err = intersect_rays([rays_1[i]['origin'], rays_2[j]['origin']], [rays_1[i]['dir'], rays_2[j]['dir']]) # if err <= max_ray_miss_m: # rank_deviation = abs(j - (i + expected_shift)) # cost_matrix[i, j] = err + (rank_deviation * rank_weight) # pts_3d_matrix[(i, j)] = (pt_3d, err) # # 4. Global Linear Assignment # row_ind, col_ind = linear_sum_assignment(cost_matrix) # reconstructed_list = [] # match_idx = 0 # print(f"\n[MATCH LOG] Resolving height ambiguities (Threshold: {max_ray_miss_mm} mm):") # for r, c in zip(row_ind, col_ind): # total_cost = cost_matrix[r, c] # if total_cost < 999.0: # pt_3d, pure_ray_miss = pts_3d_matrix[(r, c)] # lbl = f"{color_label}_C1C2_{match_idx}" # # Extract the raw, original image indices for the print statement # orig_cam1_idx = sorted_cam1[r][0] # orig_cam2_idx = sorted_cam2[c][0] # # Cleanly maps directly back to your annotated image labels # print(f" [MATCH ACCEPTED] {lbl:<10} | Cam1 Image Label #{orig_cam1_idx} -> Cam2 Image Label #{orig_cam2_idx} | Pure Miss: {pure_ray_miss*1000:.2f} mm") # reconstructed_list.append({ # 'point': pt_3d, # 'label': lbl, # 'err': pure_ray_miss, # 'phone_key': None, # 'cam1_key': (sorted_cam1[r][1][3], sorted_cam1[r][1][4]), # 'cam2_key': (sorted_cam2[c][1][3], sorted_cam2[c][1][4]), # 'rays': [rays_1[r], rays_2[c]] # }) # match_idx += 1 # print(f"\n[SUMMARY] Successfully isolated {match_idx} shared optodes.") # return reconstructed_list, (rays_1, [], rays_2) def match_90_deg_spatial_with_right_bias( color_label, cam1_pts, cam2_pts, max_ray_miss_mm=1.5, rank_weight=0.030, max_rank_deviation=4, # <-- NEW: Hard cap on how far out of sequence a match can be visualize=True ): """ Matches features between two orthogonal cameras with strict spatial limits and a hard cap on sorting sequence deviations to prevent false-positive 'desperation' matches. """ print(f"\n" + "="*80) print(f" SPATIAL INTERSECTION (ANTI-DESPERATION MODE) FOR {color_label}") print(f"="*80) indexed_cam1 = list(enumerate(cam1_pts)) indexed_cam2 = list(enumerate(cam2_pts)) # Sort left-to-right based on pixel x-coordinate sorted_cam1 = sorted(indexed_cam1, key=lambda x: x[1][3]) sorted_cam2 = sorted(indexed_cam2, key=lambda x: x[1][3]) num_1 = len(sorted_cam1) num_2 = len(sorted_cam2) print(f"[INFO] Cam1 visible cluster: {num_1} dots | Cam2 visible cluster: {num_2} dots") if num_1 == 0 or num_2 == 0: return [], ([], [], []) # Build world rays rays_1 = [{'origin': o, 'dir': d} for o, d in [pixel_to_ray_corrected(pt[3], pt[4], K1, D1, R_p1, t_p1) for _, pt in sorted_cam1]] rays_2 = [{'origin': o, 'dir': d} for o, d in [pixel_to_ray_corrected(pt[3], pt[4], K2, D2, R_p2, t_p2) for _, pt in sorted_cam2]] cost_matrix = np.full((num_1, num_2), 999.0) pts_3d_matrix = {} max_ray_miss_m = max_ray_miss_mm / 1000.0 expected_shift = max(0, num_2 - num_1) print(f"\n[COST MATRIX LOG] Evaluating pairs:") # Populate Cost Matrix for i in range(num_1): orig_1 = sorted_cam1[i][0] for j in range(num_2): orig_2 = sorted_cam2[j][0] # 1. Quick Rank Deviation Guard Rail rank_deviation = abs(j - (i + expected_shift)) if rank_deviation > max_rank_deviation: print(f" -> HARD REJECT: Cam1 #{orig_1} <-> Cam2 #{orig_2} | Deviation {rank_deviation} exceeds max cap ({max_rank_deviation})") continue # 2. Geometry Check pt_3d, err = intersect_rays([rays_1[i]['origin'], rays_2[j]['origin']], [rays_1[i]['dir'], rays_2[j]['dir']]) err_mm = err * 1000.0 if err <= max_ray_miss_m: penalty = rank_deviation * rank_weight total_cost = err + penalty cost_matrix[i, j] = total_cost pts_3d_matrix[(i, j)] = (pt_3d, err) print(f" -> PASS: Cam1 #{orig_1} (Sorted {i}) <-> Cam2 #{orig_2} (Sorted {j}) | Pure Miss: {err_mm:.2f} mm | Total Cost: {total_cost:.4f}") else: print(f" -> FAIL: Cam1 #{orig_1} <-> Cam2 #{orig_2} | Pure Miss {err_mm:.2f} mm exceeds spatial threshold.") # Global Linear Assignment row_ind, col_ind = linear_sum_assignment(cost_matrix) reconstructed_list = [] match_idx = 0 print(f"\n[MATCH LOG] Resolving Assignments:") for r, c in zip(row_ind, col_ind): total_cost = cost_matrix[r, c] orig_cam1_idx = sorted_cam1[r][0] orig_cam2_idx = sorted_cam2[c][0] # Guard against optimizer forcing a 999.0 unassigned placeholder link if total_cost < 999.0: pt_3d, pure_ray_miss = pts_3d_matrix[(r, c)] lbl = f"{color_label}_C1C2_{match_idx}" print(f" [MATCH ACCEPTED] {lbl:<10} | Cam1 #{orig_cam1_idx} -> Cam2 #{orig_cam2_idx} | Pure Miss: {pure_ray_miss*1000:.2f} mm") reconstructed_list.append({ 'point': pt_3d, 'label': lbl, 'err': pure_ray_miss, 'phone_key': None, 'cam1_key': (sorted_cam1[r][1][3], sorted_cam1[r][1][4]), 'cam2_key': (sorted_cam2[c][1][3], sorted_cam2[c][1][4]), 'rays': [rays_1[r], rays_2[c]] }) match_idx += 1 else: print(f" [MATCH DROPPED] Cam1 Label #{orig_cam1_idx} could not be safely matched.") print(f"\n[SUMMARY] Isolated {match_idx} reliable shared optodes.") return reconstructed_list, (rays_1, [], rays_2) def fuse_topological_inverse_variance(lists_of_matches): """ Fuses multi-view 3D reconstructions by tracking shared 2D pixel observations and weighting their 3D coordinates based on inverse ray-miss errors. """ # Flatten all matches from all pipelines into a master list all_matches = [] for match_list in lists_of_matches: if match_list: all_matches.extend(match_list) if not all_matches: return np.empty((0, 3)) num_matches = len(all_matches) visited = [False] * num_matches fused_points = [] print("\n[FUSION ENGINE] Resolving multi-view network topologies...") for i in range(num_matches): if visited[i]: continue # Start a new topological cluster for this physical optode cluster = [all_matches[i]] visited[i] = True # BFS/DFS expansion to find any other matches sharing ANY 2D camera points queue = [all_matches[i]] while queue: current = queue.pop(0) for j in range(num_matches): if not visited[j]: match_candidate = all_matches[j] # Check if they share the exact same physical detection on any camera shares_phone = (current['phone_key'] is not None and current['phone_key'] == match_candidate['phone_key']) shares_cam1 = (current['cam1_key'] is not None and current['cam1_key'] == match_candidate['cam1_key']) shares_cam2 = (current['cam2_key'] is not None and current['cam2_key'] == match_candidate['cam2_key']) if shares_phone or shares_cam1 or shares_cam2: visited[j] = True cluster.append(match_candidate) queue.append(match_candidate) # Calculate Inverse-Error Weighted Center Mass for the cluster weighted_sum = np.zeros(3) total_weight = 0.0 # Track which views contributed to this specific optode for debugging pipes_represented = len(cluster) for match in cluster: # Ray miss error acting as variance. Add a tiny epsilon to prevent division by zero. # A 0.8mm miss gets massive weight (~1250), a 4.0mm miss gets tiny weight (~250) weight = 1.0 / (match['err'] + 1e-6) weighted_sum += match['point'] * weight total_weight += weight final_pt = weighted_sum / total_weight fused_points.append(final_pt) if pipes_represented > 1: print(f" [CLIQUE CONFIRMED] Fused {pipes_represented} views. Weighted system pulled center toward highest-precision camera pair.") return np.array(fused_points) def draw_validated_rays_only(plotter, committed_matches, ray_color): """ Loops ONLY through verified matches that passed the filtering. Builds one combined multi-line PolyData per color instead of adding a separate VTK actor per ray (add_mesh has meaningful per-call overhead). """ if not committed_matches: return points = [] line_cells = [] for match in committed_matches: for ray in match['rays']: start = ray['origin'] end = match['point'] idx = len(points) points.append(start) points.append(end) line_cells.append(2) # number of points in this line segment line_cells.append(idx) line_cells.append(idx + 1) if not points: return poly = pv.PolyData(np.array(points)) poly.lines = np.array(line_cells) plotter.add_mesh(poly, color=ray_color, opacity=0.2, line_width=1.5) def visualize_fusion_results(greens_pairs, pinks_pairs, fused_greens, fused_pinks): """ Spawns a Matplotlib 3D window displaying raw pair locations vs final fused optodes, and prints out precise coordinates to the terminal. """ # --- TERMINAL PRINT OUTS --- print("\n" + "="*80) print(" 3D POSITION REPORT (ALL PAIRINGS VS FUSED)") print("="*80) raw_greens_dict = { 'Phone-Cam2': greens_pairs[0], 'Phone-Cam1': greens_pairs[1], 'Cam1-Cam2': greens_pairs[2] } raw_pinks_dict = { 'Phone-Cam2': pinks_pairs[0], 'Phone-Cam1': pinks_pairs[1], 'Cam1-Cam2': pinks_pairs[2] } def process_cluster(fused_list, raw_pairs_dict, color_name, marker_color): print(f"\n[{color_name.upper()} OPTODES]") for idx, fused in enumerate(fused_list): # --- SAFETY CHECK FOR DATA STRUCTURE --- if isinstance(fused, dict) and 'point' in fused: fused_pt = fused['point'] else: # If it's a direct array/list of coordinates [x, y, z] fused_pt = fused print(f" Fused Optode #{idx}: X={fused_pt[0]:.4f}, Y={fused_pt[1]:.4f}, Z={fused_pt[2]:.4f}") # Trace and plot raw inputs contributing to this specific fused point for pair_set_name, pair_list in raw_pairs_dict.items(): for item in pair_list: # Apply same structural protection to the raw pairs if isinstance(item, dict) and 'point' in item: raw_pt = item['point'] else: raw_pt = item dist = np.linalg.norm(np.array(fused_pt) - np.array(raw_pt)) if dist < 0.015: # within 15mm proximity print(f" <- Contributed by {pair_set_name:<15} | Raw: X={raw_pt[0]:.4f}, Y={raw_pt[1]:.4f}, Z={raw_pt[2]:.4f} | Offset: {dist*1000:.2f} mm") process_cluster(fused_greens, raw_greens_dict, "Green", "green") process_cluster(fused_pinks, raw_pinks_dict, "Pink", "deeppink") print("\n" + "="*80) def map_all_snirf_labels_to_3d(fused_pinks, fused_greens, src_pos, det_pos): """ Maps Sources and Detectors by selecting the 3D orientation that maximizes the structural correlation of the internal pairwise distance matrices. This guarantees that the known layout geometry (top/bottom, left/right) is perfectly preserved without relying on external anchors. """ if len(fused_pinks) == 0 or len(fused_greens) == 0: return [], [] # 1. Extract raw 3D coordinate matrices pinks_3d = np.array([p['point'] if isinstance(p, dict) else p for p in fused_pinks]) greens_3d = np.array([g['point'] if isinstance(g, dict) else g for g in fused_greens]) template_src = np.array(src_pos) template_det = np.array(det_pos) # 2. Project onto the shared 2D principal plane via SVD all_3d = np.vstack([pinks_3d, greens_3d]) global_mean_3d = np.mean(all_3d, axis=0) _, _, Vt = np.linalg.svd(all_3d - global_mean_3d) pinks_proj = (pinks_3d - global_mean_3d) @ Vt[:2, :].T greens_proj = (greens_3d - global_mean_3d) @ Vt[:2, :].T # 3. Normalize spaces to ensure consistent scaling factor evaluations all_proj = np.vstack([pinks_proj, greens_proj]) mean_proj = np.mean(all_proj, axis=0) std_proj = np.std(all_proj) + 1e-6 pinks_norm = (pinks_proj - mean_proj) / std_proj greens_norm = (greens_proj - mean_proj) / std_proj all_2d = np.vstack([template_src, template_det]) mean_2d = np.mean(all_2d, axis=0) std_2d = np.std(all_2d) + 1e-6 src_norm = (template_src - mean_2d) / std_2d det_norm = (template_det - mean_2d) / std_2d best_geom_score = -float('inf') best_pinks_labels = [] best_greens_labels = [] # Test all 8 potential coordinate frame reflections/rotations orientations = [ (1, 1, False), (1, -1, False), (-1, 1, False), (-1, -1, False), (1, 1, True), (1, -1, True), (-1, 1, True), (-1, -1, True) ] for sx, sy, swap in orientations: # Apply orientation transformation to candidate coordinates t_pinks = pinks_norm.copy() if swap: t_pinks = t_pinks[:, [1, 0]] t_pinks[:, 0] *= sx t_pinks[:, 1] *= sy dists_p = np.linalg.norm(t_pinks[:, None, :] - src_norm[None, :, :], axis=2) row_p, col_p = linear_sum_assignment(dists_p) t_greens = greens_norm.copy() if swap: t_greens = t_greens[:, [1, 0]] t_greens[:, 0] *= sx t_greens[:, 1] *= sy dists_g = np.linalg.norm(t_greens[:, None, :] - det_norm[None, :, :], axis=2) row_g, col_g = linear_sum_assignment(dists_g) pink_map = {r: c for r, c in zip(row_p, col_p)} green_map = {r: c for r, c in zip(row_g, col_g)} # 4. GEOMETRIC VALIDATION: Extract pairs to check internal distances assigned_tmpl_coords = [] assigned_3d_real_coords = [] for i in range(len(pinks_3d)): if i in pink_map: assigned_tmpl_coords.append(template_src[pink_map[i]]) assigned_3d_real_coords.append(pinks_3d[i]) for i in range(len(greens_3d)): if i in green_map: assigned_tmpl_coords.append(template_det[green_map[i]]) assigned_3d_real_coords.append(greens_3d[i]) if len(assigned_tmpl_coords) < 4: continue assigned_tmpl_coords = np.array(assigned_tmpl_coords) assigned_3d_real_coords = np.array(assigned_3d_real_coords) # Compute the pairwise distance fingerprints D_tmpl = cdist(assigned_tmpl_coords, assigned_tmpl_coords) D_3d_real = cdist(assigned_3d_real_coords, assigned_3d_real_coords) # Calculate Pearson correlation coefficient between the two matrices # If the grid is flipped/inverted, this correlation value collapses corr = np.corrcoef(D_tmpl.flatten(), D_3d_real.flatten())[0, 1] if corr > best_geom_score: best_geom_score = corr best_pinks_labels = [f"S{pink_map[i]+1}" if i in pink_map else "S?" for i in range(len(pinks_3d))] best_greens_labels = [f"D{green_map[i]+1}" if i in green_map else "D?" for i in range(len(greens_3d))] return best_pinks_labels, best_greens_labels # def transform_to_polhemus_space(fused_greens, fused_pinks, fused_reds, cam1_pos, cam2_pos): # """ # Transforms 3D world space points (meters) into Polhemus head space (millimeters) # using data-driven head axis extraction and strict user-defined constraints. # """ # if len(fused_reds) < 2: # print("[ERROR] Cannot register space: Need exactly 2 red fiducial points.") # return {}, {}, {} # # 1. Convert all points from meters to millimeters immediately # greens_mm = fused_greens * 1000.0 if fused_greens.size > 0 else np.empty((0, 3)) # pinks_mm = fused_pinks * 1000.0 if fused_pinks.size > 0 else np.empty((0, 3)) # reds_mm = fused_reds * 1000.0 # c1_mm = cam1_pos * 1000.0 # Back Camera # c2_mm = cam2_pos * 1000.0 # Left Camera # # 2. Identify tracked fiducials via camera proximity (Iz = Back, LPA = Left) # iz_idx = np.argmin(np.linalg.norm(reds_mm - c1_mm, axis=1)) # lpa_idx = np.argmin(np.linalg.norm(reds_mm - c2_mm, axis=1)) # if iz_idx == lpa_idx: # lpa_idx = 1 - iz_idx # P_iz = reds_mm[iz_idx] # P_lpa = reds_mm[lpa_idx] # # 3. Extract True Anatomical Axes from the Optode Cap Shape # # Combine all mapped optodes to find the structural center of the head # all_optodes = [] # if greens_mm.size > 0: all_optodes.append(greens_mm) # if pinks_mm.size > 0: all_optodes.append(pinks_mm) # if len(all_optodes) > 0: # centroid = np.mean(np.vstack(all_optodes), axis=0) # else: # centroid = np.mean(reds_mm, axis=0) # # Participant is upright -> vertical axis is locked to world Z # z_hat = np.array([0.0, 0.0, 1.0]) # # The forward axis (Y) points from the back skull marker (Iz) through the cap center # vec_forward = centroid - P_iz # vec_forward[2] = 0.0 # Keep it strictly horizontal # y_hat = vec_forward / np.linalg.norm(vec_forward) # # The right axis (X) is perpendicular to the longitudinal centerline # x_hat = np.cross(y_hat, z_hat) # x_hat = x_hat / np.linalg.norm(x_hat) # # 4. Calculate Coordinates Using Your Explicit Spatial Rules # # Project the left ear marker to find its true geometric offset from the midline # lpa_x_raw = np.dot(P_lpa - P_iz, x_hat) # lpa_y_raw = np.dot(P_lpa - P_iz, y_hat) # # Force LPA to the negative hemisphere, mirror it to get RPA # X_lpa = lpa_x_raw if lpa_x_raw < 0 else -lpa_x_raw # X_rpa = -X_lpa # # Solve for Iz's depth based on your hardcoded LPA Y baseline (-18) # Y_iz = -18.0 - lpa_y_raw # # Track the lowest detected point on the baseline to enforce the vertical floor # min_fid_z = min(P_iz[2], P_lpa[2]) # # 5. Map Optodes and Preserve Contour above the -45mm Floor # def convert_points(pts): # if pts.size == 0: # return np.empty((0, 3)) # transformed = np.zeros_like(pts) # for i, pt in enumerate(pts): # # Horizontal coordinates relative to the new centerline frame # x_val = np.dot(pt - P_iz, x_hat) # y_val = np.dot(pt - P_iz, y_hat) + Y_iz # # Distance scaling straight up from the lowest fiducial plane point # z_height_above_floor = pt[2] - min_fid_z # z_val = -45.0 + z_height_above_floor # transformed[i] = [x_val, y_val, z_val] # return transformed # head_greens = convert_points(greens_mm) # head_pinks = convert_points(pinks_mm) # # 6. Generate Hardcoded / Derived Output Template # derived_fiducials = { # "nz": np.array([0.0, 100.0, -45.0]), # "rpa": np.array([X_rpa, -18.0, -45.0]), # "lpa": np.array([X_lpa, -18.0, -45.0]), # "iz": np.array([0.0, Y_iz, -45.0]) # } # return head_greens, head_pinks, derived_fiducials # def print_polhemus_format(greens, pinks, fiducials): # print("\n--- FINAL POLHEMUS COORDINATES (mm) ---") # # Print Fiducials # for name in ["nz", "rpa", "lpa", "iz"]: # pt = fiducials[name] # print(f"{name}:\t{pt[0]:.2f}\t{pt[1]:.2f}\t{pt[2]:.2f}") # # Print Detectors (Green) # for i, pt in enumerate(greens): # print(f"d{i+1}:\t{pt[0]:.2f}\t{pt[1]:.2f}\t{pt[2]:.2f}") # # Print Sources (Pink) # for i, pt in enumerate(pinks): # print(f"s{i+1}:\t{pt[0]:.2f}\t{pt[1]:.2f}\t{pt[2]:.2f}") def fit_sphere_lsq(points): """ Algebraic least-squares sphere fit. Linearizes |P - C|^2 = r^2 -> |P|^2 = 2*P.C + (r^2 - |C|^2) Solves the linear system for C and r directly (no iteration needed). """ P = np.asarray(points, dtype=float) A = np.hstack([2 * P, np.ones((len(P), 1))]) b = np.sum(P**2, axis=1) sol, residuals, rank, sv = np.linalg.lstsq(A, b, rcond=None) C = sol[:3] r = np.sqrt(sol[3] + np.dot(C, C)) # Fit quality diagnostics — check before trusting this dists = np.linalg.norm(P - C, axis=1) fit_std = np.std(dists - r) return C, r, fit_std # def reconstruct_fiducials_v3(P_iz, P_lpa, optode_cloud, up=np.array([0.0, -1.0, 0.0])): # """ # Fits the head sphere using all real data (including Iz), but derives # orientation using a *virtual* Iz: same azimuthal bearing as the real # (camera-tracked) Iz, height-corrected to match LPA's level on the sphere. # This avoids Iz's low height from injecting spurious rotation into the # anatomical frame. # """ # all_points = np.vstack([optode_cloud, P_iz.reshape(1, 3), P_lpa.reshape(1, 3)]) # C_head, r_head, fit_std = fit_sphere_lsq(all_points) # # --- Diagnostic: how much does Iz actually influence the fit? --- # all_points_no_iz = np.vstack([optode_cloud, P_lpa.reshape(1, 3)]) # C_head_no_iz, r_head_no_iz, _ = fit_sphere_lsq(all_points_no_iz) # center_shift = np.linalg.norm(C_head - C_head_no_iz) # print(f"[DIAGNOSTIC] Center shift with/without Iz in fit: {center_shift:.4f} " # f"({100*center_shift/r_head:.1f}% of radius)") # # --- Interaural axis from LPA (unchanged, this part was never the problem) --- # x_hat = P_lpa - C_head # x_hat = x_hat - np.dot(x_hat, up) * up # x_hat /= np.linalg.norm(x_hat) # # Provisional horizontal axis, sign not yet resolved # z_hat_prov = np.cross(up, x_hat) # z_hat_prov /= np.linalg.norm(z_hat_prov) # # --- Resolve posterior sign using ONLY Iz's azimuth, not its height --- # v_iz = P_iz - C_head # z_comp = np.dot(v_iz, z_hat_prov) # posterior_sign = np.sign(z_comp) or 1.0 # z_hat = posterior_sign * z_hat_prov # now points toward the true "back" side # # --- Project LPA onto sphere, extract shared height offset --- # P_lpa_sphere = C_head + r_head * (P_lpa - C_head) / np.linalg.norm(P_lpa - C_head) # v_y = np.dot(P_lpa_sphere - C_head, up) # horiz_radius = np.sqrt(max(r_head**2 - v_y**2, 0.0)) # # --- Virtual Iz: real azimuth, LPA's height, on the sphere surface --- # P_iz_virtual = C_head + v_y * up + horiz_radius * z_hat # # --- NZ: opposite azimuth, same height --- # P_nz = C_head + v_y * up + horiz_radius * (-z_hat) # # --- Cz: top pole --- # P_cz = C_head + r_head * up # # --- RPA: mirror LPA across x_hat only, preserving height/depth --- # v = P_lpa_sphere - C_head # v_x, v_y2, v_z = np.dot(v, x_hat), np.dot(v, up), np.dot(v, z_hat) # P_rpa = C_head - v_x * x_hat + v_y2 * up + v_z * z_hat # # --- Pitch proxy: real Iz vs. virtual Iz height difference --- # # This is the head-pitch signal from your very first message, now # # falling out naturally instead of needing manual leveling. # pitch_offset = np.dot(P_iz - P_iz_virtual, up) # print(f"[DIAGNOSTIC] Real Iz vs virtual Iz height delta: {pitch_offset:.4f} " # f"(consistently negative across sessions = Iz sitting below LPA plane, " # f"as anatomically expected; large swings session-to-session may indicate pitch)") # return { # "C_head": C_head, "r_head": r_head, "fit_std": fit_std, # "P_lpa": P_lpa_sphere, "P_rpa": P_rpa, # "P_nz": P_nz, "P_cz": P_cz, "P_iz_virtual": P_iz_virtual, # "center_shift_from_iz": center_shift, # "pitch_offset": pitch_offset, # } def reconstruct_fiducials_v4(P_iz, P_lpa, optode_cloud): """ Reconstructs the missing fiducials (RPA, NZ, Cz) by deriving the true 3D anatomical orientation directly from tracked landmarks (P_lpa, P_iz). Completely removes reliance on world/camera 'up' vectors, allowing reconstructed points to perfectly follow real-world head pitch, roll, and yaw. """ # 1. Fit the head sphere using all available real physical points all_points = np.vstack([optode_cloud, P_iz.reshape(1, 3), P_lpa.reshape(1, 3)]) C_head, r_head, fit_std = fit_sphere_lsq(all_points) # 2. Define the true Anteroposterior (Y) direction directly from Iz # Vector from center to Iz points to the back (-Y) v_iz = P_iz - C_head dir_iz = v_iz / np.linalg.norm(v_iz) y_hat = -dir_iz # True positive AP axis pointing straight forward toward NZ # 3. Define the true Interaural (X) direction from LPA # Project LPA onto the plane orthogonal to our true AP axis v_lpa = P_lpa - C_head x_prov = v_lpa - np.dot(v_lpa, y_hat) * y_hat # User convention: LPA = -X, so negate to make x_hat point to RPA (+X) x_hat = -x_prov / np.linalg.norm(x_prov) # 4. Define the true Vertical (Z) direction using the head's actual anatomy # RPA (+X) x NZ (+Y) = Cz (+Z) z_hat = np.cross(x_hat, y_hat) z_hat /= np.linalg.norm(z_hat) # 5. Project landmarks onto the sphere surface in their true real-world locations P_lpa_sphere = C_head + r_head * (v_lpa / np.linalg.norm(v_lpa)) # NZ sits along the positive AP direction on the sphere surface P_nz = C_head + r_head * y_hat # Cz sits at the true top pole of the head sphere P_cz = C_head + r_head * z_hat # Virtual Iz sits perfectly opposite NZ on the sphere surface P_iz_virtual = C_head - r_head * y_hat # RPA mirrors LPA across the mid-sagittal (Y-Z) plane, preserving any physical height offsets v_lpa_rel = P_lpa_sphere - C_head P_rpa = (C_head - np.dot(v_lpa_rel, x_hat) * x_hat + np.dot(v_lpa_rel, y_hat) * y_hat + np.dot(v_lpa_rel, z_hat) * z_hat) return { "C_head": C_head, "r_head": r_head, "fit_std": fit_std, "P_lpa": P_lpa_sphere, "P_rpa": P_rpa, "P_nz": P_nz, "P_cz": P_cz, "P_iz_virtual": P_iz_virtual, "axes": (x_hat, y_hat, z_hat) } def drop_point_on_sphere(P, C_head, r_head, up, drop_amount): """ Lowers a point's height by drop_amount while keeping it on the sphere surface, preserving its azimuthal (horizontal) direction from center. """ v = P - C_head y = np.dot(v, up) horiz = v - y * up horiz_norm = np.linalg.norm(horiz) horiz_dir = horiz / horiz_norm if horiz_norm > 1e-9 else horiz new_y = y - drop_amount new_horiz_r_sq = r_head**2 - new_y**2 if new_horiz_r_sq < 0: print(f"[WARNING] Drop of {drop_amount} exceeds sphere geometry at this point — clamping.") new_horiz_r_sq = 0 new_horiz_r = np.sqrt(new_horiz_r_sq) return C_head + new_y * up + new_horiz_r * horiz_dir def apply_ear_canal_correction(fit, drop_amount=0.01, up=np.array([0.0, -1.0, 0.0])): """ Drops LPA, RPA, NZ by drop_amount (default 1cm) to correct for marker placement above the true ear canal, keeping all three on the sphere. Cz and virtual Iz are untouched since they weren't derived from LPA's marker height in the same way. """ C_head, r_head = fit["C_head"], fit["r_head"] P_lpa_corr = drop_point_on_sphere(fit["P_lpa"], C_head, r_head, up, drop_amount) P_rpa_corr = drop_point_on_sphere(fit["P_rpa"], C_head, r_head, up, drop_amount) # P_nz_corr = drop_point_on_sphere(fit["P_nz"], C_head, r_head, up, drop_amount) fit_corrected = dict(fit) # shallow copy, keep Cz/virtual Iz/etc unchanged fit_corrected["P_lpa"] = P_lpa_corr fit_corrected["P_rpa"] = P_rpa_corr # fit_corrected["P_nz"] = P_nz_corr return fit_corrected def sort_by_label_number(points, labels): """ Sorts points/labels so output order matches the numeric part of the label (D1, D2, ... or S1, S2, ...) instead of whatever order they arrived in from upstream fusion/matching. """ def numeric_key(lbl): m = re.search(r'(\d+)', lbl) return int(m.group(1)) if m else 0 order = sorted(range(len(labels)), key=lambda i: numeric_key(labels[i])) points_sorted = np.array([points[i] for i in order]) labels_sorted = [labels[i] for i in order] return points_sorted, labels_sorted # def build_canonical_transform(fit, up=np.array([0., -1., 0.])): # """ # Computes the local frame (center, axes, AP sign) once from the fiducials, # and returns a reusable transform function for any point or point cloud. # x_local: interaural, LPA = -x, RPA = +x # y_local: anteroposterior, NZ = +y, Iz = -y # z_local: vertical, Cz = +z # """ # C_head, r_head = fit["C_head"], fit["r_head"] # P_lpa, P_nz, P_iz_v = fit["P_lpa"], fit["P_nz"], fit["P_iz_virtual"] # x_hat = P_lpa - C_head # x_hat = x_hat - np.dot(x_hat, up) * up # x_hat /= np.linalg.norm(x_hat) # x_hat = -x_hat # z_hat = up / np.linalg.norm(up) # y_hat = np.cross(x_hat, z_hat) # y_hat /= np.linalg.norm(y_hat) # def raw_local(P): # v = P - C_head # return np.array([np.dot(v, x_hat), np.dot(v, y_hat), np.dot(v, z_hat)]) # # Determine AP sign once, using the fiducials # sign = 1.0 # if raw_local(P_nz)[1] < raw_local(P_iz_v)[1]: # sign = -1.0 # def to_local(P): # """Transform a single point or an (N,3) array of points into the local frame.""" # P = np.asarray(P) # if P.ndim == 1: # v = P - C_head # local = np.array([np.dot(v, x_hat), np.dot(v, y_hat), np.dot(v, z_hat)]) # local[1] *= sign # return local # else: # v = P - C_head # local = np.stack([v @ x_hat, v @ y_hat, v @ z_hat], axis=1) # local[:, 1] *= sign # return local # return {"to_local": to_local, "r_head": r_head, "sign": sign} def build_canonical_transform(fit): """ Transforms any real-world point cloud into the perfectly square, upright local canonical frame using the pre-computed anatomical axes. """ C_head = fit["C_head"] r_head = fit["r_head"] x_hat, y_hat, z_hat = fit["axes"] # Double check orientation sign with NZ sign = 1.0 v_nz = fit["P_nz"] - C_head if np.dot(v_nz, y_hat) < 0: sign = -1.0 def to_local(P): P = np.asarray(P) if P.ndim == 1: v = P - C_head local = np.array([np.dot(v, x_hat), np.dot(v, y_hat), np.dot(v, z_hat)]) local[1] *= sign return local else: v = P - C_head local = np.stack([v @ x_hat, v @ y_hat, v @ z_hat], axis=1) local[:, 1] *= sign return local return {"to_local": to_local, "r_head": r_head, "sign": sign} def elongate_ap(points_local, elongation_factor=1.15): """Applies AP (y) elongation to a single point or (N,3) array, in-place-safe copy.""" p = np.array(points_local, copy=True) if p.ndim == 1: p[1] *= elongation_factor else: p[:, 1] *= elongation_factor return p def build_final_transform(fit, up=np.array([0., -1., 0.]), ap_elongation=1.0, y_offset_mm=-50.0, global_scale=1.0): """ One function that combines: rotate into local frame -> AP elongation -> AP offset. Returns a single `transform(points)` callable used identically for fiducials and optodes, so the offset can never be applied inconsistently between them. Works on a single (3,) point or an (N,3) array. """ base_transform = build_canonical_transform(fit) to_local = base_transform["to_local"] r_head = base_transform["r_head"] M_TO_MM = 1000.0 def transform(points_world, in_mm=True): local = to_local(points_world) # rotate/center # local = elongate_ap(local, ap_elongation) # stretch AP axis # local = local * global_scale # uniform 20% shrink -> global_scale=0.8 local_mm = local * M_TO_MM if local_mm.ndim == 1: local_mm[1] += y_offset_mm else: local_mm[:, 1] += y_offset_mm return local_mm return {"transform": transform, "r_head": r_head * global_scale} def plot_canonical_fiducials(fit, sphere_glyph, optode_cloud_data=None, up=np.array([0., -1., 0.]), ap_elongation=1.0, y_offset_mm=-50.0, global_scale=1.0): final = build_final_transform(fit, up=up, ap_elongation=ap_elongation, y_offset_mm=y_offset_mm, global_scale=global_scale) transform, r_head = final["transform"], final["r_head"] fiducial_names = ["LPA", "RPA", "NZ", "Cz", "Iz"] fiducial_pts = [fit["P_lpa"], fit["P_rpa"], fit["P_nz"], fit["P_cz"], fit["P_iz_virtual"]] local = {name: transform(pt) for name, pt in zip(fiducial_names, fiducial_pts)} a = r_head * 1000 b = r_head * ap_elongation * 1000 c = r_head * 1000 print(f"\n[CANONICAL FRAME COORDINATES] (mm; AP elongation x{ap_elongation:.2f}, y_offset={y_offset_mm}mm)") print(f" Semi-axes (mm): x={a:.2f}, y={b:.2f}, z={c:.2f}") for name, pt in local.items(): print(f" {name:>4s}: x={pt[0]:+.2f}, y={pt[1]:+.2f}, z={pt[2]:+.2f}") print() p2 = pv.Plotter() # NOTE: ellipsoid center is offset along y to match the shifted points ellipsoid = pv.ParametricEllipsoid(xradius=a, yradius=b, zradius=c, u_res=32, v_res=32) ellipsoid.translate((0, y_offset_mm, 0), inplace=True) p2.add_mesh(ellipsoid, color="white", opacity=0.15, name="canonical_ellipsoid") colors = {"LPA": "#00ffff", "RPA": "#00ffff", "NZ": "#ffff00", "Cz": "#a855f7", "Iz": "#f97316"} for name, pt in local.items(): sph = pv.PolyData(pt.reshape(1, 3)).glyph(orient=False, scale=False, geom=sphere_glyph) p2.add_mesh(sph, color=colors[name], label=name) label_strings = [f"{name}\n({pt[0]:.1f}, {pt[1]:.1f}, {pt[2]:.1f}) mm" for name, pt in local.items()] p2.add_point_labels( np.array(list(local.values())), label_strings, font_size=12, text_color="white", point_color="#1e293b", always_visible=True, shadow=True, name="fiducial_labels_local" ) p2.add_mesh(pv.Line(local["Iz"], local["NZ"]), color="#ffff00", line_width=3) p2.add_mesh(pv.Line(local["LPA"], local["RPA"]), color="#00ffff", line_width=3) if optode_cloud_data: for points, labels, color, name in optode_cloud_data: if points is None or len(points) == 0: continue local_pts = transform(points) # same helper, same offset, guaranteed consistent cloud = pv.PolyData(local_pts) spheres = cloud.glyph(orient=False, scale=False, geom=sphere_glyph) p2.add_mesh(spheres, color=color, label=name) print(f"[{name.upper()} LOCAL POSITIONS] (mm)") for i, pt in enumerate(local_pts): lbl = labels[i] if labels is not None and i < len(labels) else f"#{i}" print(f" {lbl:>8s}: x={pt[0]:+.2f}, y={pt[1]:+.2f}, z={pt[2]:+.2f}") print() if labels is not None: # Label text now shows BOTH the name and its exact transformed # coordinates, directly on the point in the viewer — so you can # visually confirm the label sitting on a given sphere matches # what's printed in the text file for that same name. label_strings_optodes = [ f"{lbl}\n({pt[0]:.1f}, {pt[1]:.1f}, {pt[2]:.1f})" for lbl, pt in zip(labels, local_pts) ] p2.add_point_labels( local_pts, label_strings_optodes, font_size=10, text_color="white", point_color=color, always_visible=True, shadow=True, name=f"{name}_labels_local" ) p2.camera.up = (0, 0, 1) p2.camera_position = 'xz' p2.add_legend() p2.show_axes() p2.show_grid(color="#334155") p2.show() return local def write_fiducial_text_file(fit, optode_cloud_data, filepath, up=np.array([0., -1., 0.]), ap_elongation=1.0, y_offset_mm=-50.0, global_scale=1.0): final = build_final_transform(fit, up=up, ap_elongation=ap_elongation, y_offset_mm=y_offset_mm, global_scale=global_scale) transform = final["transform"] def fmt(pt): p = transform(pt) return f"{p[0]:.2f} {p[1]:.2f} {p[2]:.2f}" lines = [] lines.append(f"nz: {fmt(fit['P_nz'])}") lines.append(f"a1: {fmt(fit['P_lpa'])}") lines.append(f"a2: {fmt(fit['P_rpa'])}") lines.append(f"cz: {fmt(fit['P_cz'])}") lines.append(f"iz: {fmt(fit['P_iz_virtual'])}") for points, labels, color, name in optode_cloud_data: if points is None or len(points) == 0: continue if labels is not None: points, labels = sort_by_label_number(points, labels) # <-- add this line local_pts = transform(points) prefix = "d" if "green" in name.lower() else "s" if "pink" in name.lower() else name[0].lower() for i, pt in enumerate(local_pts): lines.append(f"{prefix}{i+1}: {pt[0]:.2f} {pt[1]:.2f} {pt[2]:.2f}") with open(filepath, "w") as f: f.write("\n".join(lines) + "\n") print(f"[WRITTEN] {filepath} (y_offset={y_offset_mm}mm applied to all points)") return filepath def plot_normalized_world(nz, iz, lpa, rpa, center, head, workspace_is_mm, sphere_glyph, final_fused_greens=None, green_labels=None, final_fused_pinks=None, pink_labels=None, output_filename="normalized_coordinates.txt"): """ Transforms landmarks and optodes using a strict 3D rigid coordinate transformation derived from LPA, RPA, and NZ to perfectly match the reference head coordinate system. """ # 1. Scaling factor scale_factor = 1.0 if workspace_is_mm else 1000.0 # 2. Extract raw arbitrary tracking coordinates P_nz_arb = nz.points[0] * scale_factor P_lpa_arb = lpa.points[0] * scale_factor P_rpa_arb = rpa.points[0] * scale_factor C_head_arb = np.array(head.center) * scale_factor radius = ((head.bounds[1] - head.bounds[0]) / 2.0) * scale_factor # 3. BUILD TRUE 3D BASIS VECTORS FROM LANDMARKS # X-Axis: Vector pointing from Left Ear to Right Ear v_X = P_rpa_arb - P_lpa_arb u_X = v_X / np.linalg.norm(v_X) # Find the physical midpoint between the ears in tracking space M_arb = (P_lpa_arb + P_rpa_arb) / 2.0 # Vector pointing from ear midpoint to the nose v_nz = P_nz_arb - M_arb # Z-Axis (Vertical Height): Perpendicular to the Ear-to-Ear and Ear-to-Nose plane v_Z = np.cross(u_X, v_nz) u_Z = v_Z / np.linalg.norm(v_Z) # Orientation Guard: Ensure Z points UP toward the top of the head if np.dot(C_head_arb - M_arb, u_Z) < 0: u_Z = -u_Z # Y-Axis (Anterior/Forward): Perpendicular to Z and X to complete right-handed system u_Y = np.cross(u_Z, u_X) # 4. RIGID 3D TRANSFORMATION HELPER def transform_points(points_array): if points_array is None or points_array.size == 0: return np.empty((0, 3)) # Scale and center relative to the arbitrary tracking ear-midpoint pts_scaled = np.atleast_2d(points_array) * scale_factor pts_rel = pts_scaled - M_arb # Project onto our 3D basis vectors and apply target standard displacements x_norm = (pts_rel @ u_X) + 15 y_norm = (pts_rel @ u_Y) - 35 # Lock ears to -18.72mm along Y z_norm = (pts_rel @ u_Z) - 10 # Lock ear/nose plane to -45.0mm along Z return np.column_stack([x_norm, y_norm, z_norm]) # Transform all landmarks cleanly through the 3D matrix P_nz_norm = transform_points(nz.points)[0] P_iz_norm = transform_points(iz.points)[0] P_lpa_norm = transform_points(lpa.points)[0] P_rpa_norm = transform_points(rpa.points)[0] P_center_norm = transform_points(center.points)[0] # Transform the dense optode clouds P_greens_norm = transform_points(final_fused_greens) P_pinks_norm = transform_points(final_fused_pinks) # 5. NATURAL SORTING PARSER def get_sorted_labeled_points(points, labels): if points is None or points.size == 0 or labels is None: return [] pairs = [(str(lbl).strip().lower(), coord) for coord, lbl in zip(points, labels)] def natural_sort_key(item): match = re.match(r"([a-z]+)(\d+)", item[0]) if match: return (match.group(1), int(match.group(2))) return (item[0], 0) return sorted(pairs, key=natural_sort_key) sorted_greens = get_sorted_labeled_points(P_greens_norm, green_labels) sorted_pinks = get_sorted_labeled_points(P_pinks_norm, pink_labels) # 6. EXPORT TO TEXT FILE (Omitting iz and center) try: with open(output_filename, "w", encoding="utf-8") as f: f.write(f"nz: {P_nz_norm[0]:.2f}\t{P_nz_norm[1]:.2f}\t{P_nz_norm[2]:.2f}\n") f.write(f"rpa: {P_rpa_norm[0]:.2f}\t{P_rpa_norm[1]:.2f}\t{P_rpa_norm[2]:.2f}\n") f.write(f"lpa: {P_lpa_norm[0]:.2f}\t{P_lpa_norm[1]:.2f}\t{P_lpa_norm[2]:.2f}\n") for label, coord in sorted_greens: f.write(f"{label}: {coord[0]:.2f}\t{coord[1]:.2f}\t{coord[2]:.2f}\n") for label, coord in sorted_pinks: f.write(f"{label}: {coord[0]:.2f}\t{coord[1]:.2f}\t{coord[2]:.2f}\n") print(f"\n[SUCCESS] Corrected 3D coordinates exported to: {output_filename}") except Exception as e: print(f"\n[ERROR] Failed to write coordinate file: {e}") # 7. VISUAL RENDERING (PyVista Plotter) plotter = pv.Plotter(title="Strict Reference Head System (3D Aligned)") nz_pt = pv.PolyData([P_nz_norm]) iz_pt = pv.PolyData([P_iz_norm]) lpa_pt = pv.PolyData([P_lpa_norm]) rpa_pt = pv.PolyData([P_rpa_norm]) center_pt = pv.PolyData([P_center_norm]) plotter.add_mesh(nz_pt.glyph(orient=False, scale=False, geom=sphere_glyph), color="yellow", label="nz") plotter.add_mesh(iz_pt.glyph(orient=False, scale=False, geom=sphere_glyph), color="orange", label="iz") plotter.add_mesh(lpa_pt.glyph(orient=False, scale=False, geom=sphere_glyph), color="red", label="lpa") plotter.add_mesh(rpa_pt.glyph(orient=False, scale=False, geom=sphere_glyph), color="lightblue", label="rpa") plotter.add_mesh(center_pt.glyph(orient=False, scale=False, geom=sphere_glyph), color="white", label="center") lbl_cfg = {"font_size": 10, "always_visible": True, "shadow": True, "point_size": 0} plotter.add_point_labels(nz_pt, [f"nz: ({P_nz_norm[0]:.1f}, {P_nz_norm[1]:.1f}, {P_nz_norm[2]:.1f})"], text_color="yellow", **lbl_cfg) plotter.add_point_labels(iz_pt, [f"iz: ({P_iz_norm[0]:.1f}, {P_iz_norm[1]:.1f}, {P_iz_norm[2]:.1f})"], text_color="orange", **lbl_cfg) plotter.add_point_labels(lpa_pt, [f"lpa: ({P_lpa_norm[0]:.1f}, {P_lpa_norm[1]:.1f}, {P_lpa_norm[2]:.1f})"], text_color="red", **lbl_cfg) plotter.add_point_labels(rpa_pt, [f"rpa: ({P_rpa_norm[0]:.1f}, {P_rpa_norm[1]:.1f}, {P_rpa_norm[2]:.1f})"], text_color="lightblue", **lbl_cfg) if P_greens_norm.size > 0: green_spheres = pv.PolyData(P_greens_norm).glyph(orient=False, scale=False, geom=sphere_glyph) plotter.add_mesh(green_spheres, color="#00ff00") plotter.add_point_labels(P_greens_norm, [str(l).strip().lower() for l in green_labels], font_size=12, text_color="white", point_color="#00ff00", always_visible=True, shadow=True, name="detector_labels") if P_pinks_norm.size > 0: pink_spheres = pv.PolyData(P_pinks_norm).glyph(orient=False, scale=False, geom=sphere_glyph) plotter.add_mesh(pink_spheres, color="#ff00ff") plotter.add_point_labels(P_pinks_norm, [str(l).strip().lower() for l in pink_labels], font_size=12, text_color="white", point_color="#ff00ff", always_visible=True, shadow=True, name="source_labels") # Wireframe Head Alignment Guide centered at (0,0,0) normalized_head = pv.Sphere(radius=radius, center=(0.0, 0.0, 0.0), theta_resolution=24, phi_resolution=24) plotter.add_mesh(normalized_head, color="white", opacity=0.10) plotter.add_axes() plotter.show_grid() plotter.show() # ====================================================================== # 4. RUN SYSTEM & VISUALIZE WITH LIVE RAY LINES # ====================================================================== def main(): phone_img, cam1_img, cam2_img = "Cam_3.jpg", "Cam_1.jpg", "Cam_2.jpg" # --- PARALLEL IMAGE PROCESSING --- # Launch 3 threads to process the images concurrently print_elapsed() print("[STEP 1] Extracting optode data from all cameras in parallel...") executor = ThreadPoolExecutor(max_workers=3) f_phone = executor.submit(extract_optode_data, phone_img) f_cam1 = executor.submit(extract_optode_data, cam1_img) f_cam2 = executor.submit(extract_optode_data, cam2_img) # 2. Kick off the heavy PyVista/VTK window generation on the main thread # While this runs, the OS will context-switch to let the 3 background threads work print("[STEP 2] Main thread initializing PyVista engine simultaneously...") plotter = pv.Plotter() plotter.background_color = "#0f172a" print_elapsed("Plotter Engine Ready") # 3. Gather results (this acts as our sync barrier) print("[STEP 3] Rejoining background data...") p_green, p_pink, p_red = f_phone.result() c1_green, c1_pink, c1_red = f_cam1.result() c2_green, c2_pink, c2_red = f_cam2.result() executor.shutdown(wait=False) print_elapsed("Image Extraction Completed & Unpacked") # save_labeled_debug_image(phone_img, p_green, p_pink, "DEBUG_CH_PHONE.jpg") # save_labeled_debug_image(cam1_img, c1_green, c1_pink, "DEBUG_CH_CAM1.jpg") # save_labeled_debug_image(cam2_img, c2_green, c2_pink, "DEBUG_CH_CAM2.jpg") # Phone <-> Camera 2 final_greens, raw_g_rays = match_and_reconstruct_2d_ordered("G", p_green, c2_green, TOLERANCE_METERS, RANK_PENALTY) final_pinks, raw_p_rays = match_and_reconstruct_2d_ordered("P", p_pink, c2_pink, TOLERANCE_METERS, RANK_PENALTY) final_reds, raw_r_rays = match_and_reconstruct_2d_ordered("R", p_red, c2_red, 0.2, 0) print_elapsed("Phone to Camera 2 Completed.") # Phone <-> Camera 1 final_greens_c1, raw_g_rays_c1 = match_and_reconstruct_cam1_right_anchored("G", p_green, c1_green, TOLERANCE_METERS2, RANK_PENALTY2) final_pinks_c1, raw_p_rays_c1 = match_and_reconstruct_cam1_right_anchored("P", p_pink, c1_pink, TOLERANCE_METERS2, RANK_PENALTY2) final_reds_c1, raw_r_rays_c1 = match_and_reconstruct_cam1_right_anchored("R", p_red, c1_red, 0.2, 0) print_elapsed("Phone to Camera 1 Completed.") # Camera 2 <-> Camera 1 or Camera 1 <-> Camera 2 final_greens_cross, _ = match_90_deg_spatial_with_right_bias("G", c1_green, c2_green) final_pinks_cross, _ = match_90_deg_spatial_with_right_bias("P", c1_pink, c2_pink,) final_reds_cross, _ = match_90_deg_spatial_with_right_bias("R", c1_red, c2_red,) print_elapsed("Camera 1 to Camera 2 Completed.") # ====================================================================== # 5. MULTI-VIEW 3D POINT FUSION & CANVAS VISUALIZATION # ====================================================================== print("\n[STEP 5] Fusing overlapping 3D blobs from multi-camera duos...") # Run fusion across all matching combinations final_fused_greens = fuse_topological_inverse_variance([final_greens, final_greens_c1, final_greens_cross]) final_fused_pinks = fuse_topological_inverse_variance([final_pinks, final_pinks_c1, final_pinks_cross]) final_fused_reds = fuse_topological_inverse_variance([final_reds, final_reds_c1, final_reds_cross]) print(f"\n[SUMMARY] Perfect Map Generated:") print(f" -> {len(final_fused_greens)} Clean Green Optodes mapped.") print(f" -> {len(final_fused_pinks)} Clean Pink Optodes mapped.") print(f" -> {len(final_fused_reds)} Clean Red Optodes mapped.") visualize_fusion_results( greens_pairs=[final_greens, final_greens_c1, final_greens_cross], pinks_pairs=[final_pinks, final_pinks_c1, final_pinks_cross], fused_greens=final_fused_greens, fused_pinks=final_fused_pinks ) # --- Initialize PyVista Clean Canvas --- print_elapsed() # Draw Camera reference positions (low-res spheres: these are small # reference markers, not the focus of the scene, so default 30x30 # subdivisions is wasted mesh-generation/render cost) plotter.add_mesh(pv.Sphere(radius=0.015, center=phone_pos, theta_resolution=12, phi_resolution=12), color="blue", label="Phone") plotter.add_mesh(pv.Sphere(radius=0.015, center=cam1_pos, theta_resolution=12, phi_resolution=12), color="yellow", label="Cam1 Hub") plotter.add_mesh(pv.Sphere(radius=0.015, center=cam2_pos, theta_resolution=12, phi_resolution=12), color="magenta", label="Cam2 Hub") print_elapsed("Mesh Completed.") # Optional: Draw the raw intersecting ray networks fading into their targets draw_validated_rays_only(plotter, final_greens, "#22c55e") draw_validated_rays_only(plotter, final_pinks, "#d946ef") draw_validated_rays_only(plotter, final_reds, "#e01616") draw_validated_rays_only(plotter, final_greens_c1, "#22c55e") draw_validated_rays_only(plotter, final_pinks_c1, "#d946ef") draw_validated_rays_only(plotter, final_reds_c1, "#e01616") draw_validated_rays_only(plotter, final_greens_cross, "#22c55e") draw_validated_rays_only(plotter, final_pinks_cross, "#d946ef") draw_validated_rays_only(plotter, final_reds_cross, "#e01616") print_elapsed("Rays Completed.") # Create a base sphere geometry to copy onto every point. Low resolution # since this shape gets glyphed onto every optode (~15-20 copies), so its # poly count directly multiplies across the whole scene. sphere_glyph = pv.Sphere(radius=0.005, theta_resolution=16, phi_resolution=16) file_path = SNIRF_FILE_PATH with h5py.File(file_path, "r") as f: # 1. Dynamically locate the primary nirs group nirs_key = [k for k in f.keys() if "nirs" in k][0] nirs = f[nirs_key] probe = nirs["probe"] # 2. Extract 2D Coordinates & Wavelengths src_pos = probe["sourcePos2D"][:, :2] det_pos = probe["detectorPos2D"][:, :2] pink_labels, green_labels = map_all_snirf_labels_to_3d( final_fused_pinks, final_fused_greens, src_pos, det_pos ) if final_fused_greens.size > 0: # Convert numpy array to PolyData green_points = pv.PolyData(final_fused_greens) # Glyph every point into a physical 3D sphere green_spheres = green_points.glyph(orient=False, scale=False, geom=sphere_glyph) plotter.add_mesh(green_spheres, color="#00ff00", label="Final Unified Green Optodes") plotter.add_point_labels( final_fused_greens, green_labels, font_size=12, text_color="white", point_color="#00ff00", always_visible=True, shadow=True, name="detector_labels" ) if final_fused_pinks.size > 0: pink_points = pv.PolyData(final_fused_pinks) pink_spheres = pink_points.glyph(orient=False, scale=False, geom=sphere_glyph) plotter.add_mesh(pink_spheres, color="#ff00ff", label="Final Unified Pink Optodes") plotter.add_point_labels( final_fused_pinks, pink_labels, font_size=12, text_color="white", point_color="#ff00ff", always_visible=True, shadow=True, name="source_labels" ) if final_fused_reds.size > 0: # 1. Render the raw tracked red dots using your low-poly glyph red_points = pv.PolyData(final_fused_reds) red_spheres = red_points.glyph(orient=False, scale=False, geom=sphere_glyph) plotter.add_mesh(red_spheres, color="#ff0000", label="Raw Tracked Red Fiducials") # 2. Scale alignment for the proximity check workspace_is_mm = np.max(np.abs(final_fused_reds)) > 10.0 c1_aligned = cam1_pos * 1000.0 if (workspace_is_mm and np.max(np.abs(cam1_pos)) < 10.0) else cam1_pos c2_aligned = cam2_pos * 1000.0 if (workspace_is_mm and np.max(np.abs(cam2_pos)) < 10.0) else cam2_pos # 3. Extract Iz and LPA 3D positions via camera proximity iz_idx = np.argmin(np.linalg.norm(final_fused_reds - c1_aligned, axis=1)) lpa_idx = np.argmin(np.linalg.norm(final_fused_reds - c2_aligned, axis=1)) if iz_idx == lpa_idx: lpa_idx = 1 - iz_idx P_iz = final_fused_reds[iz_idx] P_lpa_raw = final_fused_reds[lpa_idx] # 4. Gather the raw optode cloud and fit the global sphere optode_cloud = np.vstack([final_fused_greens, final_fused_pinks]) A = np.hstack([2 * optode_cloud, np.ones((len(optode_cloud), 1))]) b = np.sum(optode_cloud**2, axis=1) K, _, _, _ = np.linalg.lstsq(A, b, rcond=None) C_head = K[:3] r_optodes = np.sqrt(K[3] + np.sum(C_head**2)) offset_amount = 15.0 if workspace_is_mm else 0.015 r_head = r_optodes - offset_amount # 5. ORIGINAL HEIGHT PLANE (Orange / Yellow / Center Anchor) Y_target = P_lpa_raw[1] C_slice = np.array([C_head[0], Y_target, C_head[2]]) r_slice_sq = r_head**2 - (Y_target - C_head[1])**2 r_slice = np.sqrt(max(0, r_slice_sq)) # Calculate raw LPA offset from the original slice surface for consistency lpa_dist_from_center = np.linalg.norm(np.array([P_lpa_raw[0], 0.0, P_lpa_raw[2]]) - np.array([C_slice[0], 0.0, C_slice[2]])) lpa_offset = abs(lpa_dist_from_center - r_slice) # 6. EJECT & ALIGN ORANGE (Stays at original horizontal plane height) to_orange_vec_2d = np.array([P_iz[0] - C_head[0], 0.0, P_iz[2] - C_head[2]]) norm_2d = np.linalg.norm(to_orange_vec_2d) direction_2d = to_orange_vec_2d / norm_2d if norm_2d > 0 else np.array([1.0, 0.0, 0.0]) r_orange_target = r_slice + lpa_offset v_red_2d = np.array([P_lpa_raw[0] - C_slice[0], 0.0, P_lpa_raw[2] - C_slice[2]]) dir_red_2d = v_red_2d / np.linalg.norm(v_red_2d) rot_plus = np.array([-dir_red_2d[2], 0.0, dir_red_2d[0]]) rot_minus = np.array([dir_red_2d[2], 0.0, -dir_red_2d[0]]) dir_orange_ideal = rot_plus if np.dot(rot_plus, direction_2d) > np.dot(rot_minus, direction_2d) else rot_minus ang_orig = np.arctan2(direction_2d[2], direction_2d[0]) ang_ideal = np.arctan2(dir_orange_ideal[2], dir_orange_ideal[0]) ang_diff = np.arctan2(np.sin(ang_ideal - ang_orig), np.cos(ang_ideal - ang_orig)) max_movement = 10.0 if workspace_is_mm else 0.01 max_ang_move = max_movement / r_orange_target ang_move = np.sign(ang_diff) * min(abs(ang_diff), max_ang_move) ang_final = ang_orig + ang_move direction_2d_final = np.array([np.cos(ang_final), 0.0, np.sin(ang_final)]) P_iz_orange_outside = C_slice + (r_orange_target * direction_2d_final) P_yellow = 2 * C_slice - P_iz_orange_outside # Yellow stays opposite Orange # 7. DROPPED HEIGHT PLANE (LPA / RPA Ear Axis Shift) drop_val = 20.0 if workspace_is_mm else 0.02 Y_dropped = Y_target + drop_val # Calculate the narrowed sphere cross-section radius at the lower depth r_slice_dropped_sq = r_head**2 - (Y_dropped - C_head[1])**2 r_slice_dropped = np.sqrt(max(0, r_slice_dropped_sq)) r_lpa_target_dropped = r_slice_dropped + lpa_offset # Re-project LPA and RPA (Light Blue) down with the exact same spacing off the new surface boundary P_lpa_dropped = np.array([C_head[0], Y_dropped, C_head[2]]) + (r_lpa_target_dropped * dir_red_2d) P_lightblue_dropped = np.array([C_head[0], Y_dropped, C_head[2]]) - (r_lpa_target_dropped * dir_red_2d) # 8. Render All Dynamic Landings # Orange orange_point = pv.PolyData([P_iz_orange_outside]) plotter.add_mesh(orange_point.glyph(orient=False, scale=False, geom=sphere_glyph), color="orange", label="Aligned Iz (Orange)") # Yellow yellow_point = pv.PolyData([P_yellow]) plotter.add_mesh(yellow_point.glyph(orient=False, scale=False, geom=sphere_glyph), color="yellow", label="Opposite Orange (Yellow)") # Dropped Red LPA lpa_dropped_point = pv.PolyData([P_lpa_dropped]) plotter.add_mesh(lpa_dropped_point.glyph(orient=False, scale=False, geom=sphere_glyph), color="red", label="Dropped LPA (Red)") # Dropped Light Blue RPA lightblue_point = pv.PolyData([P_lightblue_dropped]) plotter.add_mesh(lightblue_point.glyph(orient=False, scale=False, geom=sphere_glyph), color="lightblue", label="Dropped RPA (Light Blue)") # Center White Anchor (Kept at the upper slice origin for structural visualization) white_point = pv.PolyData([C_slice]) plotter.add_mesh(white_point.glyph(orient=False, scale=False, geom=sphere_glyph), color="white", label="Slice Center (White)") # 9. Draw 3D Structural Mesh Lines # Radial Spokes (LPA/RPA lines now slope downward toward their dropped heights) plotter.add_mesh(pv.Line(P_lpa_dropped, C_slice), color="red", line_width=3) plotter.add_mesh(pv.Line(P_yellow, C_slice), color="yellow", line_width=3) plotter.add_mesh(pv.Line(P_lightblue_dropped, C_slice), color="lightblue", line_width=3) plotter.add_mesh(pv.Line(P_iz_orange_outside, C_slice), color="orange", line_width=3) # Perimeter Contour Frame (Traces a 3D cradle around the skull geometry) plotter.add_mesh(pv.Line(P_iz_orange_outside, P_lpa_dropped), color="white", line_width=2) plotter.add_mesh(pv.Line(P_lpa_dropped, P_yellow), color="white", line_width=2) plotter.add_mesh(pv.Line(P_yellow, P_lightblue_dropped), color="white", line_width=2) plotter.add_mesh(pv.Line(P_lightblue_dropped, P_iz_orange_outside), color="white", line_width=2) # 10. Render Head Mesh head_sphere = pv.Sphere(radius=r_head, center=C_head, theta_resolution=24, phi_resolution=24) plotter.add_mesh(head_sphere, color="white", opacity=0.15, name="fitted_head_sphere") # 11. Console Logs print("\n=== ORANGE LANDMARK ADJUSTMENT ===") print(f"Required shift to hit 90°: {r_orange_target * abs(ang_diff):.2f} mm") print(f"Actual distance moved: {r_orange_target * abs(ang_move):.2f} mm") print(f"LPA/RPA vertical drop applied: {drop_val:.2f} mm") plot_normalized_world( nz=yellow_point, iz=orange_point, lpa=lpa_dropped_point, rpa=lightblue_point, center=white_point, head=head_sphere, workspace_is_mm=workspace_is_mm, sphere_glyph=sphere_glyph, final_fused_greens=final_fused_greens, green_labels=green_labels, final_fused_pinks=final_fused_pinks, pink_labels=pink_labels ) # fit = reconstruct_fiducials_v4(P_iz, P_lpa, optode_cloud) # fit = apply_ear_canal_correction(fit, drop_amount=0.02) # 1cm marker correction # plot_canonical_fiducials( # fit, sphere_glyph, # optode_cloud_data=[ # (final_fused_greens, green_labels, "#00ff00", "Green Optodes"), # (final_fused_pinks, pink_labels, "#ff00ff", "Pink Optodes"), # ], # up=np.array([0., -1., 0.]), # ap_elongation=1, # y_offset_mm=-50.0, # global_scale=1.0, # ) # write_fiducial_text_file( # fit, # optode_cloud_data=[ # (final_fused_greens, green_labels, "#00ff00", "green_optodes"), # (final_fused_pinks, pink_labels, "#ff00ff", "pink_optodes"), # ], # filepath=r"C:\Users\PsychLab\Documents\tyler\research\lights\fiducials_optodes.txt", # up=np.array([0., -1., 0.]), # ap_elongation=1, # y_offset_mm=-50.0, # global_scale=1.0, # ) # C_head, r_head, fit_std = fit["C_head"], fit["r_head"], fit["fit_std"] # P_lpa_s, P_rpa, P_nz, P_cz, P_iz_v = ( # fit["P_lpa"], fit["P_rpa"], fit["P_nz"], fit["P_cz"], fit["P_iz_virtual"] # ) # print(f"[HEAD FIT] center={C_head}, radius={r_head:.4f}, fit_std={fit_std:.4f}") # print(f"[CHECK] LPA-RPA={np.linalg.norm(P_lpa_s-P_rpa):.4f}, " # f"NZ-IzVirtual={np.linalg.norm(P_nz-P_iz_v):.4f}") # for pt, color, label in [ # (P_lpa_s, "#00ffff", "LPA (corrected)"), # (P_rpa, "#00ffff", "Generated RPA"), # (P_nz, "#ffff00", "Generated NZ"), # (P_cz, "#a855f7", "Generated Cz"), # (P_iz_v, "#f97316", "Virtual Iz (leveled)"), # ]: # sph = pv.PolyData(pt.reshape(1, 3)).glyph(orient=False, scale=False, geom=sphere_glyph) # plotter.add_mesh(sph, color=color, label=label) # plotter.add_point_labels( # np.array([P_iz, P_lpa_s, P_rpa, P_nz, P_cz, P_iz_v]), # ["Iz (real)", "LPA", "RPA", "NZ", "Cz", "Iz (virtual)"], # font_size=12, text_color="white", # point_color="#1e293b", always_visible=True, # shadow=True, name="fiducial_labels" # ) # plotter.add_mesh(pv.Line(P_iz, P_lpa_s), color="#ef4444", line_width=4, name="hypotenuse_line") # plotter.add_mesh(pv.Line(P_lpa_s, P_rpa), color="#00ffff", line_width=3, name="coronal_axis") # plotter.add_mesh(pv.Line(P_iz, P_nz), color="#ffff00", line_width=3, name="sagittal_axis") print_elapsed("All Completed.") #TODO: Un-hardcode this. plotter.camera_position = [ (0, -1, -1.5), (0.0, 0.0, 0.45), # Focal Point (looking right at the cluster of optodes) (0.0, -1.0, 0.0) # View-up vector (adjust based on your coordinate system orientation) ] plotter.camera.up = (0, -1, 0) plotter.add_legend() plotter.show_axes() plotter.show_grid(color="#334155") print_elapsed() print("\n[Part B] Spawning Part C independently before opening 3D view...") script_c_path = os.path.join(os.path.dirname(__file__), "part_C.py") current_dir = os.path.dirname(os.path.abspath(__file__)) venv_python = os.path.join(current_dir, ".venv", "Scripts", "python.exe") cmd_args = [venv_python, "-u", script_c_path, "--start-time", str(APP_START_TIME)] if SNIRF_FILE_PATH: cmd_args.extend(["--snirf", str(SNIRF_FILE_PATH)]) subprocess.Popen( cmd_args, stdout=sys.stdout, stderr=sys.stderr, cwd=os.path.join(current_dir, "..") ) print_elapsed() plotter.show() # plotter2 = pv.Plotter(title="Canonical Grid-Aligned Head Space") # # 2. Derive the scale radius and vertical baseline center # R = L / 2.0 # center_y = (P_iz[1] + P_lpa[1]) / 2.0 # Center height baseline # # 3. Apply your exact alignment rules to the X-Z floor plane: # # - Intersection Center is locked to (0, 0) # # - NZ is purely forward (+Z direction, X = 0) # # - LPA is purely to the left (-X direction, Z = 0) # # Format: [X, Y, Z] # P_iz_aligned = np.array([0.0, P_iz[1] - center_y, -R]) # P_nz_aligned = np.array([0.0, P_nz[1] - center_y, R]) # P_lpa_aligned = np.array([-R, P_lpa[1] - center_y, 0.0]) # P_rpa_aligned = np.array([R, P_rpa[1] - center_y, 0.0]) # # Helper function to render the aligned meshes # def add_canonical_node(plotter_inst, coordinates, mesh_color, label_name): # point_data = pv.PolyData(coordinates.reshape(1, 3)) # glyph_mesh = point_data.glyph(orient=False, scale=False, geom=sphere_glyph) # plotter_inst.add_mesh(glyph_mesh, color=mesh_color, label=label_name) # # 4. Render the perfectly grid-aligned points # add_canonical_node(plotter2, P_iz_aligned, "#ff0000", "Aligned Iz") # add_canonical_node(plotter2, P_lpa_aligned, "#ff0000", "Aligned LPA") # add_canonical_node(plotter2, P_rpa_aligned, "#00ffff", "Aligned RPA") # add_canonical_node(plotter2, P_nz_aligned, "#ffff00", "Aligned NZ") # # 5. Draw the structural straight-line crosshairs # plotter2.add_mesh(pv.Line(P_lpa_aligned, P_rpa_aligned), color="#00ffff", line_width=4, name="grid_coronal") # plotter2.add_mesh(pv.Line(P_iz_aligned, P_nz_aligned), color="#ffff00", line_width=4, name="grid_sagittal") # # 6. Display coordinate labels to verify perfect alignment values # grid_labels = [ # f"Iz (0, 0, {-R:.3f})", # f"LPA ({-R:.3f}, 0, 0)", # f"RPA ({R:.3f}, 0, 0)", # f"NZ (0, 0, {R:.3f})" # ] # plotter2.add_point_labels( # np.array([P_iz_aligned, P_lpa_aligned, P_rpa_aligned, P_nz_aligned]), # grid_labels, # font_size=11, text_color="white", # point_color="#0f172a", always_visible=True, # shadow=True, name="canonical_labels" # ) # # 7. Add visual reference tools to highlight grid placement # plotter2.show_grid(color="#334155", xtitle="Left / Right (X)", ztitle="Back / Forward (Z)", ytitle="Height (Y)") # plotter2.add_axes(line_width=3) # # 8. Force the camera to look straight down at the X-Z floor wall structure # plotter2.camera.up = (0, -1, 0) # plotter2.reset_camera() # # Render the second canvas window alongside the original tracker # plotter2.show() if __name__ == "__main__": parser = argparse.ArgumentParser() parser.add_argument("--start-time", type=float, default=None) parser.add_argument("--snirf", type=str, default=None) args = parser.parse_args() # Use the passed float or fall back to your performance counter APP_START_TIME = args.start_time if args.start_time is not None else time.perf_counter() SNIRF_FILE_PATH = args.snirf print(SNIRF_FILE_PATH) main()