diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml new file mode 100644 index 0000000..19039c3 --- /dev/null +++ b/.github/workflows/ci.yml @@ -0,0 +1,37 @@ +name: CI + +on: + push: + branches: [main, master] + pull_request: + +env: + CARGO_TERM_COLOR: always + RUSTFLAGS: -D warnings + +jobs: + test: + name: build / test / lint + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + + - name: Install stable toolchain + uses: dtolnay/rust-toolchain@stable + with: + components: clippy, rustfmt + + - name: Cache cargo registry and target + uses: Swatinem/rust-cache@v2 + + - name: rustfmt + run: cargo fmt --all --check + + - name: clippy + run: cargo clippy --workspace --all-targets -- -D warnings + + - name: build + run: cargo build --workspace --all-targets + + - name: test + run: cargo test --workspace diff --git a/Cargo.lock b/Cargo.lock index a56c276..e2f1f40 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -10,6 +10,12 @@ version = "0.1.0" name = "capture-preview" version = "0.1.0" +[[package]] +name = "equivalent" +version = "1.0.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "877a4ace8713b0bcf2a4e7eec82529c029f1d0619886d18145fea96c3ffe5c0f" + [[package]] name = "etk-capture" version = "0.1.0" @@ -17,6 +23,11 @@ version = "0.1.0" [[package]] name = "etk-config" version = "0.1.0" +dependencies = [ + "serde", + "thiserror", + "toml", +] [[package]] name = "etk-geom" @@ -42,6 +53,172 @@ version = "0.1.0" name = "eye-tracker" version = "0.1.0" +[[package]] +name = "hashbrown" +version = "0.17.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ed5909b6e89a2db4456e54cd5f673791d7eca6732202bbf2a9cc504fe2f9b84a" + +[[package]] +name = "indexmap" +version = "2.14.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d466e9454f08e4a911e14806c24e16fba1b4c121d1ea474396f396069cf949d9" +dependencies = [ + "equivalent", + "hashbrown", +] + +[[package]] +name = "memchr" +version = "2.8.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6b947ae49db0d222b1dbc6b113ce7248a3fc3a6ca21b696717bfc000ba4484d8" + +[[package]] +name = "proc-macro2" +version = "1.0.106" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8fd00f0bb2e90d81d1044c2b32617f68fcb9fa3bb7640c23e9c748e53fb30934" +dependencies = [ + "unicode-ident", +] + +[[package]] +name = "quote" +version = "1.0.45" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "41f2619966050689382d2b44f664f4bc593e129785a36d6ee376ddf37259b924" +dependencies = [ + "proc-macro2", +] + +[[package]] +name = "serde" +version = "1.0.228" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9a8e94ea7f378bd32cbbd37198a4a91436180c5bb472411e48b5ec2e2124ae9e" +dependencies = [ + "serde_core", + "serde_derive", +] + +[[package]] +name = "serde_core" +version = "1.0.228" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "41d385c7d4ca58e59fc732af25c3983b67ac852c1a25000afe1175de458b67ad" +dependencies = [ + "serde_derive", +] + +[[package]] +name = "serde_derive" +version = "1.0.228" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d540f220d3187173da220f885ab66608367b6574e925011a9353e4badda91d79" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "serde_spanned" +version = "0.6.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bf41e0cfaf7226dca15e8197172c295a782857fcb97fad1808a166870dee75a3" +dependencies = [ + "serde", +] + +[[package]] +name = "syn" +version = "2.0.117" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e665b8803e7b1d2a727f4023456bbbbe74da67099c585258af0ad9c5013b9b99" +dependencies = [ + "proc-macro2", + "quote", + "unicode-ident", +] + +[[package]] +name = "thiserror" +version = "2.0.18" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4288b5bcbc7920c07a1149a35cf9590a2aa808e0bc1eafaade0b80947865fbc4" +dependencies = [ + "thiserror-impl", +] + +[[package]] +name = "thiserror-impl" +version = "2.0.18" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ebc4ee7f67670e9b64d05fa4253e753e016c6c95ff35b89b7941d6b856dec1d5" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "toml" +version = "0.8.23" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dc1beb996b9d83529a9e75c17a1686767d148d70663143c7854d8b4a09ced362" +dependencies = [ + "serde", + "serde_spanned", + "toml_datetime", + "toml_edit", +] + +[[package]] +name = "toml_datetime" +version = "0.6.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "22cddaf88f4fbc13c51aebbf5f8eceb5c7c5a9da2ac40a13519eb5b0a0e8f11c" +dependencies = [ + "serde", +] + +[[package]] +name = "toml_edit" +version = "0.22.27" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "41fe8c660ae4257887cf66394862d21dbca4a6ddd26f04a3560410406a2f819a" +dependencies = [ + "indexmap", + "serde", + "serde_spanned", + "toml_datetime", + "toml_write", + "winnow", +] + +[[package]] +name = "toml_write" +version = "0.1.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5d99f8c9a7727884afe522e9bd5edbfc91a3312b36a77b5fb8926e4c31a41801" + [[package]] name = "track-preview" version = "0.1.0" + +[[package]] +name = "unicode-ident" +version = "1.0.24" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e6e4313cd5fcd3dad5cafa179702e2b244f760991f45397d14d4ebf38247da75" + +[[package]] +name = "winnow" +version = "0.7.15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "df79d97927682d2fd8adb29682d1140b343be4ac0f08fd68b7765d9c059d3945" +dependencies = [ + "memchr", +] diff --git a/crates/etk-config/Cargo.toml b/crates/etk-config/Cargo.toml index a2f11a8..7db4cc9 100644 --- a/crates/etk-config/Cargo.toml +++ b/crates/etk-config/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "etk-config" -description = "Config loading + live-reload via serde + notify" +description = "Config data model + TOML loading (live-reload watcher wired in the daemon)" version.workspace = true edition.workspace = true license.workspace = true @@ -8,4 +8,9 @@ repository.workspace = true authors.workspace = true [dependencies] -# scaffold — see PLAN.md §2.5 +# Config (de)serialisation — see PLAN.md §2.5. +# `notify` live-reload wiring lives in the daemon's event loop (TODO in lib.rs), +# so it is not pulled in here yet. +serde = { workspace = true } +toml = { workspace = true } +thiserror = { workspace = true } diff --git a/crates/etk-config/src/lib.rs b/crates/etk-config/src/lib.rs index 1c955e5..3c2ff98 100644 --- a/crates/etk-config/src/lib.rs +++ b/crates/etk-config/src/lib.rs @@ -1,8 +1,244 @@ //! Config loading + live-reload. //! -//! Watches `~/.config/eye-tracker-rust/config.toml` via `notify`. -//! Reloads smoothing params and calibration matrix without restarting capture. +//! Defines the typed [`Config`] tree that the daemon reads from +//! `~/.config/eye-tracker-rust/config.toml`, plus helpers to load it from a +//! path or a string. Every field has a documented default drawn from +//! `PLAN.md`, so a missing or partial file still yields a working config. //! -//! See `PLAN.md` §2.5 (Config + hot reload). +//! Live reload (watching the file via `notify` and pushing updated smoothing +//! params / calibration into the running pipeline) is a daemon-event-loop +//! concern — see the `TODO` on [`config_path`]. This crate provides the data +//! model and parsing; the watcher is wired up in the `eye-tracker` binary. //! -//! Status: scaffold only. +//! See `PLAN.md` §2.5 (Config + hot reload). + +use std::path::{Path, PathBuf}; + +use serde::{Deserialize, Serialize}; + +/// Errors from loading or parsing config. +#[derive(Debug, thiserror::Error)] +pub enum ConfigError { + /// The config file could not be read. + #[error("reading config file {path}: {source}")] + Io { + /// The path that failed to read. + path: PathBuf, + /// The underlying I/O error. + #[source] + source: std::io::Error, + }, + /// The config file was not valid TOML / did not match the schema. + #[error("parsing config: {0}")] + Parse(#[from] toml::de::Error), +} + +/// Top-level configuration. +#[derive(Debug, Clone, PartialEq, Default, Serialize, Deserialize)] +#[serde(default, deny_unknown_fields)] +pub struct Config { + /// UDP capture settings. + pub capture: CaptureConfig, + /// One-Euro cursor smoothing (`PLAN.md` §6 Phase 7). + pub smoothing: SmoothingConfig, + /// Dwell-to-click behaviour (`PLAN.md` §6 Phase 7). + pub dwell: DwellConfig, + /// Binocular fusion (`PLAN.md` §6 Phase 6). + pub fusion: FusionConfig, +} + +/// UDP capture configuration (`PLAN.md` §2.1, §6 Phase 1). +#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)] +#[serde(default, deny_unknown_fields)] +pub struct CaptureConfig { + /// UDP port the left-eye module streams to. + pub left_port: u16, + /// UDP port the right-eye module streams to. + pub right_port: u16, + /// Per-eye ring-buffer capacity in frames (`PLAN.md` §1 ArrayQueue cap=4). + pub ring_capacity: usize, +} + +impl Default for CaptureConfig { + fn default() -> Self { + Self { + left_port: 5005, + right_port: 5006, + ring_capacity: 4, + } + } +} + +/// One-Euro filter parameters. +/// +/// Defaults match `PLAN.md` §6 Phase 7 (`min_cutoff = 1.0`, `beta = 0.007`). +/// These map directly onto [`etk_geom::OneEuro::with_params`] in the +/// `etk-geom` crate. +#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)] +#[serde(default, deny_unknown_fields)] +pub struct SmoothingConfig { + /// Minimum cutoff frequency (Hz). + pub min_cutoff: f64, + /// Speed coefficient. + pub beta: f64, + /// Derivative cutoff frequency (Hz). + pub d_cutoff: f64, +} + +impl Default for SmoothingConfig { + fn default() -> Self { + Self { + min_cutoff: 1.0, + beta: 0.007, + d_cutoff: 1.0, + } + } +} + +/// Dwell-click configuration. +/// +/// Defaults match `PLAN.md` §6 Phase 7: "hold within 30 px circle for 0.7s +/// → emit `BTN_LEFT`". +#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)] +#[serde(default, deny_unknown_fields)] +pub struct DwellConfig { + /// Whether dwell-click is enabled. + pub enabled: bool, + /// Radius (screen px) the gaze must stay within to count as dwelling. + pub radius_px: f64, + /// Hold time (seconds) before a click is emitted. + pub hold_secs: f64, +} + +impl Default for DwellConfig { + fn default() -> Self { + Self { + enabled: true, + radius_px: 30.0, + hold_secs: 0.7, + } + } +} + +/// Binocular fusion configuration. +/// +/// Default `min_confidence` matches `PLAN.md` §6 Phase 6 ("if one eye's +/// confidence < 0.6, ignore"). +#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)] +#[serde(default, deny_unknown_fields)] +pub struct FusionConfig { + /// Confidence-weighted binocular average if `true`; dominant-eye only if + /// `false` (the Tobii Eye X strategy, `PLAN.md` §7 risk 5). + pub binocular: bool, + /// Minimum per-eye confidence to include an eye in the fusion. + pub min_confidence: f64, +} + +impl Default for FusionConfig { + fn default() -> Self { + Self { + binocular: true, + min_confidence: 0.6, + } + } +} + +impl Config { + /// Parse a [`Config`] from a TOML string. + /// + /// Missing tables/fields fall back to their documented defaults. + pub fn from_toml_str(s: &str) -> Result { + Ok(toml::from_str(s)?) + } + + /// Serialise this config to a pretty TOML string. + pub fn to_toml_string(&self) -> String { + // `Config` is a plain struct of primitives; serialisation cannot fail. + toml::to_string_pretty(self).expect("Config serialises to TOML") + } + + /// Load a [`Config`] from a file path. + /// + /// If the file does not exist, returns [`Config::default`] rather than an + /// error — a fresh install with no config file should still run. + pub fn load(path: impl AsRef) -> Result { + let path = path.as_ref(); + match std::fs::read_to_string(path) { + Ok(s) => Self::from_toml_str(&s), + Err(e) if e.kind() == std::io::ErrorKind::NotFound => Ok(Self::default()), + Err(source) => Err(ConfigError::Io { + path: path.to_path_buf(), + source, + }), + } + } +} + +/// The canonical config path: `$XDG_CONFIG_HOME/eye-tracker-rust/config.toml`, +/// falling back to `$HOME/.config/eye-tracker-rust/config.toml`. +/// +/// Returns `None` if neither `XDG_CONFIG_HOME` nor `HOME` is set. +/// +/// TODO(Phase 7): the daemon should `notify`-watch this path and hot-reload +/// [`SmoothingConfig`], [`DwellConfig`], and the calibration matrix without +/// restarting capture (`PLAN.md` §2.5). The watcher belongs in the +/// `eye-tracker` binary's event loop, not in this data-model crate. +pub fn config_path() -> Option { + let base = std::env::var_os("XDG_CONFIG_HOME") + .map(PathBuf::from) + .or_else(|| std::env::var_os("HOME").map(|h| PathBuf::from(h).join(".config")))?; + Some(base.join("eye-tracker-rust").join("config.toml")) +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn defaults_match_plan() { + let c = Config::default(); + assert_eq!(c.smoothing.min_cutoff, 1.0); + assert_eq!(c.smoothing.beta, 0.007); + assert_eq!(c.dwell.radius_px, 30.0); + assert_eq!(c.dwell.hold_secs, 0.7); + assert_eq!(c.fusion.min_confidence, 0.6); + assert_eq!(c.capture.ring_capacity, 4); + } + + #[test] + fn empty_string_yields_defaults() { + let c = Config::from_toml_str("").unwrap(); + assert_eq!(c, Config::default()); + } + + #[test] + fn partial_config_merges_with_defaults() { + let c = Config::from_toml_str("[smoothing]\nbeta = 0.05\n").unwrap(); + assert_eq!(c.smoothing.beta, 0.05); // overridden + assert_eq!(c.smoothing.min_cutoff, 1.0); // default retained + assert_eq!(c.dwell.hold_secs, 0.7); // unrelated table defaulted + } + + #[test] + fn round_trips_through_toml() { + let mut c = Config::default(); + c.smoothing.beta = 0.02; + c.dwell.enabled = false; + c.capture.left_port = 7000; + let s = c.to_toml_string(); + let back = Config::from_toml_str(&s).unwrap(); + assert_eq!(c, back); + } + + #[test] + fn unknown_field_is_rejected() { + let err = Config::from_toml_str("[smoothing]\nbogus = 1\n"); + assert!(err.is_err()); + } + + #[test] + fn load_missing_file_returns_default() { + let c = Config::load("/nonexistent/eye-tracker-rust/config.toml").unwrap(); + assert_eq!(c, Config::default()); + } +} diff --git a/crates/etk-geom/src/ellipse.rs b/crates/etk-geom/src/ellipse.rs new file mode 100644 index 0000000..ae81e56 --- /dev/null +++ b/crates/etk-geom/src/ellipse.rs @@ -0,0 +1,119 @@ +//! Pupil ellipse parameterisation. +//! +//! `EllSeg`'s regression head emits an ellipse directly; this type is the +//! interchange representation the geometry and mapping stages consume. +//! (`PLAN.md` §6 Phase 3.) + +use crate::Point; + +/// An axis-parameterised 2D ellipse. +/// +/// `angle` is the rotation of the major axis in radians, measured +/// counter-clockwise from the positive `x` axis. `a` and `b` are the +/// semi-axis lengths; by convention `a` is the semi-major axis (`a >= b`), +/// but the type does not enforce this so it can faithfully round-trip +/// whatever the regression head produces. +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct Ellipse { + /// Centre of the ellipse. + pub center: Point, + /// First semi-axis length (semi-major by convention). + pub a: f64, + /// Second semi-axis length (semi-minor by convention). + pub b: f64, + /// Rotation of the `a` axis, radians, CCW from +x. + pub angle: f64, +} + +impl Ellipse { + /// Construct an ellipse from centre, semi-axes, and rotation. + #[inline] + pub const fn new(center: Point, a: f64, b: f64, angle: f64) -> Self { + Self { + center, + a, + b, + angle, + } + } + + /// Area of the ellipse, `π·a·b`. + #[inline] + pub fn area(&self) -> f64 { + std::f64::consts::PI * self.a * self.b + } + + /// Eccentricity in `[0, 1)`. + /// + /// Returns `0` for a circle. Computed from the larger and smaller of the + /// two semi-axes so it is independent of which axis is stored in `a`. + pub fn eccentricity(&self) -> f64 { + let major = self.a.abs().max(self.b.abs()); + let minor = self.a.abs().min(self.b.abs()); + if major == 0.0 { + return 0.0; + } + let ratio = minor / major; + (1.0 - ratio * ratio).max(0.0).sqrt() + } + + /// Sample the ellipse boundary at parameter `t` (radians). + /// + /// `t` runs `0..2π` around the un-rotated ellipse before the `angle` + /// rotation and centre translation are applied. + pub fn point_at(&self, t: f64) -> Point { + let (st, ct) = t.sin_cos(); + let ux = self.a * ct; + let uy = self.b * st; + let (sa, ca) = self.angle.sin_cos(); + Point::new( + self.center.x + ux * ca - uy * sa, + self.center.y + ux * sa + uy * ca, + ) + } +} + +#[cfg(test)] +mod tests { + use super::*; + + fn approx(a: f64, b: f64) { + assert!((a - b).abs() < 1e-9, "{a} != {b}"); + } + + #[test] + fn circle_has_zero_eccentricity() { + let e = Ellipse::new(Point::new(0.0, 0.0), 5.0, 5.0, 0.0); + approx(e.eccentricity(), 0.0); + } + + #[test] + fn area_matches_formula() { + let e = Ellipse::new(Point::new(1.0, 2.0), 3.0, 2.0, 0.0); + approx(e.area(), std::f64::consts::PI * 6.0); + } + + #[test] + fn eccentricity_is_axis_order_independent() { + let wide = Ellipse::new(Point::new(0.0, 0.0), 4.0, 2.0, 0.0); + let tall = Ellipse::new(Point::new(0.0, 0.0), 2.0, 4.0, 0.0); + approx(wide.eccentricity(), tall.eccentricity()); + } + + #[test] + fn point_at_zero_is_on_major_axis() { + let e = Ellipse::new(Point::new(10.0, 20.0), 4.0, 2.0, 0.0); + let p = e.point_at(0.0); + approx(p.x, 14.0); + approx(p.y, 20.0); + } + + #[test] + fn point_at_respects_rotation() { + // 90° rotation: the +a axis now points along +y. + let e = Ellipse::new(Point::new(0.0, 0.0), 4.0, 2.0, std::f64::consts::FRAC_PI_2); + let p = e.point_at(0.0); + approx(p.x, 0.0); + approx(p.y, 4.0); + } +} diff --git a/crates/etk-geom/src/homography.rs b/crates/etk-geom/src/homography.rs new file mode 100644 index 0000000..1557c22 --- /dev/null +++ b/crates/etk-geom/src/homography.rs @@ -0,0 +1,301 @@ +//! 2D homography for gaze-feature → screen mapping. +//! +//! `PLAN.md` §6 Phase 3 & 5: a 9-point calibration fits a planar homography +//! that maps a gaze feature (glint-relative pupil position, in normalised +//! camera coordinates) onto screen pixels. +//! +//! A homography has 8 degrees of freedom and needs ≥4 point correspondences; +//! the 3×3 grid gives 9, so the system is over-determined and solved in the +//! least-squares sense. We use the Direct Linear Transform: each +//! correspondence contributes two rows to a linear system in the 8 unknowns +//! (`h33` is fixed to 1), which is then solved via the normal equations +//! (`AᵀA h = Aᵀb`) with a small hand-rolled Gaussian-elimination solver — no +//! external linear-algebra dependency, keeping this crate light per +//! `PLAN.md` §2 (lean deps). + +use crate::Point; + +/// Errors that can arise while fitting or applying a homography. +#[derive(Debug, Clone, PartialEq, Eq)] +pub enum HomographyError { + /// Fewer than 4 correspondences were supplied (8 DoF needs ≥4 points). + TooFewPoints { + /// Number of correspondences provided. + got: usize, + }, + /// `src` and `dst` slices had different lengths. + LengthMismatch { + /// Length of the source slice. + src: usize, + /// Length of the destination slice. + dst: usize, + }, + /// The normal-equations system was singular (degenerate / collinear + /// calibration points). No unique homography exists. + Singular, +} + +impl std::fmt::Display for HomographyError { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + match self { + HomographyError::TooFewPoints { got } => { + write!(f, "need at least 4 correspondences, got {got}") + } + HomographyError::LengthMismatch { src, dst } => { + write!(f, "src/dst length mismatch: {src} vs {dst}") + } + HomographyError::Singular => { + write!(f, "degenerate correspondences: system is singular") + } + } + } +} + +impl std::error::Error for HomographyError {} + +/// A 2D projective transform stored as a row-major 3×3 matrix with `h[8] = 1`. +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct Homography { + /// Row-major 3×3 matrix entries `[h11, h12, h13, h21, h22, h23, h31, h32, h33]`. + pub h: [f64; 9], +} + +impl Homography { + /// The identity homography. + pub const fn identity() -> Self { + Self { + h: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], + } + } + + /// Apply the homography to a point. + /// + /// Returns `None` if the projective denominator collapses to zero (point + /// maps to infinity). + pub fn map(&self, p: Point) -> Option { + let h = &self.h; + let w = h[6] * p.x + h[7] * p.y + h[8]; + if w.abs() < 1e-12 { + return None; + } + Some(Point::new( + (h[0] * p.x + h[1] * p.y + h[2]) / w, + (h[3] * p.x + h[4] * p.y + h[5]) / w, + )) + } + + /// Fit a homography from `src → dst` correspondences (least squares, DLT). + /// + /// Requires `src.len() == dst.len() >= 4`. For the 9-point calibration in + /// `PLAN.md` §6 Phase 5 this is called with 9 correspondences. + pub fn fit(src: &[Point], dst: &[Point]) -> Result { + if src.len() != dst.len() { + return Err(HomographyError::LengthMismatch { + src: src.len(), + dst: dst.len(), + }); + } + if src.len() < 4 { + return Err(HomographyError::TooFewPoints { got: src.len() }); + } + + // Build the 2N×8 system A·h = b, with h = [h11..h32], h33 = 1. + // x' = (h11·x + h12·y + h13) / (h31·x + h32·y + 1) + // y' = (h21·x + h22·y + h23) / (h31·x + h32·y + 1) + // Rearranged to linear form: + // h11·x + h12·y + h13 - h31·x·x' - h32·y·x' = x' + // h21·x + h22·y + h23 - h31·x·y' - h32·y·y' = y' + let n = src.len(); + let mut a = vec![[0.0f64; 8]; 2 * n]; + let mut b = vec![0.0f64; 2 * n]; + + for i in 0..n { + let (x, y) = (src[i].x, src[i].y); + let (xp, yp) = (dst[i].x, dst[i].y); + + let r0 = 2 * i; + a[r0] = [x, y, 1.0, 0.0, 0.0, 0.0, -x * xp, -y * xp]; + b[r0] = xp; + + let r1 = 2 * i + 1; + a[r1] = [0.0, 0.0, 0.0, x, y, 1.0, -x * yp, -y * yp]; + b[r1] = yp; + } + + // Normal equations: (AᵀA) h = Aᵀb → 8×8 symmetric system. + let mut ata = [[0.0f64; 8]; 8]; + let mut atb = [0.0f64; 8]; + for row in 0..2 * n { + for i in 0..8 { + atb[i] += a[row][i] * b[row]; + for j in 0..8 { + ata[i][j] += a[row][i] * a[row][j]; + } + } + } + + let sol = solve_8x8(ata, atb).ok_or(HomographyError::Singular)?; + + Ok(Homography { + h: [ + sol[0], sol[1], sol[2], sol[3], sol[4], sol[5], sol[6], sol[7], 1.0, + ], + }) + } +} + +impl Default for Homography { + fn default() -> Self { + Self::identity() + } +} + +/// Solve an 8×8 linear system `A x = b` via Gaussian elimination with partial +/// pivoting. Returns `None` if the matrix is singular. +fn solve_8x8(mut a: [[f64; 8]; 8], mut b: [f64; 8]) -> Option<[f64; 8]> { + const N: usize = 8; + for col in 0..N { + // Partial pivot: find the row with the largest magnitude in `col`. + let (pivot, best) = a + .iter() + .enumerate() + .skip(col) + .map(|(r, row)| (r, row[col].abs())) + .fold( + (col, 0.0f64), + |acc, (r, v)| if v > acc.1 { (r, v) } else { acc }, + ); + if best < 1e-12 { + return None; // singular + } + a.swap(col, pivot); + b.swap(col, pivot); + + // Eliminate below the pivot. Copy the pivot row out so the borrow + // checker (and clippy) are happy mutating the rows below it. + let pivot_row = a[col]; + let pivot_b = b[col]; + let pivot_diag = pivot_row[col]; + for (r, row) in a.iter_mut().enumerate().skip(col + 1) { + let factor = row[col] / pivot_diag; + if factor != 0.0 { + for c in col..N { + row[c] -= factor * pivot_row[c]; + } + b[r] -= factor * pivot_b; + } + } + } + + // Back-substitution. + let mut x = [0.0f64; N]; + for i in (0..N).rev() { + let mut sum = b[i]; + for j in (i + 1)..N { + sum -= a[i][j] * x[j]; + } + x[i] = sum / a[i][i]; + } + Some(x) +} + +#[cfg(test)] +mod tests { + use super::*; + + fn approx_pt(a: Point, b: Point, tol: f64) { + assert!( + (a.x - b.x).abs() < tol && (a.y - b.y).abs() < tol, + "{a:?} != {b:?}" + ); + } + + #[test] + fn identity_maps_points_unchanged() { + let h = Homography::identity(); + let p = Point::new(3.0, 7.0); + approx_pt(h.map(p).unwrap(), p, 1e-12); + } + + #[test] + fn rejects_too_few_points() { + let src = [Point::new(0.0, 0.0); 3]; + let dst = [Point::new(0.0, 0.0); 3]; + assert_eq!( + Homography::fit(&src, &dst), + Err(HomographyError::TooFewPoints { got: 3 }) + ); + } + + #[test] + fn rejects_length_mismatch() { + let src = [Point::new(0.0, 0.0); 5]; + let dst = [Point::new(0.0, 0.0); 4]; + assert!(matches!( + Homography::fit(&src, &dst), + Err(HomographyError::LengthMismatch { src: 5, dst: 4 }) + )); + } + + #[test] + fn recovers_affine_scale_and_translate() { + // Map [0,1]² gaze space onto a 1920×1080 screen: x' = 1920x, y' = 1080y. + let src = [ + Point::new(0.0, 0.0), + Point::new(1.0, 0.0), + Point::new(0.0, 1.0), + Point::new(1.0, 1.0), + Point::new(0.5, 0.5), + ]; + let dst: Vec = src + .iter() + .map(|p| Point::new(p.x * 1920.0, p.y * 1080.0)) + .collect(); + let h = Homography::fit(&src, &dst).unwrap(); + approx_pt( + h.map(Point::new(0.25, 0.75)).unwrap(), + Point::new(480.0, 810.0), + 1e-6, + ); + } + + #[test] + fn recovers_a_true_projective_map() { + // A known non-affine homography (note nonzero h31/h32). + let truth = Homography { + h: [1.2, 0.1, 5.0, -0.05, 0.9, 3.0, 0.001, 0.002, 1.0], + }; + // 9-point 3×3 grid in gaze space. + let mut src = Vec::new(); + for gy in 0..3 { + for gx in 0..3 { + src.push(Point::new(gx as f64, gy as f64)); + } + } + let dst: Vec = src.iter().map(|p| truth.map(*p).unwrap()).collect(); + let fitted = Homography::fit(&src, &dst).unwrap(); + // Fitted map reproduces a held-out interior point. + let test = Point::new(1.5, 0.5); + approx_pt(fitted.map(test).unwrap(), truth.map(test).unwrap(), 1e-4); + } + + #[test] + fn collinear_points_are_singular() { + // All source points on a line → degenerate. + let src = [ + Point::new(0.0, 0.0), + Point::new(1.0, 1.0), + Point::new(2.0, 2.0), + Point::new(3.0, 3.0), + Point::new(4.0, 4.0), + ]; + let dst = [ + Point::new(0.0, 0.0), + Point::new(1.0, 0.0), + Point::new(2.0, 0.0), + Point::new(3.0, 0.0), + Point::new(4.0, 0.0), + ]; + assert_eq!(Homography::fit(&src, &dst), Err(HomographyError::Singular)); + } +} diff --git a/crates/etk-geom/src/lib.rs b/crates/etk-geom/src/lib.rs index 85a85ff..0488eb0 100644 --- a/crates/etk-geom/src/lib.rs +++ b/crates/etk-geom/src/lib.rs @@ -1,9 +1,57 @@ //! Ellipse fitting, glint detection, and calibration math. //! -//! Levenberg-Marquardt refinement of the model's ellipse regression head; -//! brightest-CC glint detection within the iris mask; -//! 2D 9-point homography from gaze-features → screen coords. +//! Scope of this crate (per `PLAN.md` §6 Phase 3 and §2.7): //! -//! See `PLAN.md` §6 Phase 3 (Geometry). +//! * [`Ellipse`] — the pupil ellipse parameterisation produced by the model's +//! regression head and consumed by the screen mapper. +//! * [`OneEuro`] — the One-Euro filter used to smooth the cursor signal +//! (`PLAN.md` §6 Phase 7, defaults `min_cutoff = 1.0`, `beta = 0.007`). +//! * [`Homography`] — the 2D 9-point homography that maps a gaze feature in +//! normalised camera space to screen coordinates (`PLAN.md` §6 Phase 3 & 5). +//! +//! These three pieces are pure CPU math with no camera, GPU, or Wayland +//! dependency, so they are implemented and unit-tested here in full. The +//! GPU-bound mask/ellipse *inference* lives in `etk-infer`; this crate only +//! does the geometry on top of the engine's output. //! -//! Status: scaffold only. +//! See `PLAN.md` §6 Phase 3 (Geometry). + +mod ellipse; +mod homography; +mod one_euro; + +pub use ellipse::Ellipse; +pub use homography::{Homography, HomographyError}; +pub use one_euro::OneEuro; + +/// A 2D point in `f64` precision. +/// +/// Used throughout the geometry layer for pupil centroids, glint positions, +/// gaze features, and screen coordinates. +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct Point { + pub x: f64, + pub y: f64, +} + +impl Point { + /// Construct a point. + #[inline] + pub const fn new(x: f64, y: f64) -> Self { + Self { x, y } + } + + /// Euclidean distance to another point. + #[inline] + pub fn distance(&self, other: &Point) -> f64 { + let dx = self.x - other.x; + let dy = self.y - other.y; + (dx * dx + dy * dy).sqrt() + } +} + +impl From<(f64, f64)> for Point { + fn from((x, y): (f64, f64)) -> Self { + Point::new(x, y) + } +} diff --git a/crates/etk-geom/src/one_euro.rs b/crates/etk-geom/src/one_euro.rs new file mode 100644 index 0000000..f5f1435 --- /dev/null +++ b/crates/etk-geom/src/one_euro.rs @@ -0,0 +1,183 @@ +//! One-Euro filter for cursor smoothing. +//! +//! Reference: Casiez, Roussel & Vogel, "1€ Filter: A Simple Speed-based +//! Low-pass Filter for Noisy Input in Interactive Systems" (CHI 2012). +//! +//! Defaults follow `PLAN.md` §6 Phase 7: `min_cutoff = 1.0`, `beta = 0.007`. +//! Hand-rolled per `PLAN.md` §2.7 ("~30 LoC, beats Kalman for cursor +//! smoothing"). + +/// A single-channel One-Euro filter. +/// +/// Construct one filter per scalar signal (e.g. one each for `x` and `y`). +/// Feed timestamped samples to [`OneEuro::filter`]; the filter adapts its +/// cutoff frequency to the signal speed, trading lag for jitter the faster +/// the signal moves. +#[derive(Debug, Clone)] +pub struct OneEuro { + /// Minimum cutoff frequency (Hz). Lower = smoother but laggier when still. + min_cutoff: f64, + /// Speed coefficient. Higher = less lag when moving fast. + beta: f64, + /// Cutoff frequency (Hz) for the derivative low-pass. + d_cutoff: f64, + + /// Previous filtered value, `None` until the first sample. + x_prev: Option, + /// Previous filtered derivative. + dx_prev: f64, + /// Timestamp (seconds) of the previous sample. + t_prev: Option, +} + +impl OneEuro { + /// Create a filter with the plan defaults (`min_cutoff = 1.0`, + /// `beta = 0.007`, `d_cutoff = 1.0`). + pub fn new() -> Self { + Self::with_params(1.0, 0.007, 1.0) + } + + /// Create a filter with explicit parameters. + /// + /// # Panics + /// Panics if `min_cutoff` or `d_cutoff` is not strictly positive, since a + /// non-positive cutoff yields a meaningless filter alpha. + pub fn with_params(min_cutoff: f64, beta: f64, d_cutoff: f64) -> Self { + assert!(min_cutoff > 0.0, "min_cutoff must be > 0"); + assert!(d_cutoff > 0.0, "d_cutoff must be > 0"); + Self { + min_cutoff, + beta, + d_cutoff, + x_prev: None, + dx_prev: 0.0, + t_prev: None, + } + } + + /// Reset the filter to its initial (uninitialised) state. + /// + /// The next sample is passed through unfiltered, as on first use. + pub fn reset(&mut self) { + self.x_prev = None; + self.dx_prev = 0.0; + self.t_prev = None; + } + + /// Smoothing factor for a cutoff frequency `cutoff` (Hz) at timestep + /// `dt` (seconds). + #[inline] + fn alpha(cutoff: f64, dt: f64) -> f64 { + let tau = 1.0 / (2.0 * std::f64::consts::PI * cutoff); + 1.0 / (1.0 + tau / dt) + } + + /// Filter one sample taken at absolute time `t` (seconds). + /// + /// Returns the smoothed value. Samples must be supplied in non-decreasing + /// time order; a non-positive timestep (duplicate or out-of-order + /// timestamp) is treated as "no time elapsed" and the raw value is + /// returned to avoid a divide-by-zero blow-up. + pub fn filter(&mut self, x: f64, t: f64) -> f64 { + let (x_prev, t_prev) = match (self.x_prev, self.t_prev) { + (Some(xp), Some(tp)) => (xp, tp), + // First sample: seed state, pass through. + _ => { + self.x_prev = Some(x); + self.t_prev = Some(t); + self.dx_prev = 0.0; + return x; + } + }; + + let dt = t - t_prev; + if dt <= 0.0 { + // Out-of-order / duplicate timestamp: keep state, return raw. + return x; + } + + // Low-pass the derivative. + let dx = (x - x_prev) / dt; + let a_d = Self::alpha(self.d_cutoff, dt); + let dx_hat = a_d * dx + (1.0 - a_d) * self.dx_prev; + + // Speed-adaptive cutoff, then low-pass the value. + let cutoff = self.min_cutoff + self.beta * dx_hat.abs(); + let a = Self::alpha(cutoff, dt); + let x_hat = a * x + (1.0 - a) * x_prev; + + self.x_prev = Some(x_hat); + self.dx_prev = dx_hat; + self.t_prev = Some(t); + x_hat + } +} + +impl Default for OneEuro { + fn default() -> Self { + Self::new() + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn first_sample_passes_through() { + let mut f = OneEuro::new(); + assert_eq!(f.filter(3.5, 0.0), 3.5); + } + + #[test] + fn constant_signal_converges_to_value() { + let mut f = OneEuro::new(); + let mut y = 0.0; + for i in 0..200 { + y = f.filter(10.0, i as f64 / 60.0); + } + assert!((y - 10.0).abs() < 1e-3, "converged to {y}"); + } + + #[test] + fn output_lags_a_step_input() { + // A sudden jump should be smoothed: the first filtered value after the + // step lands strictly between the old and new level. + let mut f = OneEuro::new(); + f.filter(0.0, 0.0); + let y = f.filter(100.0, 1.0 / 60.0); + assert!(y > 0.0 && y < 100.0, "expected smoothing, got {y}"); + } + + #[test] + fn reduces_jitter_variance() { + // Noisy signal around a constant mean: filtered variance < raw. + let raw = [0.0, 2.0, -2.0, 1.5, -1.0, 2.0, -1.5, 0.5, -2.0, 1.0]; + let mut f = OneEuro::with_params(1.0, 0.0, 1.0); // beta=0 => pure low-pass + let mut filtered = Vec::new(); + for (i, &v) in raw.iter().enumerate() { + filtered.push(f.filter(v, i as f64 / 60.0)); + } + let var = |xs: &[f64]| { + let m = xs.iter().sum::() / xs.len() as f64; + xs.iter().map(|x| (x - m).powi(2)).sum::() / xs.len() as f64 + }; + assert!(var(&filtered) < var(&raw), "filter did not reduce variance"); + } + + #[test] + fn out_of_order_timestamp_returns_raw_without_panic() { + let mut f = OneEuro::new(); + f.filter(1.0, 1.0); + assert_eq!(f.filter(5.0, 0.5), 5.0); // dt <= 0 => raw + } + + #[test] + fn reset_restores_passthrough() { + let mut f = OneEuro::new(); + f.filter(1.0, 0.0); + f.filter(2.0, 0.1); + f.reset(); + assert_eq!(f.filter(9.0, 0.2), 9.0); + } +}