mirror of
https://github.com/EyeTrackVR/EyeTrackVR.git
synced 2025-11-04 14:39:42 +08:00
116 lines
3.7 KiB
Python
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]) |