2156 lines
88 KiB
Python
2156 lines
88 KiB
Python
"""
|
|
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() |