|
| 1 | +import numpy as np |
| 2 | + |
| 3 | + |
| 4 | +class CalibrationEllipse: |
| 5 | + def __init__(self, n_std_devs=2.5): |
| 6 | + self.xs = [] |
| 7 | + self.ys = [] |
| 8 | + self.n_std_devs = float(n_std_devs) |
| 9 | + self.fitted = False |
| 10 | + |
| 11 | + self.scale_factor = 0.80 |
| 12 | + |
| 13 | + self.flip_y = False # Set to True if up/down are backwards |
| 14 | + self.flip_x = False # Adjust if left/right are backwards |
| 15 | + |
| 16 | + # Ellipse parameters |
| 17 | + self.center = None # Mean pupil position (ellipse center) |
| 18 | + self.axes = None # Semi-axes (std_dev based) |
| 19 | + self.rotation = None # Rotation angle |
| 20 | + self.evecs = None # Eigenvectors |
| 21 | + |
| 22 | + def add_sample(self, x, y): |
| 23 | + self.xs.append(float(x)) |
| 24 | + self.ys.append(float(y)) |
| 25 | + self.fitted = False |
| 26 | + |
| 27 | + def set_inset_percent(self, percent_smaller=0.0): |
| 28 | + clamped_percent = np.clip(percent_smaller, 0.0, 100.0) |
| 29 | + self.scale_factor = 1.0 - (clamped_percent / 100.0) |
| 30 | + |
| 31 | + def init_from_save(self, evecs, axes): |
| 32 | + """Initialize calibration from saved data with validation""" |
| 33 | + try: |
| 34 | + evecs_array = np.asarray(evecs, dtype=float) |
| 35 | + axes_array = np.asarray(axes, dtype=float) |
| 36 | + |
| 37 | + # Validate evecs shape |
| 38 | + if evecs_array.shape != (2, 2): |
| 39 | + print( |
| 40 | + f"\033[91m[ERROR] Invalid evecs shape in saved data: {evecs_array.shape}. Expected (2, 2).\033[0m" |
| 41 | + ) |
| 42 | + self.fitted = False |
| 43 | + return False |
| 44 | + |
| 45 | + # Validate axes shape |
| 46 | + if axes_array.shape != (2,): |
| 47 | + print(f"\033[91m[ERROR] Invalid axes shape in saved data: {axes_array.shape}. Expected (2,).\033[0m") |
| 48 | + self.fitted = False |
| 49 | + return False |
| 50 | + |
| 51 | + # Check for zero or invalid values |
| 52 | + if np.all(axes_array == 0) or np.any(np.isnan(axes_array)) or np.any(np.isnan(evecs_array)): |
| 53 | + print("\033[91m[ERROR] Saved calibration data contains zero or NaN values.\033[0m") |
| 54 | + self.fitted = False |
| 55 | + return False |
| 56 | + |
| 57 | + self.evecs = evecs_array |
| 58 | + self.axes = axes_array |
| 59 | + self.fitted = True |
| 60 | + return True |
| 61 | + |
| 62 | + except (ValueError, TypeError) as e: |
| 63 | + print(f"\033[91m[ERROR] Failed to load calibration data: {e}\033[0m") |
| 64 | + self.fitted = False |
| 65 | + return False |
| 66 | + |
| 67 | + def fit_ellipse(self): |
| 68 | + N = len(self.xs) |
| 69 | + if N < 2: |
| 70 | + print("Warning: Need >= 2 samples to fit PCA. Fit failed.") |
| 71 | + self.fitted = False |
| 72 | + return 0, 0 |
| 73 | + |
| 74 | + points = np.column_stack([self.xs, self.ys]) |
| 75 | + self.center = np.mean(points, axis=0) |
| 76 | + centered_points = points - self.center |
| 77 | + |
| 78 | + cov = np.cov(centered_points, rowvar=False) |
| 79 | + |
| 80 | + try: |
| 81 | + evals_cov, evecs_cov = np.linalg.eigh(cov) |
| 82 | + except np.linalg.LinAlgError: |
| 83 | + self.fitted = False |
| 84 | + return 0, 0 |
| 85 | + |
| 86 | + # Sort eigenvectors by alignment with screen axes (X, Y) |
| 87 | + x_alignment = np.abs(evecs_cov[0, :]) # How much each evec points in X direction |
| 88 | + |
| 89 | + if x_alignment[0] > x_alignment[1]: |
| 90 | + # evec 0 is more X-aligned, evec 1 is more Y-aligned |
| 91 | + self.evecs = evecs_cov |
| 92 | + std_devs = np.sqrt(evals_cov) |
| 93 | + else: |
| 94 | + # evec 1 is more X-aligned, swap them |
| 95 | + self.evecs = evecs_cov[:, [1, 0]] |
| 96 | + std_devs = np.sqrt(evals_cov[[1, 0]]) |
| 97 | + |
| 98 | + # --- FIX STARTS HERE --- |
| 99 | + # 1. Ensure the X-aligned eigenvector points Right (Positive X) |
| 100 | + if self.evecs[0, 0] < 0: |
| 101 | + self.evecs[:, 0] *= -1 |
| 102 | + |
| 103 | + # 2. Ensure Y-aligned eigenvector maintains a Right-Handed Coordinate System. |
| 104 | + # Instead of checking Y-sign independently, check the Determinant. |
| 105 | + # In screen coords (Y down), X=(1,0) and Y=(0,1) gives det = 1. |
| 106 | + # If det < 0, the axes are mirrored; we flip Y to fix it. |
| 107 | + det = (self.evecs[0, 0] * self.evecs[1, 1]) - (self.evecs[0, 1] * self.evecs[1, 0]) |
| 108 | + |
| 109 | + if det < 0: |
| 110 | + self.evecs[:, 1] *= -1 |
| 111 | + # --- FIX ENDS HERE --- |
| 112 | + |
| 113 | + self.axes = std_devs * self.n_std_devs |
| 114 | + |
| 115 | + if self.axes[0] < 1e-12: |
| 116 | + self.axes[0] = 1e-12 |
| 117 | + if self.axes[1] < 1e-12: |
| 118 | + self.axes[1] = 1e-12 |
| 119 | + |
| 120 | + major_index = np.argmax(std_devs) |
| 121 | + major_vec = self.evecs[:, major_index] |
| 122 | + self.rotation = np.arctan2(major_vec[1], major_vec[0]) |
| 123 | + |
| 124 | + self.fitted = True |
| 125 | + return self.evecs, self.axes |
| 126 | + |
| 127 | + def fit_and_visualize(self): |
| 128 | + try: |
| 129 | + import matplotlib.pyplot as plt |
| 130 | + except ImportError: |
| 131 | + print("\033[91m[ERROR] matplotlib is required for visualization.\033[0m") |
| 132 | + return |
| 133 | + |
| 134 | + plt.figure(figsize=(10, 8)) |
| 135 | + plt.plot(self.xs, self.ys, "k.", label="Calibration Samples", alpha=0.5, markersize=8) |
| 136 | + plt.axis("equal") |
| 137 | + plt.grid(True, alpha=0.3) |
| 138 | + plt.xlabel("Pupil X (pixels)") |
| 139 | + plt.ylabel("Pupil Y (pixels)") |
| 140 | + |
| 141 | + if not self.fitted: |
| 142 | + self.fit_ellipse() |
| 143 | + |
| 144 | + if self.fitted: |
| 145 | + scaled_axes = self.axes * self.scale_factor |
| 146 | + |
| 147 | + t = np.linspace(0, 2 * np.pi, 200) |
| 148 | + local_coords = np.column_stack([scaled_axes[0] * np.cos(t), scaled_axes[1] * np.sin(t)]) |
| 149 | + world_coords = (self.evecs @ local_coords.T).T + self.center |
| 150 | + |
| 151 | + plt.plot( |
| 152 | + world_coords[:, 0], |
| 153 | + world_coords[:, 1], |
| 154 | + "b-", |
| 155 | + linewidth=2, |
| 156 | + label=f"Calibration Ellipse ({self.scale_factor * 100:.0f}% scale)", |
| 157 | + ) |
| 158 | + plt.plot(self.center[0], self.center[1], "r+", markersize=15, markeredgewidth=3, label="Ellipse Center (Mean)") |
| 159 | + |
| 160 | + # Draw principal axes |
| 161 | + for i, (axis_len, color, name) in enumerate( |
| 162 | + [(scaled_axes[0], "g", "Major"), (scaled_axes[1], "m", "Minor")] |
| 163 | + ): |
| 164 | + axis_vec = self.evecs[:, i] * axis_len |
| 165 | + plt.arrow( |
| 166 | + self.center[0], |
| 167 | + self.center[1], |
| 168 | + axis_vec[0], |
| 169 | + axis_vec[1], |
| 170 | + head_width=5, |
| 171 | + head_length=7, |
| 172 | + fc=color, |
| 173 | + ec=color, |
| 174 | + alpha=0.6, |
| 175 | + label=f"{name} Axis", |
| 176 | + ) |
| 177 | + |
| 178 | + plt.title(f"Eye Tracking Calibration Ellipse (PCA, {self.n_std_devs}σ)") |
| 179 | + else: |
| 180 | + plt.title("Ellipse Fit FAILED (Not enough points)") |
| 181 | + |
| 182 | + plt.legend() |
| 183 | + plt.tight_layout() |
| 184 | + plt.show() |
| 185 | + |
| 186 | + def normalize(self, pupil_pos, target_pos=None, clip=True): |
| 187 | + if not self.fitted: |
| 188 | + return 0.0, 0.0 |
| 189 | + |
| 190 | + if self.evecs is None or self.axes is None: |
| 191 | + print("\033[91m[ERROR] Calibration data (evecs/axes) is None. Please calibrate.\033[0m") |
| 192 | + return 0.0, 0.0 |
| 193 | + |
| 194 | + if not isinstance(self.evecs, np.ndarray) or self.evecs.shape != (2, 2): |
| 195 | + print(f"\033[91m[ERROR] Invalid evecs shape. Expected (2, 2). Please recalibrate.\033[0m") |
| 196 | + return 0.0, 0.0 |
| 197 | + |
| 198 | + if not isinstance(self.axes, np.ndarray) or self.axes.shape != (2,): |
| 199 | + print(f"\033[91m[ERROR] Invalid axes shape. Expected (2,). Please recalibrate.\033[0m") |
| 200 | + return 0.0, 0.0 |
| 201 | + |
| 202 | + if np.all(self.axes == 0) or np.any(np.isnan(self.axes)): |
| 203 | + print("\033[91m[ERROR] Calibration axes are zero or invalid. Please recalibrate.\033[0m") |
| 204 | + return 0.0, 0.0 |
| 205 | + |
| 206 | + x, y = float(pupil_pos[0]), float(pupil_pos[1]) |
| 207 | + p = np.array([x, y], dtype=float) |
| 208 | + |
| 209 | + if target_pos is None: |
| 210 | + reference = self.center |
| 211 | + else: |
| 212 | + reference = np.asarray(target_pos, dtype=float) |
| 213 | + |
| 214 | + p_centered = p - reference |
| 215 | + |
| 216 | + try: |
| 217 | + p_rot = self.evecs.T @ p_centered |
| 218 | + except (ValueError, TypeError) as e: |
| 219 | + print(f"\033[91m[ERROR] Matrix multiplication failed in normalize: {e}. Please recalibrate.\033[0m") |
| 220 | + return 0.0, 0.0 |
| 221 | + |
| 222 | + scaled_axes = self.axes * self.scale_factor |
| 223 | + scaled_axes[scaled_axes < 1e-12] = 1e-12 |
| 224 | + |
| 225 | + norm = p_rot / scaled_axes |
| 226 | + |
| 227 | + norm_x = -norm[0] if self.flip_x else norm[0] |
| 228 | + norm_y = norm[1] if self.flip_y else -norm[1] |
| 229 | + |
| 230 | + if clip: |
| 231 | + norm_x = np.clip(norm_x, -1.0, 1.0) |
| 232 | + norm_y = np.clip(norm_y, -1.0, 1.0) |
| 233 | + |
| 234 | + return float(norm_x), float(norm_y) |
| 235 | + |
| 236 | + def denormalize(self, norm_x, norm_y, target_pos=None): |
| 237 | + if not self.fitted: |
| 238 | + print("ERROR: Ellipse not fitted yet.") |
| 239 | + return 0.0, 0.0 |
| 240 | + |
| 241 | + nx = -norm_x if self.flip_x else norm_x |
| 242 | + ny = norm_y if self.flip_y else -norm_y |
| 243 | + |
| 244 | + scaled_axes = self.axes * self.scale_factor |
| 245 | + p_rot = np.array([nx, ny]) * scaled_axes |
| 246 | + |
| 247 | + p_centered = self.evecs @ p_rot |
| 248 | + reference = self.center if target_pos is None else np.asarray(target_pos, dtype=float) |
| 249 | + p = p_centered + reference |
| 250 | + |
| 251 | + return float(p[0]), float(p[1]) |
| 252 | + |
0 commit comments