From 77a76cb77b299402e53b10cd957cc43c1f1a7fda Mon Sep 17 00:00:00 2001 From: schluis Date: Sun, 4 Oct 2026 07:35:51 +0000 Subject: [PATCH 01/11] Improve ball-filter tracking, timing and publication estimates --- crates/nodes/ball_filter/src/competition.rs | 129 ++ crates/nodes/ball_filter/src/field_prior.rs | 791 +++++++ crates/nodes/ball_filter/src/filter.rs | 507 ++++- crates/nodes/ball_filter/src/hypothesis.rs | 657 +++++- .../src/hypothesis/motion_evidence.rs | 130 ++ .../ball_filter/src/hypothesis/moving.rs | 249 ++- .../ball_filter/src/hypothesis/resting.rs | 74 +- crates/nodes/ball_filter/src/lib.rs | 1422 ++++++++++-- .../ball_filter/src/negative_evidence.rs | 338 +++ .../nodes/ball_filter/src/obstacle_input.rs | 159 ++ crates/nodes/ball_filter/src/reacquisition.rs | 108 + crates/nodes/ball_filter/src/tracker.rs | 1923 +++++++++++++++++ .../nodes/ball_filter/src/validity_decay.rs | 72 + crates/nodes/obstacle_filter/src/lib.rs | 907 ++++++-- crates/types/src/parameters.rs | 167 +- etc/parameters/base/ball_filter.json5 | 20 + 16 files changed, 7279 insertions(+), 374 deletions(-) create mode 100644 crates/nodes/ball_filter/src/competition.rs create mode 100644 crates/nodes/ball_filter/src/field_prior.rs create mode 100644 crates/nodes/ball_filter/src/hypothesis/motion_evidence.rs create mode 100644 crates/nodes/ball_filter/src/negative_evidence.rs create mode 100644 crates/nodes/ball_filter/src/obstacle_input.rs create mode 100644 crates/nodes/ball_filter/src/reacquisition.rs create mode 100644 crates/nodes/ball_filter/src/tracker.rs create mode 100644 crates/nodes/ball_filter/src/validity_decay.rs diff --git a/crates/nodes/ball_filter/src/competition.rs b/crates/nodes/ball_filter/src/competition.rs new file mode 100644 index 0000000000..2267cea954 --- /dev/null +++ b/crates/nodes/ball_filter/src/competition.rs @@ -0,0 +1,129 @@ +//! A soft single-ball preference, earned by sustained actual leader observations. +use std::time::Duration; + +use projection::camera_matrix::CameraMatrix; +use ros_z::{Message, time::Time}; +use serde::{Deserialize, Serialize}; +use types::{ + object_detection::{Object, RobocupObjectLabel}, + obstacles::Obstacle, + parameters::BallFilterParameters, +}; + +use crate::{ + BallFilter, + negative_evidence::{self, Visibility}, +}; + +const MINIMUM_LEADER_VALIDITY: f32 = 10.0; +const LEADER_WARMUP: Duration = Duration::from_secs(1); +const MAXIMUM_EXPOSURE_INTERVAL: Duration = Duration::from_millis(120); + +#[derive(Clone, Debug, Serialize, Deserialize, Message)] +pub struct Evidence { + pub first_match: Time, + pub last_match: Time, +} + +pub fn enabled(parameters: &BallFilterParameters) -> bool { + parameters + .competing_hypothesis_validity_decay_rate + .is_some() +} + +#[expect( + clippy::too_many_arguments, + reason = "Keep same-exposure detections and Ground obstacle geometry explicit" +)] +pub fn apply( + filter: &mut BallFilter, + time: Time, + matched: &[bool], + camera: Option<&CameraMatrix>, + obstacles: Option<&[Obstacle]>, + detections: &[Object], + ball_radius: f32, + parameters: &BallFilterParameters, +) { + let rate = parameters + .competing_hypothesis_validity_decay_rate + .filter(|rate| rate.is_finite() && *rate > 0.0); + let leader = rate + .and_then(|_| filter.best_hypothesis(parameters.validity_output_threshold)) + .and_then(|leader| { + filter + .hypotheses + .iter() + .position(|hypothesis| std::ptr::eq(leader, hypothesis)) + }) + .filter(|&index| { + let hypothesis = &filter.hypotheses[index]; + matched.get(index).copied().unwrap_or(false) + && hypothesis.validity.is_finite() + && hypothesis.validity + >= MINIMUM_LEADER_VALIDITY.max(parameters.validity_output_threshold) + && camera.is_some_and(|camera| { + negative_evidence::classify_with_detections( + &hypothesis.position(), + camera, + ball_radius, + obstacles, + detections, + ) != Visibility::Unknown + }) + }); + for (index, hypothesis) in filter.hypotheses.iter_mut().enumerate() { + if Some(index) != leader { + hypothesis.leadership_evidence = None; + } + } + let Some(leader) = leader else { + return; + }; + let hypothesis = &mut filter.hypotheses[leader]; + if hypothesis + .leadership_evidence + .as_ref() + .is_some_and(|previous| previous.last_match >= time) + { + return; + } + let previous = hypothesis + .leadership_evidence + .take() + .filter(|previous| time.duration_since(previous.last_match) <= MAXIMUM_EXPOSURE_INTERVAL); + let interval = previous.as_ref().map_or(Duration::ZERO, |previous| { + time.duration_since(previous.last_match) + }); + let first_match = previous + .as_ref() + .map_or(time, |previous| previous.first_match); + hypothesis.leadership_evidence = Some(Evidence { + first_match, + last_match: time, + }); + // Do not back-charge the interval that established the warmup period. + let mature_interval = time + .duration_since(first_match) + .saturating_sub(LEADER_WARMUP) + .min(interval); + let factor = (-rate.unwrap_or_default() * mature_interval.as_secs_f32()).exp(); + for (index, hypothesis) in filter.hypotheses.iter_mut().enumerate() { + // Every real current-frame match remains protected, including a second + // real ball or a newly reacquired candidate. Raw validity is not a probability. + if !matched.get(index).copied().unwrap_or(false) + && camera.is_some_and(|camera| { + crate::hypothesis_visibility( + hypothesis, + camera, + ball_radius, + obstacles, + detections, + parameters, + ) == Visibility::Visible + }) + { + hypothesis.validity *= factor; + } + } +} diff --git a/crates/nodes/ball_filter/src/field_prior.rs b/crates/nodes/ball_filter/src/field_prior.rs new file mode 100644 index 0000000000..e6a6ce92a2 --- /dev/null +++ b/crates/nodes/ball_filter/src/field_prior.rs @@ -0,0 +1,791 @@ +//! Field-boundary confidence prior and optional elapsed-time validity decay. +use std::{collections::BTreeMap, time::Duration}; + +use coordinate_systems::{Field, Ground}; +use linear_algebra::Isometry2; +use ros_z::time::Time; +use types::{field_dimensions::FieldDimensions, parameters::BallFilterParameters}; + +use crate::BallHypothesis; + +const POSE_HISTORY_CAPACITY: usize = 512; +const MAXIMUM_POSE_TIME_DIFFERENCE: Duration = Duration::from_millis(20); + +#[derive(Default)] +pub(crate) struct FieldPoseHistory { + poses: BTreeMap>, +} + +impl FieldPoseHistory { + pub fn insert(&mut self, time: Time, pose: Isometry2) { + self.poses.insert(time, pose); + while self.poses.len() > POSE_HISTORY_CAPACITY { + self.poses.pop_first(); + } + } + + pub fn at(&self, time: Time) -> Option> { + let before = self.poses.range(..=time).next_back(); + let after = self.poses.range(time..).next(); + [before, after] + .into_iter() + .flatten() + .min_by_key(|(stamp, _)| stamp.as_nanos().abs_diff(time.as_nanos())) + .filter(|(stamp, _)| { + u128::from(stamp.as_nanos().abs_diff(time.as_nanos())) + <= MAXIMUM_POSE_TIME_DIFFERENCE.as_nanos() + }) + .map(|(_, pose)| *pose) + } +} + +// A long processing/pose gap is not evidence that the ball stayed outside the +// field throughout that interval. Ordinary 500 Hz fusion and 25 Hz replay are +// integrated in seconds rather than charged once per cycle. +const MAXIMUM_VALIDITY_DECAY_INTERVAL: Duration = Duration::from_millis(120); + +#[derive(Default)] +pub(crate) struct ValidityDecayClock { + previous: Option<(Time, bool)>, +} + +impl ValidityDecayClock { + pub fn reset(&mut self) { + self.previous = None; + } + + pub fn elapsed(&mut self, time: Time, valid_pose: bool) -> Duration { + let elapsed = match self.previous { + Some((previous, _)) if time <= previous => return Duration::ZERO, + Some((previous, true)) if valid_pose => { + let elapsed = time.duration_since(previous); + if elapsed <= MAXIMUM_VALIDITY_DECAY_INTERVAL { + elapsed + } else { + Duration::ZERO + } + } + _ => Duration::ZERO, + }; + self.previous = Some((time, valid_pose)); + elapsed + } +} + +pub(crate) fn decay_stored_validity( + hypotheses: &mut [BallHypothesis], + elapsed: Duration, + ground_to_field: Option>, + dimensions: &FieldDimensions, + parameters: &BallFilterParameters, +) { + let maximum_rate = parameters.field_boundary_validity_decay_rate; + if !parameters.good_localization + || elapsed.is_zero() + || !maximum_rate.is_finite() + || maximum_rate <= 0.0 + { + return; + } + for hypothesis in hypotheses { + let weight = confidence_weight(hypothesis, ground_to_field, dimensions, parameters); + let rate = maximum_rate * (1.0 - weight.clamp(0.0, 1.0)); + hypothesis.validity *= (-rate * elapsed.as_secs_f32()).exp(); + } +} + +pub(crate) fn effective_validity( + hypothesis: &BallHypothesis, + ground_to_field: Option>, + dimensions: &FieldDimensions, + parameters: &BallFilterParameters, +) -> f32 { + hypothesis.validity * confidence_weight(hypothesis, ground_to_field, dimensions, parameters) +} + +pub(crate) fn confidence_weight( + hypothesis: &BallHypothesis, + ground_to_field: Option>, + dimensions: &FieldDimensions, + parameters: &BallFilterParameters, +) -> f32 { + if !parameters.good_localization { + return 1.0; + } + let decay_distance = parameters.field_boundary_confidence_decay_distance; + let Some(ground_to_field) = + ground_to_field.filter(|_| decay_distance.is_finite() && decay_distance > 0.0) + else { + return 1.0; + }; + let position = ground_to_field * hypothesis.position().position; + if !position.x().is_finite() || !position.y().is_finite() { + return 1.0; + } + // A ball is wholly out only once its nearest edge has crossed the field + // rectangle. Border strips are outside the playing field, too. + let dx = (position.x().abs() - dimensions.length / 2.0).max(0.0); + let dy = (position.y().abs() - dimensions.width / 2.0).max(0.0); + let whole_ball_distance = (dx.hypot(dy) - dimensions.ball_radius).max(0.0); + // Allow localization/radius uncertainty beyond the existing whole-ball + // clearance. The margin delays both ranking and stored-validity penalties; + // their exponential shape remains unchanged outside that buffer. + let margin = parameters.field_boundary_margin; + let margin = if margin.is_finite() && margin > 0.0 { + margin + } else { + 0.0 + }; + let penalized_distance = (whole_ball_distance - margin).max(0.0); + (-penalized_distance / decay_distance).exp() +} + +#[cfg(test)] +mod tests { + use linear_algebra::{point, vector}; + use nalgebra::Matrix4; + use types::multivariate_normal_distribution::MultivariateNormalDistribution; + + use crate::{BallFilter, BallMode, tracker::Tracker}; + + use super::*; + + fn parameters() -> BallFilterParameters { + let mut parameters = BallFilterParameters::default(); + parameters.field_boundary_confidence_decay_distance = 0.3; + parameters.validity_output_threshold = 0.5; + parameters.hypothesis_timeout = Duration::from_secs(20); + parameters.maximum_number_of_hypotheses = 15; + parameters + } + + fn hypothesis(x: f32, y: f32, validity: f32) -> BallHypothesis { + BallHypothesis { + mode: BallMode::Moving(MultivariateNormalDistribution { + mean: nalgebra::vector![x, y, 0.3, -0.2], + covariance: Matrix4::identity(), + }), + last_seen: Time::zero(), + validity, + motion_evidence: None, + negative_evidence: None, + validity_decay_evidence: None, + leadership_evidence: None, + last_observation_size_plausible: None, + merge_observation_start: None, + } + } + + fn run_stored_decay(x: f32, step_millis: i64) -> f32 { + let mut parameters = parameters(); + parameters.field_boundary_validity_decay_rate = 2.0; + let dimensions = FieldDimensions::SPL_2025; + let mut tracker = Tracker::default(); + tracker.filter.hypotheses.push(hypothesis(x, 0.0, 25.0)); + for millis in (0..=1000).step_by(step_millis as usize) { + tracker.finish_with_field_pose( + Time::from_nanos(millis * 1_000_000), + ¶meters, + &dimensions, + Some(Isometry2::identity()), + ); + } + tracker.filter.hypotheses[0].validity + } + + #[test] + fn untrusted_localization_preserves_candidate_confidence_and_output_selection() { + let dimensions = FieldDimensions::SPL_2025; + let mut parameters = parameters(); + assert!( + parameters.good_localization, + "preserve existing default behavior" + ); + parameters.good_localization = false; + parameters.field_boundary_validity_decay_rate = 2.0; + parameters.validity_discard_threshold = 1.0; + let outside = hypothesis(dimensions.length / 2.0 + 1.5, 0.0, 25.0); + let original = outside.position(); + let mut tracker = Tracker::default(); + tracker.filter.hypotheses = vec![outside, hypothesis(1.0, 0.0, 4.0)]; + for millis in (0..=2000).step_by(40) { + let pose = if millis < 1000 { + Some(Isometry2::identity()) + } else { + Some(Isometry2::from_parts(vector![9.0, -5.0], 1.0)) + }; + let selected = tracker + .finish_with_field_pose( + Time::from_nanos(millis * 1_000_000), + ¶meters, + &dimensions, + pose, + ) + .unwrap(); + assert_eq!(tracker.filter.hypotheses.len(), 2); + assert_eq!(tracker.filter.hypotheses[0].validity, 25.0); + assert_eq!(tracker.filter.hypotheses[1].validity, 4.0); + assert_eq!(selected.position, original.position); + assert_eq!(selected.velocity, original.velocity); + assert_eq!(selected.last_seen, original.last_seen); + assert!(crate::hypothetical_ball_positions( + &tracker.filter, ¶meters, &dimensions, pose, + ).is_empty()); + } + parameters.good_localization = true; + let selected = tracker + .finish_with_field_pose( + Time::from_nanos(2_040_000_000), + ¶meters, + &dimensions, + Some(Isometry2::identity()), + ) + .unwrap(); + assert_eq!(selected.position, point![1.0, 0.0]); + assert_eq!( + tracker.filter.hypotheses[0].validity, 25.0, + "the first trusted pose must not charge disabled time" + ); + assert_eq!( + crate::hypothetical_ball_positions( + &tracker.filter, + ¶meters, + &dimensions, + Some(Isometry2::identity()), + ) + .len(), + 1 + ); + } + + #[test] + fn disabling_localization_resets_decay_even_at_duplicate_or_older_finish_times() { + let dimensions = FieldDimensions::SPL_2025; + let pose = Some(Isometry2::identity()); + for disabled_millis in [20, 40, 60] { + let mut parameters = parameters(); + parameters.field_boundary_validity_decay_rate = 2.0; + let mut tracker = Tracker::default(); + tracker + .filter + .hypotheses + .push(hypothesis(dimensions.length / 2.0 + 0.6, 0.0, 25.0)); + for millis in [0, 40] { + tracker.finish_with_field_pose( + Time::from_nanos(millis * 1_000_000), + ¶meters, + &dimensions, + pose, + ); + } + let before = tracker.filter.hypotheses[0].validity; + assert!(before < 25.0); + parameters.good_localization = false; + tracker.finish_with_field_pose( + Time::from_nanos(disabled_millis * 1_000_000), + ¶meters, + &dimensions, + pose, + ); + assert_eq!(tracker.filter.hypotheses[0].validity, before); + parameters.good_localization = true; + tracker.finish_with_field_pose( + Time::from_nanos(80_000_000), + ¶meters, + &dimensions, + pose, + ); + assert_eq!(tracker.filter.hypotheses[0].validity, before); + tracker.finish_with_field_pose( + Time::from_nanos(120_000_000), + ¶meters, + &dimensions, + pose, + ); + assert!(tracker.filter.hypotheses[0].validity < before); + } + } + + #[test] + fn direct_field_decay_and_ranking_are_neutral_when_localization_is_untrusted() { + let dimensions = FieldDimensions::SPL_2025; + let mut parameters = parameters(); + parameters.good_localization = false; + parameters.field_boundary_margin = 0.5; + parameters.field_boundary_validity_decay_rate = 2.0; + let mut balls = vec![hypothesis(1.0, 0.0, 2.0)]; + let pose = Some(Isometry2::from_parts(vector![100.0, 0.0], 0.0)); + decay_stored_validity( + &mut balls, + Duration::from_secs(5), + pose, + &dimensions, + ¶meters, + ); + assert_eq!(balls[0].validity, 2.0); + assert_eq!( + effective_validity(&balls[0], pose, &dimensions, ¶meters), + 2.0 + ); + } + + #[test] + fn stored_field_decay_integrates_seconds_independently_of_finish_frequency() { + let dimensions = FieldDimensions::SPL_2025; + let x = dimensions.length / 2.0 + + dimensions.ball_radius + + parameters().field_boundary_confidence_decay_distance; + let expected = 25.0 * (-2.0_f32 * (1.0 - (-1.0_f32).exp())).exp(); + for step in [2, 20, 40, 100] { + let validity = run_stored_decay(x, step); + assert!( + (validity - expected).abs() < 0.001, + "{step} ms: {validity} vs {expected}" + ); + } + } + + #[test] + fn stored_decay_is_neutral_inside_and_stronger_outside_with_a_bounded_rate() { + let dimensions = FieldDimensions::SPL_2025; + let edge = dimensions.length / 2.0 + dimensions.ball_radius; + assert_eq!(run_stored_decay(0.0, 20), 25.0); + assert!((run_stored_decay(edge, 20) - 25.0).abs() < 0.001); + let near = run_stored_decay(edge + 0.1, 20); + let farther = run_stored_decay(edge + 0.6, 20); + assert!(farther < near && near < 25.0); + assert!( + farther >= 25.0 * (-2.0_f32).exp(), + "maximum rate is two per second" + ); + } + + #[test] + fn boundary_margin_protects_nearby_balls_and_preserves_decay_beyond_it() { + let dimensions = FieldDimensions::SPL_2025; + let mut parameters = parameters(); + parameters.field_boundary_margin = 0.5; + parameters.field_boundary_validity_decay_rate = 2.0; + let edge = dimensions.length / 2.0 + dimensions.ball_radius; + let pose = Some(Isometry2::identity()); + for distance in [0.0, 0.1, 0.3, 0.49] { + let mut balls = [hypothesis(edge + distance, 0.0, 25.0)]; + assert_eq!( + confidence_weight(&balls[0], pose, &dimensions, ¶meters), + 1.0 + ); + decay_stored_validity( + &mut balls, + Duration::from_secs(1), + pose, + &dimensions, + ¶meters, + ); + assert_eq!(balls[0].validity, 25.0); + } + let buffered_edge = hypothesis(edge + 0.5, 0.0, 25.0); + assert!( + (confidence_weight(&buffered_edge, pose, &dimensions, ¶meters) - 1.0).abs() < 1e-5 + ); + for distance in [0.1_f32, 0.3, 1.0] { + let mut balls = [hypothesis(edge + 0.5 + distance, 0.0, 25.0)]; + let expected_weight = (-distance / 0.3).exp(); + assert!( + (confidence_weight(&balls[0], pose, &dimensions, ¶meters) - expected_weight) + .abs() + < 1e-5 + ); + decay_stored_validity( + &mut balls, + Duration::from_secs(1), + pose, + &dimensions, + ¶meters, + ); + let expected_validity = 25.0 * (-2.0 * (1.0 - expected_weight)).exp(); + assert!((balls[0].validity - expected_validity).abs() < 1e-4); + } + } + + #[test] + fn boundary_margin_handles_corner_distance_and_transformed_ground_positions() { + let dimensions = FieldDimensions::SPL_2025; + let mut parameters = parameters(); + parameters.field_boundary_margin = 0.5; + let pose = Isometry2::from_parts(vector![2.0, -1.0], 1.4); + let corner = point![dimensions.length / 2.0, dimensions.width / 2.0]; + for clearance in [0.4_f32, 0.8] { + let offset = (dimensions.ball_radius + clearance) / 2.0_f32.sqrt(); + let ground_position = pose.inverse() * (corner + vector![offset, offset]); + let ball = hypothesis(ground_position.x(), ground_position.y(), 25.0); + let expected = (-(clearance - 0.5).max(0.0) / 0.3).exp(); + assert!( + (confidence_weight(&ball, Some(pose), &dimensions, ¶meters) - expected).abs() + < 1e-5 + ); + } + } + + #[test] + fn localization_wobble_near_the_sideline_does_not_decay_a_track() { + let dimensions = FieldDimensions::SPL_2025; + let mut parameters = parameters(); + parameters.field_boundary_margin = 0.5; + parameters.field_boundary_validity_decay_rate = 2.0; + let mut tracker = Tracker::default(); + tracker.filter.hypotheses.push(hypothesis( + dimensions.length / 2.0 + dimensions.ball_radius, + 0.0, + 25.0, + )); + for tick in 0..=50 { + let offset = if tick % 2 == 0 { -0.1 } else { 0.1 }; + assert!( + tracker + .finish_with_field_pose( + Time::from_nanos(tick * 20_000_000), + ¶meters, + &dimensions, + Some(Isometry2::from_parts(vector![offset, 0.0], 0.0)), + ) + .is_some() + ); + } + assert_eq!(tracker.filter.hypotheses[0].validity, 25.0); + } + + #[test] + fn boundary_buffer_adds_no_decay_for_hidden_or_visible_hypotheses() { + use crate::{negative_evidence::Visibility, validity_decay::Evidence}; + + let dimensions = FieldDimensions::SPL_2025; + let mut parameters = parameters(); + parameters.field_boundary_margin = 0.5; + parameters.field_boundary_validity_decay_rate = 2.0; + parameters.hidden_validity_decay_rate = Some(0.0); + parameters.visible_missed_validity_decay_rate = Some(1.0); + for visibility in [Visibility::Hidden, Visibility::Visible] { + let mut tracker = Tracker::default(); + let mut ball = hypothesis( + dimensions.length / 2.0 + dimensions.ball_radius + 0.3, + 0.0, + 25.0, + ); + ball.validity_decay_evidence = Some(Evidence { + time: Time::zero(), + visibility, + }); + tracker.filter.hypotheses.push(ball); + // Finishes between detector exposures may apply field decay, but + // cannot treat visibility or an occluded opponent as a new miss. + for tick in 0..=50 { + assert!( + tracker + .finish_with_field_pose( + Time::from_nanos(tick * 20_000_000), + ¶meters, + &dimensions, + Some(Isometry2::identity()), + ) + .is_some() + ); + } + assert_eq!(tracker.filter.hypotheses[0].validity, 25.0); + } + } + + #[test] + fn default_or_invalid_boundary_margin_preserves_legacy_geometry() { + let mut parameters = parameters(); + assert_eq!(parameters.field_boundary_margin, 0.0); + let dimensions = FieldDimensions::SPL_2025; + let ball = hypothesis( + dimensions.length / 2.0 + dimensions.ball_radius + 0.3, + 0.0, + 25.0, + ); + for margin in [0.0, -0.5, f32::NAN, f32::INFINITY] { + parameters.field_boundary_margin = margin; + assert!( + (confidence_weight(&ball, Some(Isometry2::identity()), &dimensions, ¶meters) + - (-1.0_f32).exp()) + .abs() + < 1e-5 + ); + } + } + + #[test] + fn missing_pose_sensor_gaps_and_nonmonotonic_finishes_do_not_charge_unknown_time() { + let mut parameters = parameters(); + parameters.field_boundary_validity_decay_rate = 2.0; + let dimensions = FieldDimensions::SPL_2025; + let mut tracker = Tracker::default(); + tracker + .filter + .hypotheses + .push(hypothesis(dimensions.length / 2.0 + 0.6, 0.0, 25.0)); + for (millis, pose) in [ + (0, Some(Isometry2::identity())), + (40, None), + (80, Some(Isometry2::identity())), + (80, Some(Isometry2::identity())), + (60, Some(Isometry2::identity())), + (10_000, Some(Isometry2::identity())), + ] { + tracker.finish_with_field_pose( + Time::from_nanos(millis * 1_000_000), + ¶meters, + &dimensions, + pose, + ); + assert_eq!(tracker.filter.hypotheses[0].validity, 25.0); + } + tracker.finish_with_field_pose( + Time::from_nanos(10_040_000_000), + ¶meters, + &dimensions, + Some(Isometry2::identity()), + ); + let decayed = tracker.filter.hypotheses[0].validity; + assert!(decayed < 25.0 && decayed > 23.0); + tracker.finish_with_field_pose( + Time::from_nanos(10_040_000_000), + ¶meters, + &dimensions, + Some(Isometry2::identity()), + ); + assert_eq!(tracker.filter.hypotheses[0].validity, decayed); + } + + #[test] + fn whole_ball_prior_is_neutral_at_line_and_decays_monotonically_outside() { + let dimensions = FieldDimensions::SPL_2025; + let parameters = parameters(); + let confidence = |x, y| { + effective_validity( + &hypothesis(x, y, 2.0), + Some(Isometry2::identity()), + &dimensions, + ¶meters, + ) + }; + let edge = dimensions.length / 2.0; + assert_eq!(confidence(0.0, 0.0), 2.0); + assert_eq!(confidence(edge, 0.0), 2.0); + assert!((confidence(edge + dimensions.ball_radius, 0.0) - 2.0).abs() < 1e-5); + let near = confidence( + edge + dimensions.ball_radius + parameters.field_boundary_confidence_decay_distance, + 0.0, + ); + let far = confidence(edge + dimensions.ball_radius + 0.6, 0.0); + assert!((near - 2.0 / std::f32::consts::E).abs() < 1e-5); + assert!(0.0 < far && far < near && near < 2.0); + assert_eq!(confidence(-edge - 0.5, 0.0), confidence(edge + 0.5, 0.0)); + let corner = confidence(edge + 0.3, dimensions.width / 2.0 + 0.3); + assert!(corner < confidence(edge + 0.3, 0.0)); + } + + #[test] + fn field_prior_accounts_for_robot_translation_and_rotation() { + let dimensions = FieldDimensions::SPL_2025; + let parameters = parameters(); + let field_position = point![ + dimensions.length / 2.0 + + dimensions.ball_radius + + parameters.field_boundary_confidence_decay_distance, + 0.4 + ]; + for pose in [ + Isometry2::::identity(), + Isometry2::from_parts(vector![2.0, -1.0], 1.4), + Isometry2::from_parts(vector![-3.0, 2.0], -2.7), + ] { + let ground_position = pose.inverse() * field_position; + let ball = hypothesis(ground_position.x(), ground_position.y(), 2.0); + let confidence = effective_validity(&ball, Some(pose), &dimensions, ¶meters); + assert!((confidence - 2.0 / std::f32::consts::E).abs() < 1e-5); + } + } + + #[test] + fn pose_history_uses_bounded_source_time_matching_with_missing_fallback() { + let mut history = FieldPoseHistory::default(); + let query = Time::from_nanos(100_000_000); + assert!(history.at(query).is_none()); + history.insert(Time::from_nanos(79_999_999), Isometry2::identity()); + assert!(history.at(query).is_none()); + history.insert(Time::from_nanos(120_000_000), Isometry2::identity()); + assert!(history.at(query).is_some()); + let older = Isometry2::from_parts(vector![1.0, 0.0], 0.0); + history.insert(Time::from_nanos(80_000_000), older); + assert_eq!(history.at(query), Some(older)); // earlier wins a symmetric tie + for tick in 0..600 { + history.insert( + Time::from_nanos(1_000_000_000 + tick * 2_000_000), + Isometry2::identity(), + ); + } + assert_eq!(history.poses.len(), POSE_HISTORY_CAPACITY); + assert!(history.at(query).is_none()); + assert_eq!( + effective_validity( + &hypothesis(6.0, 0.0, 2.0), + history.at(query), + &FieldDimensions::SPL_2025, + ¶meters() + ), + 2.0, + ); + } + + #[test] + fn localization_jump_can_recover_without_destroying_or_decaying_the_track() { + let parameters = parameters(); + let dimensions = FieldDimensions::SPL_2025; + let mut tracker = Tracker::default(); + tracker.filter.hypotheses.push(hypothesis(1.0, 0.0, 2.0)); + let original = tracker.filter.hypotheses[0].position(); + let bad_pose = Some(Isometry2::from_parts(vector![9.0, 0.0], 0.0)); + for tick in 0..100 { + assert!( + tracker + .finish_with_field_pose( + Time::from_nanos(tick * 2_000_000), + ¶meters, + &dimensions, + bad_pose, + ) + .is_none() + ); + } + assert_eq!(tracker.filter.hypotheses.len(), 1); + assert_eq!(tracker.filter.hypotheses[0].validity, 2.0); + let recovered = tracker + .finish_with_field_pose( + Time::from_nanos(200_000_000), + ¶meters, + &dimensions, + Some(Isometry2::identity()), + ) + .unwrap(); + assert_eq!(recovered.position, original.position); + assert_eq!(recovered.velocity, original.velocity); + assert_eq!(recovered.last_seen, original.last_seen); + assert!( + tracker + .finish_with_field_pose( + Time::from_nanos(200_000_000), + ¶meters, + &dimensions, + None, + ) + .is_some() + ); + } + + #[test] + fn field_prior_is_unchanged_until_a_confirmed_track_can_replace_a_stale_one() { + let dimensions = FieldDimensions::SPL_2025; + let parameters = parameters(); + let outside = hypothesis( + dimensions.length / 2.0 + dimensions.ball_radius + 0.6, + 0.0, + 1000.0, + ); + let inside = hypothesis(1.0, 0.0, 4.0); + let mut filter = BallFilter { + hypotheses: vec![outside, inside], + }; + let pose = Some(Isometry2::identity()); + assert!(effective_validity(&filter.hypotheses[0], pose, &dimensions, ¶meters) > 4.0); + // The prior remains soft: strong recent evidence outside the field can + // still outweigh a weaker inside track, exactly as before recovery. + assert!( + filter + .best_hypothesis_with_field_pose(¶meters, &dimensions, pose) + .unwrap() + .position() + .position + .x() + > dimensions.length / 2.0 + ); + filter.hypotheses[1].last_seen = Time::from_nanos(40_000_000); + assert!( + filter + .best_hypothesis_with_field_pose(¶meters, &dimensions, pose) + .unwrap() + .position() + .position + .x() + > dimensions.length / 2.0 + ); + // Once stale, accumulated outside confidence cannot neutralize the + // prior in the recovery comparison against a confirmed fresh track. + filter.hypotheses[1].last_seen = Time::from_nanos(500_000_000); + filter.hypotheses[1].validity = 2.0; + assert!( + filter + .best_hypothesis_with_field_pose(¶meters, &dimensions, pose) + .unwrap() + .position() + .position + .x() + > dimensions.length / 2.0 + ); + filter.hypotheses[1].validity = 4.0; + let chosen = filter + .best_hypothesis_with_field_pose(¶meters, &dimensions, pose) + .unwrap(); + assert_eq!(chosen.position().position, point![1.0, 0.0]); + assert_eq!(filter.hypotheses[0].validity, 1000.0); + assert_eq!( + filter + .best_hypothesis(parameters.validity_output_threshold) + .unwrap() + .position() + .position, + filter + .best_hypothesis_with_field_pose(¶meters, &dimensions, None) + .unwrap() + .position() + .position + ); + } + + #[test] + fn primary_output_and_hypothetical_output_share_the_same_confidence() { + let dimensions = FieldDimensions::SPL_2025; + let parameters = parameters(); + let outside = hypothesis( + dimensions.length / 2.0 + dimensions.ball_radius + 0.9, + 0.0, + 4.0, + ); + let inside = hypothesis(1.0, 0.0, 1.0); + let mut tracker = Tracker::default(); + tracker.filter = BallFilter { + hypotheses: vec![outside, inside], + }; + let pose = Some(Isometry2::identity()); + let chosen = tracker + .finish_with_field_pose(Time::zero(), ¶meters, &dimensions, pose) + .unwrap(); + let best = tracker + .filter + .best_hypothesis_with_field_pose(¶meters, &dimensions, pose) + .unwrap(); + assert_eq!(chosen.position, point![1.0, 0.0]); + assert_eq!(chosen.position, best.position().position); + let hypothetical = + crate::hypothetical_ball_positions(&tracker.filter, ¶meters, &dimensions, pose); + assert_eq!(hypothetical.len(), 1); + assert!(hypothetical[0].position.x() > dimensions.length / 2.0); + assert!(hypothetical[0].validity < parameters.validity_output_threshold); + assert_eq!(tracker.filter.hypotheses[0].validity, 4.0); + let without_pose = tracker + .finish(Time::zero(), ¶meters, &dimensions) + .unwrap(); + assert_eq!(without_pose.position, hypothetical[0].position); + } +} diff --git a/crates/nodes/ball_filter/src/filter.rs b/crates/nodes/ball_filter/src/filter.rs index fb648fd46a..8d4a38a8aa 100644 --- a/crates/nodes/ball_filter/src/filter.rs +++ b/crates/nodes/ball_filter/src/filter.rs @@ -1,16 +1,23 @@ use std::time::Duration; -use coordinate_systems::Ground; -use filtering::kalman_filter::KalmanFilter; -use linear_algebra::{IntoFramed, Isometry2, distance}; -use nalgebra::{Matrix2, Matrix2x4, Matrix4}; -use ordered_float::NotNan; +use coordinate_systems::{Field, Ground}; +use linear_algebra::Isometry2; +use nalgebra::{Matrix2, Matrix4}; use ros_z::{Message, time::Time}; use serde::{Deserialize, Serialize}; -use types::multivariate_normal_distribution::MultivariateNormalDistribution; +use types::{ + field_dimensions::FieldDimensions, + multivariate_normal_distribution::MultivariateNormalDistribution, + parameters::BallFilterParameters, +}; use crate::hypothesis::{BallHypothesis, BallMode}; +// Allow short perception interruptions before a confirmed competing track can +// replace the most trusted track. At 25 Hz this covers three image intervals; +// this is a switching-delay budget, not the lifetime of a hidden hypothesis. +const STALE_TRACK_RECOVERY_GRACE: Duration = Duration::from_millis(120); + #[derive(Debug, Default, Clone, Serialize, Deserialize, Message)] pub struct BallFilter { pub hypotheses: Vec, @@ -18,10 +25,104 @@ pub struct BallFilter { impl BallFilter { pub fn best_hypothesis(&self, validity_threshold: f32) -> Option<&BallHypothesis> { - self.hypotheses - .iter() - .filter(|hypothesis| hypothesis.validity >= validity_threshold) - .max_by(|a, b| a.validity.total_cmp(&b.validity)) + self.select_hypothesis(validity_threshold, |_| 1.0, 0.0, 0.0) + } + + pub fn best_hypothesis_with_field_pose( + &self, + parameters: &BallFilterParameters, + dimensions: &FieldDimensions, + ground_to_field: Option>, + ) -> Option<&BallHypothesis> { + self.select_hypothesis( + parameters.validity_output_threshold, + |hypothesis| { + crate::field_prior::confidence_weight( + hypothesis, + ground_to_field, + dimensions, + parameters, + ) + }, + parameters.hypothesis_uncertainty_weight, + parameters.selection_confidence_cap, + ) + } + + fn select_hypothesis( + &self, + validity_threshold: f32, + confidence_weight: impl Fn(&BallHypothesis) -> f32, + uncertainty_weight: f32, + confidence_cap: f32, + ) -> Option<&BallHypothesis> { + let confirmation_confidence = 3.0_f32.max(validity_threshold); + let candidates = self.hypotheses.iter().filter_map(|hypothesis| { + let weight = confidence_weight(hypothesis); + let effective_validity = hypothesis.validity * weight; + (effective_validity >= validity_threshold).then_some(( + hypothesis, + // Cap before applying the field prior only when comparing + // possible replacements for a stale, established track. + hypothesis.validity.min(confirmation_confidence) * weight, + effective_validity, + )) + }); + // Preserve accumulated confidence and the existing soft field prior by + // default. The experimental selection cap retains confirmation and + // eligibility requirements and never changes stored confidence. + let cap_enabled = confidence_cap.is_finite() && confidence_cap > 0.0; + let rank = |hypothesis: &BallHypothesis, validity: f32| { + let validity = if cap_enabled { + hypothesis + .validity + .min(confidence_cap.max(confirmation_confidence)) + * confidence_weight(hypothesis) + } else { + validity + }; + let weight = if uncertainty_weight.is_finite() { + uncertainty_weight.max(0.0) + } else { + 0.0 + }; + validity / (1.0 + weight * hypothesis.position_covariance().trace().max(0.0)) + }; + let established_incumbent = ((uncertainty_weight.is_finite() && uncertainty_weight > 0.0) + || cap_enabled) + && candidates + .clone() + .max_by(|(_, _, a), (_, _, b)| a.total_cmp(b)) + .is_some_and(|(track, _, _)| track.validity >= confirmation_confidence); + let (incumbent, incumbent_recovery_rank, _) = candidates + .clone() + // A newborn's small covariance is not independent confirmation. + // Require the same support as stale-track recovery before it + // can displace an established track on uncertainty alone. + .filter(|(track, _, _)| { + !established_incumbent || track.validity >= confirmation_confidence + }) + .max_by(|(a, _, validity_a), (b, _, validity_b)| { + rank(a, *validity_a).total_cmp(&rank(b, *validity_b)) + })?; + let recovered = candidates + .filter(|(candidate, recovery_rank, _)| { + candidate.validity >= confirmation_confidence + && candidate.last_seen > incumbent.last_seen + && candidate.last_seen.duration_since(incumbent.last_seen) + > STALE_TRACK_RECOVERY_GRACE + && *recovery_rank >= incumbent_recovery_rank + }) + .max_by(|(a, rank_a, validity_a), (b, rank_b, validity_b)| { + rank_a + .total_cmp(rank_b) + .then_with(|| a.last_seen.cmp(&b.last_seen)) + .then_with(|| validity_a.total_cmp(validity_b)) + }) + .map(|(hypothesis, _, _)| hypothesis); + // Selection does not change stored validity, output eligibility, decay, + // or timeout. A lone false observation cannot trigger stale recovery. + Some(recovered.unwrap_or(incumbent)) } pub fn decay_hypotheses(&mut self, decay_factor_criterion: impl Fn(&BallHypothesis) -> f32) { @@ -63,21 +164,16 @@ impl BallFilter { ) -> Vec { let (valid, removed): (Vec<_>, Vec<_>) = self.hypotheses.drain(..).partition(is_valid); - self.hypotheses = valid - .into_iter() - .fold(vec![], |mut deduplicated, hypothesis| { - let mergeable_hypothesis = deduplicated - .iter_mut() - .find(|existing_hypothesis| merge_criterion(existing_hypothesis, &hypothesis)); - - if let Some(mergeable_hypothesis) = mergeable_hypothesis { - mergeable_hypothesis.merge(hypothesis) - } else { - deduplicated.push(hypothesis); - } - - deduplicated + let mut deduplicated: Vec = Vec::new(); + for hypothesis in valid { + let merged = deduplicated.iter_mut().any(|existing| { + merge_criterion(existing, &hypothesis) && existing.merge(&hypothesis) }); + if !merged { + deduplicated.push(hypothesis); + } + } + self.hypotheses = deduplicated; removed } @@ -87,41 +183,352 @@ impl BallFilter { detection_time: Time, measurement: MultivariateNormalDistribution<2>, initial_moving_covariance: Matrix4, + nearby_spawn_validity_factor: Option, ) { - let closest_hypothesis = self.hypotheses.iter().min_by_key(|hypothesis| { - NotNan::new(distance( - measurement.mean.framed().as_point(), - hypothesis.position().position, - )) - .expect("distance is nan") - }); - - let mut new_hypothesis = MultivariateNormalDistribution { - mean: closest_hypothesis.map_or( - nalgebra::vector![measurement.mean.x, measurement.mean.y, 0.0, 0.0], - |hypothesis| { - let old_position = hypothesis.position().position.inner.coords; - nalgebra::vector![old_position.x, old_position.y, 0.0, 0.0] - }, - ), + // Nearby recent support can survive a tight association gate without + // manufacturing confidence or copying an old position/unknown velocity. + // Current-exposure matches (including other newborns) cannot donate. + const MAXIMUM_PARENT_AGE: Duration = Duration::from_millis(250); + const MAXIMUM_PARENT_DISTANCE: f32 = 0.2; + let factor = nearby_spawn_validity_factor + .filter(|factor| factor.is_finite()) + .unwrap_or(0.0) + .clamp(0.0, 1.0); + let parent = (factor > 0.0) + .then(|| { + self.hypotheses + .iter() + .enumerate() + .filter_map(|(index, hypothesis)| { + if !hypothesis.validity.is_finite() + || hypothesis.validity < 3.0 + || hypothesis.last_seen >= detection_time + || detection_time.duration_since(hypothesis.last_seen) + > MAXIMUM_PARENT_AGE + { + return None; + } + let distance = + (hypothesis.position().position.inner.coords - measurement.mean).norm(); + (distance.is_finite() && distance <= MAXIMUM_PARENT_DISTANCE) + .then_some((index, distance)) + }) + .min_by(|(_, left), (_, right)| left.total_cmp(right)) + .map(|(index, _)| index) + }) + .flatten(); + let bonus = if let Some(parent) = parent { + let parent = &mut self.hypotheses[parent]; + let bonus = factor * (parent.validity - 1.0).clamp(0.0, 2.0); + parent.validity -= bonus; + bonus + } else { + 0.0 + }; + // An unmatched percept represents a new ball or an abrupt motion change. + // Starting at the nearest old track biases the new position toward an + // unrelated ball and cannot infer velocity without a temporal match. + let new_hypothesis = MultivariateNormalDistribution { + mean: nalgebra::vector![measurement.mean.x, measurement.mean.y, 0.0, 0.0], covariance: initial_moving_covariance, }; - if closest_hypothesis.is_some() { - KalmanFilter::update( - &mut new_hypothesis, - Matrix2x4::identity(), - measurement.mean, - measurement.covariance, - ) - } - let new_hypothesis = BallHypothesis { mode: BallMode::Moving(new_hypothesis), last_seen: detection_time, - validity: 1.0, + validity: 1.0 + bonus, + motion_evidence: None, + negative_evidence: None, + validity_decay_evidence: None, + leadership_evidence: None, + last_observation_size_plausible: None, + merge_observation_start: None, }; self.hypotheses.push(new_hypothesis) } } + +#[cfg(test)] +mod tests { + use super::*; + + fn track(x: f32, validity: f32, last_seen: i64) -> BallHypothesis { + let mut track = BallHypothesis::new( + MultivariateNormalDistribution { + mean: nalgebra::vector![x, 0.0, 0.0, 0.0], + covariance: Matrix4::identity(), + }, + Time::from_nanos(last_seen), + ); + track.validity = validity; + track + } + + fn spawn_nearby(filter: &mut BallFilter, x: f32, factor: Option) { + filter.spawn( + Time::from_nanos(300_000_000), + MultivariateNormalDistribution { + mean: nalgebra::vector![x, 0.0], + covariance: Matrix2::identity() * 0.01, + }, + Matrix4::identity() * 0.2, + factor, + ); + } + + #[test] + fn uncertainty_ranking_preserves_eligibility_and_stored_confidence() { + let mut uncertain = track(4.0, 10.0, 100_000_000); + if let BallMode::Moving(state) = &mut uncertain.mode { + state.covariance *= 100.0; + } + let precise = track(0.5, 5.0, 100_000_000); + let filter = BallFilter { + hypotheses: vec![uncertain, precise], + }; + assert_eq!( + filter + .select_hypothesis(0.5, |_| 1.0, 0.0, 0.0) + .unwrap() + .validity, + 10.0 + ); + assert_eq!( + filter + .select_hypothesis(0.5, |_| 1.0, 1.0, 0.0) + .unwrap() + .validity, + 5.0 + ); + // Penalizing uncertainty cannot hide the only eligible track. + assert_eq!( + filter + .select_hypothesis(6.0, |_| 1.0, 1.0, 0.0) + .unwrap() + .validity, + 10.0 + ); + assert!(filter.select_hypothesis(11.0, |_| 1.0, 1.0, 0.0).is_none()); + assert_eq!(filter.hypotheses[0].validity, 10.0); + assert_eq!(filter.hypotheses[1].validity, 5.0); + } + + #[test] + fn capped_selection_preserves_confirmation_eligibility_and_stored_confidence() { + let mut established = track(1.0, 50.0, 0); + let mut recent = track(2.0, 4.0, 0); + if let BallMode::Moving(state) = &mut established.mode { + state.covariance *= 10.0; + } + if let BallMode::Moving(state) = &mut recent.mode { + state.covariance *= 0.1; + } + let mut filter = BallFilter { + hypotheses: vec![established, recent], + }; + assert_eq!( + filter + .select_hypothesis(0.5, |_| 1.0, 1.0, 5.0) + .unwrap() + .validity, + 4.0 + ); + filter.hypotheses[1].validity = 2.0; + assert_eq!( + filter + .select_hypothesis(0.5, |_| 1.0, 1.0, 5.0) + .unwrap() + .validity, + 50.0 + ); + assert_eq!( + filter + .select_hypothesis(10.0, |_| 1.0, 1.0, 5.0) + .unwrap() + .validity, + 50.0 + ); + assert_eq!(filter.hypotheses[0].validity, 50.0); + assert!(filter.select_hypothesis(100.0, |_| 1.0, 1.0, 5.0).is_none()); + } + + #[test] + fn nearby_birth_transfers_bounded_confidence_without_copying_position_or_velocity() { + for (factor, expected) in [ + (None, 1.0), + (Some(0.0), 1.0), + (Some(0.5), 2.0), + (Some(1.0), 3.0), + (Some(5.0), 3.0), + (Some(f32::NAN), 1.0), + ] { + let mut parent = track(1.0, 50.0, 100_000_000); + if let BallMode::Moving(state) = &mut parent.mode { + state.mean.z = 5.0; + } + let mut filter = BallFilter { + hypotheses: vec![parent], + }; + spawn_nearby(&mut filter, 1.1, factor); + let born = &filter.hypotheses[1]; + assert_eq!(born.validity, expected); + assert_eq!(born.position().position, linear_algebra::point![1.1, 0.0]); + assert_eq!(born.position().velocity, linear_algebra::Vector2::zeros()); + assert_eq!(born.position_covariance(), Matrix2::identity() * 0.2); + assert_eq!(born.last_seen, Time::from_nanos(300_000_000)); + assert_eq!( + filter.hypotheses[0].last_seen, + Time::from_nanos(100_000_000) + ); + assert_eq!(filter.hypotheses[0].validity + born.validity, 51.0); + } + } + + #[test] + fn nearby_birth_requires_recent_unmatched_confirmed_parent() { + for parent in [ + track(0.89, 10.0, 100_000_000), // More than 0.2 m away. + track(1.0, 10.0, 49_999_999), // Older than 250 ms. + track(1.0, 10.0, 300_000_000), // Already observed in this image. + track(1.0, 10.0, 320_000_000), // Future observation is not evidence. + track(1.0, 2.99, 100_000_000), + track(1.0, f32::INFINITY, 100_000_000), + ] { + let validity = parent.validity; + let mut filter = BallFilter { + hypotheses: vec![parent], + }; + spawn_nearby(&mut filter, 1.1, Some(1.0)); + assert_eq!(filter.hypotheses[1].validity, 1.0); + assert_eq!(filter.hypotheses[0].validity, validity); + } + } + + #[test] + fn multiple_births_cannot_clone_confidence_or_reuse_newborns_as_parents() { + let mut filter = BallFilter { + hypotheses: vec![track(1.0, 3.0, 100_000_000)], + }; + spawn_nearby(&mut filter, 1.1, Some(1.0)); + spawn_nearby(&mut filter, 1.15, Some(1.0)); + assert_eq!( + filter + .hypotheses + .iter() + .map(|h| h.validity) + .collect::>(), + vec![1.0, 3.0, 1.0] + ); + assert_eq!( + filter.hypotheses.iter().map(|h| h.validity).sum::(), + 5.0 + ); + } + + #[test] + fn nearby_birth_uses_nearest_parent_after_odometry_compensation() { + let mut filter = BallFilter { + hypotheses: vec![track(1.0, 10.0, 100_000_000), track(1.08, 8.0, 100_000_000)], + }; + filter.predict( + Duration::ZERO, + Isometry2::from_parts(linear_algebra::vector![-0.5, 0.0], 0.0), + 1.0, + Matrix4::zeros(), + Matrix2::zeros(), + f32::INFINITY, + ); + spawn_nearby(&mut filter, 0.6, Some(0.5)); + assert_eq!(filter.hypotheses[0].validity, 10.0); + assert_eq!(filter.hypotheses[1].validity, 7.0); + assert_eq!(filter.hypotheses[2].validity, 2.0); + } + + #[test] + fn confirmed_recent_track_can_replace_long_observed_stale_track() { + let mut filter = BallFilter { + hypotheses: vec![track(0.0, 25.0, 0), track(3.0, 1.0, 500_000_000)], + }; + assert_eq!( + filter.best_hypothesis(0.5).unwrap().position().position.x(), + 0.0 + ); + filter.hypotheses[1].validity = 2.99; + assert_eq!( + filter.best_hypothesis(0.5).unwrap().position().position.x(), + 0.0 + ); + filter.hypotheses[1].validity = 3.0; + assert_eq!( + filter.best_hypothesis(0.5).unwrap().position().position.x(), + 3.0 + ); + assert_eq!(filter.hypotheses[0].validity, 25.0); + assert_eq!(filter.hypotheses[0].last_seen, Time::zero()); + } + + #[test] + fn confirmed_competitor_cannot_take_over_during_short_perception_interruptions() { + let mut filter = BallFilter { + hypotheses: vec![track(0.0, 25.0, 0), track(3.0, 7.0, 40_000_000)], + }; + for observed in [40_000_000, 80_000_000, 120_000_000] { + filter.hypotheses[1].last_seen = Time::from_nanos(observed); + assert_eq!( + filter.best_hypothesis(0.5).unwrap().position().position.x(), + 0.0 + ); + } + filter.hypotheses[1].last_seen = Time::from_nanos(120_000_001); + assert_eq!( + filter.best_hypothesis(0.5).unwrap().position().position.x(), + 3.0 + ); + + // A competing track observed during intermittent one-image dropouts + // must not repeatedly steal selection from the well-established ball. + for observed in [200_000_000, 280_000_000, 360_000_000] { + filter.hypotheses[0].last_seen = Time::from_nanos(observed); + filter.hypotheses[1].last_seen = Time::from_nanos(observed + 40_000_000); + assert_eq!( + filter.best_hypothesis(0.5).unwrap().position().position.x(), + 0.0 + ); + } + } + + #[test] + fn simultaneously_observed_balls_keep_confidence_order_in_either_storage_order() { + let mut filter = BallFilter { + hypotheses: vec![track(0.0, 25.0, 1_000_000), track(3.0, 7.0, 1_000_000)], + }; + assert_eq!( + filter.best_hypothesis(0.5).unwrap().position().position.x(), + 0.0 + ); + filter.hypotheses.reverse(); + assert_eq!( + filter.best_hypothesis(0.5).unwrap().position().position.x(), + 0.0 + ); + } + + #[test] + fn high_output_threshold_still_requires_enough_evidence() { + let mut filter = BallFilter { + hypotheses: vec![track(0.0, 25.0, 0), track(3.0, 4.0, 500_000_000)], + }; + assert_eq!( + filter.best_hypothesis(5.0).unwrap().position().position.x(), + 0.0 + ); + filter.hypotheses[1].validity = 5.0; + assert_eq!( + filter.best_hypothesis(5.0).unwrap().position().position.x(), + 3.0 + ); + filter.hypotheses.remove(0); + assert!(filter.best_hypothesis(5.1).is_none()); + } +} diff --git a/crates/nodes/ball_filter/src/hypothesis.rs b/crates/nodes/ball_filter/src/hypothesis.rs index 549c2f803d..72b8527b21 100644 --- a/crates/nodes/ball_filter/src/hypothesis.rs +++ b/crates/nodes/ball_filter/src/hypothesis.rs @@ -1,8 +1,7 @@ use std::{f32::consts::PI, time::Duration}; -use filtering::kalman_filter::KalmanFilter; use moving::{MovingPredict, MovingUpdate}; -use nalgebra::{Matrix2, Matrix4}; +use nalgebra::{Matrix2, Matrix4, SMatrix}; use resting::{RestingPredict, RestingUpdate}; use ros_z::{Message, time::Time}; use serde::{Deserialize, Serialize}; @@ -14,6 +13,9 @@ use types::{ ball_position::BallPosition, multivariate_normal_distribution::MultivariateNormalDistribution, }; +mod motion_evidence; +pub use motion_evidence::MotionEvidence; + pub mod moving; pub mod resting; @@ -27,7 +29,25 @@ pub enum BallMode { pub struct BallHypothesis { pub mode: BallMode, pub last_seen: Time, + /// Conservative interval of merged observation support through last_seen. + /// Retained across predictions; a genuinely new matched observation resets it. + #[serde(default)] + pub merge_observation_start: Option