EyeTrackVR/EyeTrackApp/utils/calibration_elipse.py
2025-10-30 15:38:30 -05:00

116 lines
3.7 KiB
Python

import numpy as np
import matplotlib.pyplot as plt
class CalibrationEllipse:
def __init__(self, n_std_devs=2.5):
self.xs = []
self.ys = []
self.n_std_devs = float(n_std_devs)
self.fitted = False
self.scale_factor = 0.85 #TODO Test different values
# Ellipse parameters
self.center = None # (x0,y0) - mean of the point cloud
self.axes = None # (a, b) semi-axes (N*std_dev AT 100% SCALE)
self.rotation = None # angle in radians (from PCA)
self.evecs = None # Eigenvectors (principal axes directions)
def add_sample(self, x, y):
self.xs.append(float(x))
self.ys.append(float(y))
self.fitted = False
def set_inset_percent(self, percent_smaller=0.0):
clamped_percent = np.clip(percent_smaller, 0.0, 100.0)
self.scale_factor = 1.0 - (clamped_percent / 100.0)
print(f"Set inset to {clamped_percent}%. New scale_factor: {self.scale_factor}")
def fit_ellipse(self):
N = len(self.xs)
if N < 2:
print("Warning: Need >= 2 samples to fit PCA. Fit failed.")
self.fitted = False
return
points = np.column_stack([self.xs, self.ys])
self.center = np.mean(points, axis=0)
centered_points = points - self.center
cov = np.cov(centered_points, rowvar=False)
try:
evals_cov, evecs_cov = np.linalg.eigh(cov)
except np.linalg.LinAlgError as e:
print(f"PCA Eigen-decomposition failed: {e}")
self.fitted = False
return
self.evecs = evecs_cov
std_devs = np.sqrt(evals_cov)
self.axes = std_devs * self.n_std_devs
if self.axes[0] < 1e-12: self.axes[0] = 1e-12
if self.axes[1] < 1e-12: self.axes[1] = 1e-12
major_index = np.argmax(evals_cov)
major_vec = self.evecs[:, major_index]
self.rotation = np.arctan2(major_vec[1], major_vec[0])
self.fitted = True
def fit_and_visualize(self): # Helper function for debug
plt.figure(figsize=(10, 8))
plt.plot(self.xs, self.ys, 'k.', label='All Samples', alpha=0.3)
plt.axis('equal')
plt.grid(True)
plt.xlabel('X')
plt.ylabel('Y')
if not self.fitted:
self.fit_ellipse()
if self.fitted:
scaled_axes = self.axes * self.scale_factor
t = np.linspace(0, 2 * np.pi, 200)
local_coords = np.column_stack([scaled_axes[0] * np.cos(t),
scaled_axes[1] * np.sin(t)])
world_coords = (self.evecs @ local_coords.T).T + self.center
plt.plot(world_coords[:, 0], world_coords[:, 1], 'b-', linewidth=2, label=f'Fitted Ellipse ({self.scale_factor*100:.0f}% size)')
plt.plot(self.center[0], self.center[1], 'b+', markersize=15, label=f'Fitted Center (Mean)')
plt.title(f'Successful Robust Fit (PCA, {self.n_std_devs} std devs)')
else:
plt.title("Robust Fit FAILED (Not enough points)")
plt.legend()
plt.show()
def normalize(self, point, center_point, clip=True):
if not self.fitted:
print("Ellipse not fitted yet. Call fit_ellipse() or fit_and_visualize().")
return 0,0
x, y = float(point[0]), float(point[1])
p = np.array([x, y], dtype=float)
p_centered = p - np.asarray(center_point, dtype=float)
p_rot = self.evecs.T @ p_centered
scaled_axes = self.axes * self.scale_factor
scaled_axes[scaled_axes < 1e-12] = 1e-12
norm = p_rot / scaled_axes
if clip:
norm = np.clip(norm, -1.0, 1.0)
return float(norm[0]), float(norm[1])