From df871fa9ed4e06a029dd705feb8b17c726253f14 Mon Sep 17 00:00:00 2001 From: Fred Huang Date: Mon, 26 Oct 2026 13:51:05 +0000 Subject: [PATCH 1/4] fix(vision): preserve plausible nearby ball observations Prefer plausible nearby alternatives before per-camera confidence selection, and keep each camera from overweighting close duplicate observations. Preserve observed ball/robot geometry when independent smoothing would move the ball inside its robot, while retaining the existing estimate during camera loss. Add proximity-boundary, confidence, field/goal, camera-loss and minimized real-log regression tests. --- .../refiners/filters/kalman.py | 6 + .../data_processing/refiners/position.py | 151 +++++++++-- .../tests/refiners/camera_combiner_test.py | 6 +- .../tests/refiners/one_camera_ball_repro.json | 107 ++++++++ .../refiners/test_local_camera_consistency.py | 254 ++++++++++++++++++ 5 files changed, 498 insertions(+), 26 deletions(-) create mode 100644 utama_core/tests/refiners/one_camera_ball_repro.json create mode 100644 utama_core/tests/refiners/test_local_camera_consistency.py diff --git a/utama_core/data_processing/refiners/filters/kalman.py b/utama_core/data_processing/refiners/filters/kalman.py index 29f2236c..2bb18578 100644 --- a/utama_core/data_processing/refiners/filters/kalman.py +++ b/utama_core/data_processing/refiners/filters/kalman.py @@ -393,6 +393,12 @@ def _step( return tuple(self.state) + def correct_position(self, position: Vector3D) -> None: + """Keep prediction consistent after a current local-geometry correction.""" + self.state = np.array((position.x, position.y, position.z)) + self._lost = None + self._unseen_s = 0.0 + def _rolled(self, p0: np.ndarray, v0: np.ndarray, t: float) -> np.ndarray: """Where a ball at `p0` rolling at `v0` is after `t` s of `BALL_ROLL_DECEL`, within `bounds`.""" speed = math.hypot(v0[0], v0[1]) diff --git a/utama_core/data_processing/refiners/position.py b/utama_core/data_processing/refiners/position.py index 6e31ffae..11970d8c 100644 --- a/utama_core/data_processing/refiners/position.py +++ b/utama_core/data_processing/refiners/position.py @@ -2,11 +2,12 @@ from collections import defaultdict from dataclasses import dataclass, replace from functools import partial -from typing import Dict, List, Optional, Tuple +from typing import Dict, List, Optional, Sequence, Tuple import numpy as np from utama_core.config.field_params import FieldDimensions +from utama_core.config.physical_constants import BALL_RADIUS, ROBOT_RADIUS from utama_core.config.settings import ( BALL_MERGE_THRESHOLD, BALL_ROBOT_OFFSET_MEMORY, @@ -26,6 +27,12 @@ from utama_core.global_utils.mapping_utils import map_friendly_enemy_to_colors +def _in_field_or_goal(x: float, y: float, dimensions: FieldDimensions) -> bool: + if abs(y) <= dimensions.full_field_half_width and abs(x) <= dimensions.full_field_half_length: + return True + return abs(y) <= dimensions.half_goal_width and abs(x) <= dimensions.full_field_half_length + dimensions.goal_depth + + @dataclass class VisionBounds: x_min: float @@ -83,7 +90,7 @@ def __init__( False # Only start filtering once we have valid data to filter (i.e. after the first valid game frame) ) - self._combiner = CameraCombiner() + self._combiner = CameraCombiner(full_field_dims) self._field_dims = full_field_dims self.exp_ball = exp_ball self.allowed_yellow_ids = allowed_yellow_ids @@ -105,19 +112,30 @@ def refine( game_frame: GameFrame, data: List[RawVisionData], ) -> GameFrame: - frames = [frame for frame in data if frame is not None] + frames = [ + replace( + frame, + yellow_robots=[ + r for r in frame.yellow_robots if self.allowed_yellow_ids is None or r.id in self.allowed_yellow_ids + ], + blue_robots=[ + r for r in frame.blue_robots if self.allowed_blue_ids is None or r.id in self.allowed_blue_ids + ], + ) + for frame in data + if frame is not None + ] # If no information just return the original if not frames: return game_frame - # class VisionData: ts: float; yellow_robots: List[VisionRobotData]; blue_robots: List[VisionRobotData]; balls: List[VisionBallData] - # class VisionRobotData: id: int; x: float; y: float; orientation: float combined_vision_data: VisionData = self._combiner.combine_cameras( frames, bounds=self.vision_bounds, ) + measurement_vision = combined_vision_data time_elapsed = combined_vision_data.ts - game_frame.ts # Kalman smoothing and vanished-robot imputation share the same predict/update filter, @@ -189,6 +207,7 @@ def refine( ) new_ball = self._pick_ball(combined_vision_data.balls, game_frame.ball) + measured_ball = new_ball # Skip filtering when there's no ball and we don't expect one if new_ball is not None or self.exp_ball: @@ -210,6 +229,10 @@ def refine( # from this ball rather than carrying on the old roll. self.kalman_filter_ball = self._ball_filter() + new_ball = self._preserve_local_geometry( + new_ball, measured_ball, measurement_vision, new_yellow_robots, new_blue_robots + ) + if game_frame.my_team_is_yellow: new_game_frame = replace( game_frame, @@ -229,6 +252,41 @@ def refine( return new_game_frame + def _preserve_local_geometry( + self, + ball: Optional[Ball], + measured_ball: Optional[Ball], + measurements: VisionData, + yellows: dict[int, Robot], + blues: dict[int, Robot], + ) -> Optional[Ball]: + """Do not let separate smoothing put an observed dribbler-side ball inside its robot. + + This uses a current measured relative vector, not assumed possession or a global + collision clamp. Distant balls, missing measurements and measurements already inside + the robot are left alone. + """ + if ball is None or measured_ball is None: + return ball + for observed, refined in ((measurements.yellow_robots, yellows), (measurements.blue_robots, blues)): + for raw in observed: + robot = refined.get(raw.id) + if robot is None: + continue + dx, dy = measured_ball.p.x - raw.x, measured_ball.p.y - raw.y + distance = math.hypot(dx, dy) + forward = dx * math.cos(raw.orientation) + dy * math.sin(raw.orientation) + lateral = -dx * math.sin(raw.orientation) + dy * math.cos(raw.orientation) + if not (ROBOT_RADIUS <= distance <= BALL_ROBOT_SHIFT_RANGE and forward >= abs(lateral)): + continue + if math.hypot(ball.p.x - robot.p.x, ball.p.y - robot.p.y) >= ROBOT_RADIUS: + continue + position = Vector3D(robot.p.x + dx, robot.p.y + dy, ball.p.z) + if self._uses_kalman and self.kalman_filter_ball.state is not None: + self.kalman_filter_ball.correct_position(position) + return replace(ball, p=position) + return ball + def reset(self): """ Resets the internal state of the refiner, including Kalman filters and vanishing trackers. @@ -336,19 +394,13 @@ def _pick_ball(self, balls: List[VisionBallData], last: Optional[Ball]) -> Optio leaves the field, stray orange things off it do: 2026-10-22, four steady "balls" 15 cm past the touch line, as confident as the real one), then the nearest to `last`, the ball the frame before. With no last ball, the most confident.""" - inside = [b for b in balls if self._in_field_or_goal(b.x, b.y)] + inside = [b for b in balls if _in_field_or_goal(b.x, b.y, self._field_dims)] candidates = inside or balls if last is None or not candidates: return PositionRefiner._get_most_confident_ball(candidates) nearest = min(candidates, key=lambda b: math.hypot(b.x - last.p.x, b.y - last.p.y)) return PositionRefiner._ball_from_vision(nearest) - def _in_field_or_goal(self, x: float, y: float) -> bool: - d = self._field_dims - if abs(y) <= d.full_field_half_width and abs(x) <= d.full_field_half_length: - return True - return abs(y) <= d.half_goal_width and abs(x) <= d.full_field_half_length + d.goal_depth - @staticmethod def _get_most_confident_ball(balls: List[VisionBallData]) -> Ball: balls_by_confidence = sorted(balls, key=lambda ball: ball.confidence, reverse=True) @@ -418,7 +470,7 @@ class CameraCombiner: best-placed camera's is dropped, so a camera that disagrees (a poor calibration far from it) does not pull the robot away. Without camera positions every camera counts the same. A ball is fused by the same rule, its detections grouped by being within `BALL_MERGE_THRESHOLD` - of each other: fused one way and the robots another, a ball at a robot's dribbler came out + of every other member: fused one way and the robots another, a ball at a robot's dribbler came out inside the robot (2026-10-22). Each camera's ball is first moved by that camera's offset at the nearest robot it also sees (within `BALL_ROBOT_SHIFT_RANGE`): the fused robot minus its own detection of it. So a ball seen by one camera only (the robot hides it from the other) @@ -428,7 +480,8 @@ class CameraCombiner: calibrations' disagreement, steady over that time. Without it the ball came out inside the robot (2.4% of one 2026-10-22 log).""" - def __init__(self): + def __init__(self, full_field_dims: Optional[FieldDimensions] = None): + self._field_dims = full_field_dims # (camera id, other camera id, (colour, robot id)) -> (camera's detection minus the other's # in x, y; frame time) self._gaps: Dict[tuple, Tuple[float, float, float]] = {} @@ -448,6 +501,14 @@ def combine_cameras( VisionData: A combined VisionData object containing averaged robot positions and the most confident ball position. """ + frames = [ + replace( + frame, + yellow_robots=self._nearby_robot_duplicates(frame.yellow_robots), + blue_robots=self._nearby_robot_duplicates(frame.blue_robots), + ) + for frame in frames + ] ts = [] # maps robot id to the (detection, its camera's position) of every camera that saw it yellow_captured = defaultdict(list) @@ -499,11 +560,34 @@ def combine_cameras( if near is not None and math.hypot(near[0] - b.x, near[1] - b.y) <= BALL_ROBOT_SHIFT_RANGE: x, y, f = near b = RawBallData(b.x + f.x - x, b.y + f.y - y, b.z, b.confidence) - shifted.append((b, frame.camera_position)) - balls = self._combine_balls_by_proximity(shifted) + shifted.append((b, frame.camera_position, frame.camera_id)) + balls = self._combine_balls_by_proximity(shifted, avg_yellows + avg_blues) return VisionData(sum(ts) / len(ts), avg_yellows, avg_blues, balls) + @staticmethod + def _nearby_robot_duplicates(robots: List[RawRobotData]) -> List[RawRobotData]: + """Nearby repeats of one ID in an image are one camera observation.""" + by_id = defaultdict(list) + for robot in robots: + by_id[robot.id].append(robot) + result = [] + for robot_id, seen in by_id.items(): + if len(seen) < 2 or any(math.hypot(a.x - b.x, a.y - b.y) > CAMERA_AGREEMENT for a in seen for b in seen): + result.extend(seen) + continue + n = len(seen) + result.append( + RawRobotData( + robot_id, + sum(r.x for r in seen) / n, + sum(r.y for r in seen) / n, + math.atan2(sum(math.sin(r.orientation) for r in seen), sum(math.cos(r.orientation) for r in seen)), + max(r.confidence for r in seen), + ) + ) + return result + @staticmethod def _view_weight(x: float, y: float, camera: Optional[Tuple[float, float, float]]) -> float: """cos^4 of the angle from the vertical at which `camera` sees (`x`, `y`); 1 when unknown.""" @@ -539,20 +623,39 @@ def _avg_robots(self, seen: List[Tuple[RawRobotData, Optional[Tuple[float, float ) return VisionRobotData(base_id, x, y, avg_orientation) - def _combine_balls_by_proximity(self, seen: list) -> List[VisionBallData]: - """One ball per group of detections (`seen`: (detection, camera position)) within - `BALL_MERGE_THRESHOLD` of the group's first, fused like a robot.""" + def _plausible_ball(self, ball: RawBallData, robots: Sequence[VisionRobotData]) -> bool: + in_field = self._field_dims is None or _in_field_or_goal(ball.x, ball.y, self._field_dims) + clear_of_robots = ball.z > BALL_RADIUS or all( + math.hypot(ball.x - robot.x, ball.y - robot.y) >= ROBOT_RADIUS for robot in robots + ) + return in_field and clear_of_robots + + def _combine_balls_by_proximity( + self, + seen: list[tuple[RawBallData, Optional[tuple[float, float, float]], int]], + robots: Sequence[VisionRobotData] = (), + ) -> List[VisionBallData]: + """Fuse close observations, using the strongest candidate per camera in each group.""" groups: list = [] - for b, camera in seen: + for detection in seen: + ball = detection[0] for group in groups: - if math.hypot(b.x - group[0][0].x, b.y - group[0][0].y) < BALL_MERGE_THRESHOLD: - group.append((b, camera)) + if all(math.hypot(ball.x - b.x, ball.y - b.y) < BALL_MERGE_THRESHOLD for b, _, _ in group): + group.append(detection) break else: - groups.append([(b, camera)]) + groups.append([detection]) balls = [] for group in groups: - kept = self._fuse(group) + # Prefer a plausible nearby alternative before confidence can discard it. + # If every candidate is uncertain, retain the existing measurement path. + candidates = [observation for observation in group if self._plausible_ball(observation[0], robots)] or group + by_camera = {} + for ball, camera, camera_id in candidates: + previous = by_camera.get(camera_id) + if previous is None or ball.confidence > previous[0].confidence: + by_camera[camera_id] = (ball, camera) + kept = self._fuse(list(by_camera.values())) total = sum(w for _, w in kept) balls.append( RawBallData( diff --git a/utama_core/tests/refiners/camera_combiner_test.py b/utama_core/tests/refiners/camera_combiner_test.py index 5da6cbab..18678c74 100644 --- a/utama_core/tests/refiners/camera_combiner_test.py +++ b/utama_core/tests/refiners/camera_combiner_test.py @@ -57,7 +57,7 @@ def test_combine_proximity_multiple_balls(): (RawBallData(0.02, 0.03, 0, 1), None), (RawBallData(1, 1, 0, 1), None), ] - balls = CameraCombiner()._combine_balls_by_proximity(seen) + balls = CameraCombiner()._combine_balls_by_proximity([(b, camera, i) for i, (b, camera) in enumerate(seen)]) assert len(balls) == 3 assert (balls[0].x, balls[0].y) == (0.01, 0.015) @@ -78,7 +78,9 @@ def test_combine_filters_low_confidence(): (RawBallData(5, 5, 0, 0.05), None), (RawBallData(0.02, 0.02, 0, 0.05), None), ] - assert len(CameraCombiner()._combine_balls_by_proximity(seen)) == 2 + assert ( + len(CameraCombiner()._combine_balls_by_proximity([(b, camera, i) for i, (b, camera) in enumerate(seen)])) == 2 + ) # --- Bounds filtering tests --- diff --git a/utama_core/tests/refiners/one_camera_ball_repro.json b/utama_core/tests/refiners/one_camera_ball_repro.json new file mode 100644 index 00000000..7b5565a0 --- /dev/null +++ b/utama_core/tests/refiners/one_camera_ball_repro.json @@ -0,0 +1,107 @@ +[ + [ + { + "ts": 1792699911.262031, + "camera_id": 0, + "yellow_robots": [], + "blue_robots": [], + "balls": [], + "camera_position": [ + -1.728633544921875, + -0.0574510383605957, + 2.077995849609375 + ] + }, + { + "ts": 1792699911.259359, + "camera_id": 1, + "yellow_robots": [], + "blue_robots": [ + { + "id": 2, + "x": -0.6380708618164063, + "y": 0.2897994384765625, + "orientation": -0.20379391312599182, + "confidence": 0.9494209885597229 + } + ], + "balls": [ + { + "x": -0.5209290161132812, + "y": 0.290102294921875, + "z": 0.0, + "confidence": 0.8251418471336365 + } + ], + "camera_position": null + } + ], + [ + { + "ts": 1792699911.295273, + "camera_id": 0, + "yellow_robots": [], + "blue_robots": [ + { + "id": 2, + "x": -0.5537928466796875, + "y": 0.28197738647460935, + "orientation": -0.19114355742931366, + "confidence": 0.9518543481826782 + } + ], + "balls": [], + "camera_position": [ + -1.728633544921875, + -0.0574510383605957, + 2.077995849609375 + ] + } + ], + [], + [ + { + "ts": 1792699911.328597, + "camera_id": 0, + "yellow_robots": [], + "blue_robots": [ + { + "id": 2, + "x": -0.5540905151367187, + "y": 0.2820794677734375, + "orientation": -0.19513890147209167, + "confidence": 0.9517627358436584 + } + ], + "balls": [], + "camera_position": [ + -1.728633544921875, + -0.0574510383605957, + 2.077995849609375 + ] + }, + { + "ts": 1792699911.327261, + "camera_id": 1, + "yellow_robots": [], + "blue_robots": [ + { + "id": 2, + "x": -0.638071533203125, + "y": 0.28938409423828126, + "orientation": -0.1888488084077835, + "confidence": 0.9497724175453186 + } + ], + "balls": [ + { + "x": -0.5207621459960937, + "y": 0.2896724853515625, + "z": 0.0, + "confidence": 0.8302320837974548 + } + ], + "camera_position": null + } + ] +] \ No newline at end of file diff --git a/utama_core/tests/refiners/test_local_camera_consistency.py b/utama_core/tests/refiners/test_local_camera_consistency.py new file mode 100644 index 00000000..0f7af267 --- /dev/null +++ b/utama_core/tests/refiners/test_local_camera_consistency.py @@ -0,0 +1,254 @@ +"""Local camera consistency: one physical object, one contribution per camera.""" + +import json +import math +from pathlib import Path + +import pytest + +from utama_core.config.field_params import FieldDimensions +from utama_core.data_processing.refiners import PositionRefiner, VelocityRefiner +from utama_core.data_processing.refiners.position import CameraCombiner, VisionBounds +from utama_core.entities.data.raw_vision import RawBallData, RawRobotData, RawVisionData +from utama_core.entities.game import GameFrame, GameHistory + +BOUNDS = VisionBounds(-10, 10, -10, 10) + + +def test_nearby_ball_duplicates_use_the_strongest_detection_per_camera(): + frames = [ + RawVisionData(1, [], [], [RawBallData(0, 0, 0, 0.4), RawBallData(0.02, 0, 0, 0.95)], 0), + RawVisionData(1, [], [], [RawBallData(0.1, 0, 0, 1)], 1), + ] + result = CameraCombiner().combine_cameras(frames, BOUNDS) + assert len(result.balls) == 1 + assert result.balls[0].x == pytest.approx(0.06) # camera 0 winner .02; camera 1 .10 + + +def test_nearby_same_id_robot_duplicates_do_not_give_one_camera_extra_weight(): + frames = [ + RawVisionData(1, [], [RawRobotData(2, 0, 0, 0, 1), RawRobotData(2, 0.02, 0, 0, 1)], [], 0), + RawVisionData(1, [], [RawRobotData(2, 0.1, 0, 0, 1)], [], 1), + ] + result = CameraCombiner().combine_cameras(frames, BOUNDS) + assert len(result.blue_robots) == 1 + assert result.blue_robots[0].x == pytest.approx(0.055) + + +def test_close_different_robot_identities_are_not_silently_combined(): + frame = RawVisionData(1, [], [RawRobotData(2, 0, 0, 0, 1), RawRobotData(3, 0.08, 0, 0, 1)], [], 0) + assert {r.id for r in CameraCombiner().combine_cameras([frame], BOUNDS).blue_robots} == {2, 3} + + +def test_real_single_camera_ball_does_not_move_inside_robot_after_filtering(): + # Four minimized control inputs from 20261022_211142; only the far camera sees the ball. + steps = json.loads((Path(__file__).with_name("one_camera_ball_repro.json")).read_text()) + position = PositionRefiner( + FieldDimensions(1.5, 1.125, 0.25, 0.5, 0.4, 0.4), + allowed_blue_ids=frozenset({2}), + allowed_yellow_ids=frozenset(), + ) + velocity = VelocityRefiner() + history = GameHistory(20) + current = GameFrame(0, False, False, {}, {}, None) + for step in steps: + frames = [ + RawVisionData( + s["ts"], + [RawRobotData(**r) for r in s["yellow_robots"]], + [RawRobotData(**r) for r in s["blue_robots"]], + [RawBallData(**b) for b in s["balls"]], + s["camera_id"], + s["camera_position"], + ) + for s in step + ] + new = position.refine(current, frames) + if position.filter_running: + new = velocity.refine(history, new) + elif new.ball is not None: + position.start_filtering() + if current.ts: + history.add_game_frame(current) + current = new + ball = current.ball.p + robot = current.friendly_robots[2].p + assert math.hypot(ball.x - robot.x, ball.y - robot.y) >= 0.09 + # The Kalman state must follow the correction rather than springing back next tick. + assert tuple(position.kalman_filter_ball.state[:2]) == pytest.approx((ball.x, ball.y)) + + +def test_ignored_nearby_robot_id_cannot_displace_the_ball_from_active_robot(): + position = PositionRefiner( + FieldDimensions(1.5, 1.125, 0.25, 0.5, 0.4, 0.4), + filtering=False, + allowed_blue_ids=frozenset({2}), + allowed_yellow_ids=frozenset(), + ) + frames = [ + RawVisionData( + 1, [], [RawRobotData(2, 0, 0, 0, 1), RawRobotData(3, 0.01, 0, 0, 1)], [RawBallData(0.115, 0, 0, 1)], 0 + ), + RawVisionData(1, [], [RawRobotData(2, 0.08, 0, 0, 1)], [], 1), + ] + result = position.refine(GameFrame(0, False, False, {}, {}, None), frames) + assert set(result.friendly_robots) == {2} + assert result.ball.p.x - result.friendly_robots[2].p.x == pytest.approx(0.115) + + +def test_far_camera_ball_keeps_offset_when_nearer_camera_dominates_robot_pose(): + frames = [ + RawVisionData(1, [], [RawRobotData(2, 0.8, 0, 0, 1)], [RawBallData(0.915, 0, 0, 1)], 0, (-1.73, 0, 2.08)), + RawVisionData(1, [], [RawRobotData(2, 0.885, 0, 0, 1)], [], 1, (1.73, 0, 2.08)), + ] + result = CameraCombiner().combine_cameras(frames, BOUNDS) + assert result.blue_robots[0].x > 0.8425 + assert result.balls[0].x - result.blue_robots[0].x == pytest.approx(0.115) + assert frames[0].balls[0].x == pytest.approx(0.915) # original camera observation stays intact + + +@pytest.mark.parametrize("reverse", [False, True]) +def test_a_stronger_distant_candidate_cannot_displace_the_nearby_track(reverse): + position = PositionRefiner(FieldDimensions(1.5, 1.125, 0.25, 0.5, 0.4, 0.4), filtering=False) + start = RawVisionData(1, [], [], [RawBallData(0, 0, 0, 0.8)], 0) + current = position.refine(GameFrame(0, False, False, {}, {}, None), [start]) + candidates = [RawBallData(0.02, 0, 0, 0.8), RawBallData(0.04, 0, 0, 0.9), RawBallData(1.4, 0, 0, 1)] + if reverse: + candidates.reverse() + frame = RawVisionData(1.02, [], [], candidates, 0) + result = position.refine(current, [frame]) + assert result.ball.p.x == pytest.approx(0.04) + assert len(frame.balls) == 3 # do not alter original camera observations + + +def test_a_chain_of_nearby_candidates_cannot_merge_two_distant_endpoints(): + frames = [RawVisionData(1, [], [], [RawBallData(x, 0, 0, 1)], i) for i, x in enumerate((0, -0.19, 0.19))] + balls = CameraCombiner().combine_cameras(frames, BOUNDS).balls + assert len(balls) == 2 # endpoints are 380 mm apart, beyond the 200 mm merge limit + + +def test_a_camera_that_loses_the_ball_stops_contributing_to_fusion(): + combiner = CameraCombiner() + combiner.combine_cameras( + [ + RawVisionData(1, [], [], [RawBallData(0, 0, 0, 1)], 0), + RawVisionData(1, [], [], [RawBallData(0.1, 0, 0, 1)], 1), + ], + BOUNDS, + ) + result = combiner.combine_cameras( + [ + RawVisionData(1.02, [], [], [], 0), + RawVisionData(1.02, [], [], [RawBallData(0.12, 0, 0, 1)], 1), + ], + BOUNDS, + ) + assert result.balls[0].x == pytest.approx(0.12) + missing = combiner.combine_cameras([RawVisionData(1.04, [], [], [], 0), RawVisionData(1.04, [], [], [], 1)], BOUNDS) + assert missing.balls == [] + + +@pytest.mark.parametrize("filtering", [False, True]) +def test_strategy_keeps_a_ball_estimate_when_both_cameras_lose_it(filtering): + position = PositionRefiner(FieldDimensions(1.5, 1.125, 0.25, 0.5, 0.4, 0.4), filtering=filtering) + start = RawVisionData(1, [], [], [RawBallData(0.4, 0.2, 0, 1)], 0) + current = position.refine(GameFrame(0, False, False, {}, {}, None), [start]) + position.start_filtering() + for ts in (1.02, 1.04, 2): + result = position.refine(current, [RawVisionData(ts, [], [], [], 0), RawVisionData(ts, [], [], [], 1)]) + assert result.ball is not None + assert (result.ball.p.x, result.ball.p.y) == pytest.approx((0.4, 0.2)) + current = result + assert position.refine(current, [None, None]).ball is current.ball + + +@pytest.mark.parametrize("distance,count", [(0.1999, 1), (0.2, 2), (0.2001, 2)]) +def test_ball_merge_limit_is_strictly_200mm(distance, count): + frames = [ + RawVisionData(1, [], [], [RawBallData(0, 0, 0, 1)], 0), + RawVisionData(1, [], [], [RawBallData(distance, 0, 0, 1)], 1), + ] + assert len(CameraCombiner().combine_cameras(frames, BOUNDS).balls) == count + + +@pytest.mark.parametrize("filtering", [False, True]) +@pytest.mark.parametrize("reverse", [False, True]) +@pytest.mark.parametrize("cameras", [1, 2]) +def test_robot_pattern_cannot_replace_a_plausible_nearby_ball(filtering, reverse, cameras): + position = PositionRefiner( + FieldDimensions(1.5, 1.125, 0.25, 0.5, 0.4, 0.4), filtering=filtering, allowed_blue_ids=frozenset({2}) + ) + + def frames(ts, candidates): + result = [RawVisionData(ts, [], [RawRobotData(2, 0, 0, 0, 1)], candidates, 0)] + if cameras == 2: + result.append(RawVisionData(ts, [], [RawRobotData(2, 0.08, 0, 0, 1)], [], 1)) + return result + + current = position.refine(GameFrame(0, False, False, {}, {}, None), frames(1, [RawBallData(0.115, 0, 0, 0.9)])) + position.start_filtering() + candidates = [RawBallData(0.115, 0, 0, 0.9), RawBallData(0, 0, 0, 0.99)] + if reverse: + candidates.reverse() + observation = frames(1.02, candidates) + result = position.refine(current, observation) + assert result.ball.p.x - result.friendly_robots[2].p.x == pytest.approx(0.115) + assert observation[0].balls == candidates # raw observations remain intact + + +@pytest.mark.parametrize("filtering", [False, True]) +@pytest.mark.parametrize("reverse", [False, True]) +def test_in_field_ball_is_not_discarded_for_a_stronger_nearby_outside_candidate(filtering, reverse): + position = PositionRefiner(FieldDimensions(1.5, 1.125, 0.25, 0.5, 0.4, 0.4), filtering=filtering) + current = position.refine( + GameFrame(0, False, False, {}, {}, None), [RawVisionData(1, [], [], [RawBallData(1.4, 0.8, 0, 0.9)], 0)] + ) + position.start_filtering() + candidates = [RawBallData(1.4, 0.8, 0, 0.9), RawBallData(1.55, 0.8, 0, 0.99)] + if reverse: + candidates.reverse() + result = position.refine(current, [RawVisionData(1.02, [], [], candidates, 0)]) + assert result.ball.p.x == pytest.approx(1.4) + + +def test_a_ball_inside_the_goal_is_a_plausible_candidate(): + position = PositionRefiner(FieldDimensions(1.5, 1.125, 0.25, 0.5, 0.4, 0.4), filtering=False) + candidates = [RawBallData(1.4, 0, 0, 0.9), RawBallData(1.55, 0, 0, 0.99)] + result = position.refine(GameFrame(0, False, False, {}, {}, None), [RawVisionData(1, [], [], candidates, 0)]) + assert result.ball.p.x == pytest.approx(1.55) + + +def test_a_raised_ball_is_not_rejected_by_a_flat_robot_footprint(): + position = PositionRefiner(FieldDimensions(1.5, 1.125, 0.25, 0.5, 0.4, 0.4), filtering=False) + frame = RawVisionData( + 1, [], [RawRobotData(2, 0, 0, 0, 1)], [RawBallData(0.115, 0, 0, 0.9), RawBallData(0, 0, 0.2, 0.99)], 0 + ) + result = position.refine(GameFrame(0, False, False, {}, {}, None), [frame]) + assert (result.ball.p.x, result.ball.p.z) == pytest.approx((0, 0.2)) + + +def test_a_single_uncertain_ball_measurement_is_not_erased_by_a_shape_proxy(): + position = PositionRefiner(FieldDimensions(1.5, 1.125, 0.25, 0.5, 0.4, 0.4), filtering=False) + frame = RawVisionData(1, [], [RawRobotData(2, 0, 0, 0, 1)], [RawBallData(0.075, 0, 0, 0.9)], 0) + result = position.refine(GameFrame(0, False, False, {}, {}, None), [frame]) + assert result.ball.p.x == pytest.approx(0.075) + + +def test_a_second_camera_robot_pattern_cannot_pull_a_valid_ball_inside_the_robot(): + position = PositionRefiner(FieldDimensions(1.5, 1.125, 0.25, 0.5, 0.4, 0.4), filtering=False) + frames = [ + RawVisionData(1, [], [RawRobotData(2, 0, 0, 0, 1)], [RawBallData(0.115, 0, 0, 0.9)], 0), + RawVisionData(1, [], [RawRobotData(2, 0.08, 0, 0, 1)], [RawBallData(0.08, 0, 0, 0.99)], 1), + ] + result = position.refine(GameFrame(0, False, False, {}, {}, None), frames) + assert result.ball.p.x - result.friendly_robots[2].p.x == pytest.approx(0.115) + + +def test_a_second_camera_outside_candidate_cannot_pull_a_valid_ball_out_of_field(): + position = PositionRefiner(FieldDimensions(1.5, 1.125, 0.25, 0.5, 0.4, 0.4), filtering=False) + frames = [ + RawVisionData(1, [], [], [RawBallData(1.4, 0.8, 0, 0.9)], 0), + RawVisionData(1, [], [], [RawBallData(1.55, 0.8, 0, 0.99)], 1), + ] + result = position.refine(GameFrame(0, False, False, {}, {}, None), frames) + assert result.ball.p.x == pytest.approx(1.4) From d35e3ab2e2cd43a662410e1662a77dad23b9e94d Mon Sep 17 00:00:00 2001 From: Fred Huang <60901360+fred-huang122@users.noreply.github.com> Date: Mon, 26 Oct 2026 14:40:19 +0000 Subject: [PATCH 2/4] fix(vision): retain valid boundary observations and reset dimensions Filter original robot detections by vision bounds before averaging same-camera repeats, and retain field dimensions when resetting the camera combiner. Add 28 regression cases covering both teams, all four vision boundaries, position bias, and repeated resets with smoothing enabled and disabled. All 168 refiner tests pass; formatting and lint checks pass. --- .../data_processing/refiners/position.py | 10 ++++-- .../refiners/test_local_camera_consistency.py | 35 +++++++++++++++++++ 2 files changed, 42 insertions(+), 3 deletions(-) diff --git a/utama_core/data_processing/refiners/position.py b/utama_core/data_processing/refiners/position.py index 11970d8c..4150f8e0 100644 --- a/utama_core/data_processing/refiners/position.py +++ b/utama_core/data_processing/refiners/position.py @@ -293,7 +293,7 @@ def reset(self): Should be called at the start of each game to ensure no leakage of information between games. """ self._filter_running = False - self._combiner = CameraCombiner() + self._combiner = CameraCombiner(self._field_dims) if self._uses_kalman: self.kalman_filters_yellow = {} self.kalman_filters_blue = {} @@ -504,8 +504,12 @@ def combine_cameras( frames = [ replace( frame, - yellow_robots=self._nearby_robot_duplicates(frame.yellow_robots), - blue_robots=self._nearby_robot_duplicates(frame.blue_robots), + yellow_robots=self._nearby_robot_duplicates( + [r for r in frame.yellow_robots if self._bounds_check(r.x, r.y, bounds)] + ), + blue_robots=self._nearby_robot_duplicates( + [r for r in frame.blue_robots if self._bounds_check(r.x, r.y, bounds)] + ), ) for frame in frames ] diff --git a/utama_core/tests/refiners/test_local_camera_consistency.py b/utama_core/tests/refiners/test_local_camera_consistency.py index 0f7af267..4605da2c 100644 --- a/utama_core/tests/refiners/test_local_camera_consistency.py +++ b/utama_core/tests/refiners/test_local_camera_consistency.py @@ -35,6 +35,24 @@ def test_nearby_same_id_robot_duplicates_do_not_give_one_camera_extra_weight(): assert result.blue_robots[0].x == pytest.approx(0.055) +@pytest.mark.parametrize("colour", ["yellow", "blue"]) +@pytest.mark.parametrize("axis,sign", [("x", -1), ("x", 1), ("y", -1), ("y", 1)]) +@pytest.mark.parametrize("inside", [2.45, 2.49, 2.5]) +def test_outside_robot_repeat_cannot_erase_or_bias_an_inside_observation(colour, axis, sign, inside): + bounds = VisionBounds(-2.5, 2.5, -2.5, 2.5) + x, y = (sign * inside, 0) if axis == "x" else (0, sign * inside) + outside_x, outside_y = (sign * 2.55, 0) if axis == "x" else (0, sign * 2.55) + robots = [RawRobotData(2, x, y, 0.4, 0.9), RawRobotData(2, outside_x, outside_y, 1, 0.99)] + frame = RawVisionData(1, robots if colour == "yellow" else [], robots if colour == "blue" else [], [], 0) + + result = CameraCombiner().combine_cameras([frame], bounds) + + combined = result.yellow_robots if colour == "yellow" else result.blue_robots + assert len(combined) == 1 + assert combined[0].id == 2 + assert (combined[0].x, combined[0].y, combined[0].orientation) == pytest.approx((x, y, 0.4)) + + def test_close_different_robot_identities_are_not_silently_combined(): frame = RawVisionData(1, [], [RawRobotData(2, 0, 0, 0, 1), RawRobotData(3, 0.08, 0, 0, 1)], [], 0) assert {r.id for r in CameraCombiner().combine_cameras([frame], BOUNDS).blue_robots} == {2, 3} @@ -211,6 +229,23 @@ def test_in_field_ball_is_not_discarded_for_a_stronger_nearby_outside_candidate( assert result.ball.p.x == pytest.approx(1.4) +@pytest.mark.parametrize("filtering", [False, True]) +@pytest.mark.parametrize("reverse", [False, True]) +def test_reset_preserves_preference_for_an_in_field_ball(filtering, reverse): + position = PositionRefiner(FieldDimensions(1.5, 1.125, 0.25, 0.5, 0.4, 0.4), filtering=filtering) + candidates = [RawBallData(1.4, 0.8, 0, 0.9), RawBallData(1.55, 0.8, 0, 0.99)] + if reverse: + candidates.reverse() + + for _ in range(3): + current = position.refine(GameFrame(0, False, False, {}, {}, None), [RawVisionData(1, [], [], candidates, 0)]) + assert (current.ball.p.x, current.ball.p.y) == pytest.approx((1.4, 0.8)) + position.start_filtering() + result = position.refine(current, [RawVisionData(1.02, [], [], candidates, 0)]) + assert (result.ball.p.x, result.ball.p.y) == pytest.approx((1.4, 0.8)) + position.reset() + + def test_a_ball_inside_the_goal_is_a_plausible_candidate(): position = PositionRefiner(FieldDimensions(1.5, 1.125, 0.25, 0.5, 0.4, 0.4), filtering=False) candidates = [RawBallData(1.4, 0, 0, 0.9), RawBallData(1.55, 0, 0, 0.99)] From 1997730fe24c5b916c919dd11e1908708357c46e Mon Sep 17 00:00:00 2001 From: Fred Huang <60901360+fred-huang122@users.noreply.github.com> Date: Mon, 26 Oct 2026 14:54:59 +0000 Subject: [PATCH 3/4] fix(ci): separate push and pull request concurrency groups Include the event type in each branch's concurrency group so the push and pull-request checks cannot cancel each other on the same commit. Continue canceling superseded runs for each event and branch. --- .github/workflows/ci.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index a9d7cb4b..d89c6fff 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -4,9 +4,9 @@ on: push: pull_request: -# A PR's push and pull_request events run the same tests: let the later one cancel the earlier. +# Keep push and PR checks independent; only cancel older runs of the same event and branch. concurrency: - group: ${{ github.workflow }}-${{ github.head_ref || github.ref_name }} + group: ${{ github.workflow }}-${{ github.event_name }}-${{ github.head_ref || github.ref_name }} cancel-in-progress: true jobs: From ec25ced97bcf4ea357c810574c645da94ec5f6e3 Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Mon, 26 Oct 2026 17:46:29 +0000 Subject: [PATCH 4/4] fix(vision): keep single-camera ticks in fused frame instead of post-Kalman clamp --- .github/workflows/ci.yml | 4 +- .../refiners/filters/kalman.py | 6 -- .../data_processing/refiners/position.py | 92 ++++++++++--------- .../refiners/test_local_camera_consistency.py | 59 ++++++++++++ 4 files changed, 109 insertions(+), 52 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index d89c6fff..a9d7cb4b 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -4,9 +4,9 @@ on: push: pull_request: -# Keep push and PR checks independent; only cancel older runs of the same event and branch. +# A PR's push and pull_request events run the same tests: let the later one cancel the earlier. concurrency: - group: ${{ github.workflow }}-${{ github.event_name }}-${{ github.head_ref || github.ref_name }} + group: ${{ github.workflow }}-${{ github.head_ref || github.ref_name }} cancel-in-progress: true jobs: diff --git a/utama_core/data_processing/refiners/filters/kalman.py b/utama_core/data_processing/refiners/filters/kalman.py index 2bb18578..29f2236c 100644 --- a/utama_core/data_processing/refiners/filters/kalman.py +++ b/utama_core/data_processing/refiners/filters/kalman.py @@ -393,12 +393,6 @@ def _step( return tuple(self.state) - def correct_position(self, position: Vector3D) -> None: - """Keep prediction consistent after a current local-geometry correction.""" - self.state = np.array((position.x, position.y, position.z)) - self._lost = None - self._unseen_s = 0.0 - def _rolled(self, p0: np.ndarray, v0: np.ndarray, t: float) -> np.ndarray: """Where a ball at `p0` rolling at `v0` is after `t` s of `BALL_ROLL_DECEL`, within `bounds`.""" speed = math.hypot(v0[0], v0[1]) diff --git a/utama_core/data_processing/refiners/position.py b/utama_core/data_processing/refiners/position.py index 4150f8e0..1a443258 100644 --- a/utama_core/data_processing/refiners/position.py +++ b/utama_core/data_processing/refiners/position.py @@ -135,7 +135,6 @@ def refine( bounds=self.vision_bounds, ) - measurement_vision = combined_vision_data time_elapsed = combined_vision_data.ts - game_frame.ts # Kalman smoothing and vanished-robot imputation share the same predict/update filter, @@ -207,7 +206,6 @@ def refine( ) new_ball = self._pick_ball(combined_vision_data.balls, game_frame.ball) - measured_ball = new_ball # Skip filtering when there's no ball and we don't expect one if new_ball is not None or self.exp_ball: @@ -229,10 +227,6 @@ def refine( # from this ball rather than carrying on the old roll. self.kalman_filter_ball = self._ball_filter() - new_ball = self._preserve_local_geometry( - new_ball, measured_ball, measurement_vision, new_yellow_robots, new_blue_robots - ) - if game_frame.my_team_is_yellow: new_game_frame = replace( game_frame, @@ -252,41 +246,6 @@ def refine( return new_game_frame - def _preserve_local_geometry( - self, - ball: Optional[Ball], - measured_ball: Optional[Ball], - measurements: VisionData, - yellows: dict[int, Robot], - blues: dict[int, Robot], - ) -> Optional[Ball]: - """Do not let separate smoothing put an observed dribbler-side ball inside its robot. - - This uses a current measured relative vector, not assumed possession or a global - collision clamp. Distant balls, missing measurements and measurements already inside - the robot are left alone. - """ - if ball is None or measured_ball is None: - return ball - for observed, refined in ((measurements.yellow_robots, yellows), (measurements.blue_robots, blues)): - for raw in observed: - robot = refined.get(raw.id) - if robot is None: - continue - dx, dy = measured_ball.p.x - raw.x, measured_ball.p.y - raw.y - distance = math.hypot(dx, dy) - forward = dx * math.cos(raw.orientation) + dy * math.sin(raw.orientation) - lateral = -dx * math.sin(raw.orientation) + dy * math.cos(raw.orientation) - if not (ROBOT_RADIUS <= distance <= BALL_ROBOT_SHIFT_RANGE and forward >= abs(lateral)): - continue - if math.hypot(ball.p.x - robot.p.x, ball.p.y - robot.p.y) >= ROBOT_RADIUS: - continue - position = Vector3D(robot.p.x + dx, robot.p.y + dy, ball.p.z) - if self._uses_kalman and self.kalman_filter_ball.state is not None: - self.kalman_filter_ball.correct_position(position) - return replace(ball, p=position) - return ball - def reset(self): """ Resets the internal state of the refiner, including Kalman filters and vanishing trackers. @@ -485,6 +444,10 @@ def __init__(self, full_field_dims: Optional[FieldDimensions] = None): # (camera id, other camera id, (colour, robot id)) -> (camera's detection minus the other's # in x, y; frame time) self._gaps: Dict[tuple, Tuple[float, float, float]] = {} + # (colour, robot id) -> (fused robot, {camera id: (x, y)}, frame time) + self._remembered: Dict[Tuple[str, int], Tuple[VisionRobotData, Dict[int, Tuple[float, float]], float]] = {} + # (camera id, (colour, robot id)) -> (x, y, frame time, camera position) + self._last_seen: Dict[tuple, Tuple[float, float, float, Optional[Tuple[float, float, float]]]] = {} def combine_cameras( self, @@ -529,6 +492,7 @@ def combine_cameras( blue_captured[br.id].append((br, frame.camera_position)) ts.append(frame.ts) + frame_ts = sum(ts) / len(ts) avg_yellows = list(map(self._avg_robots, yellow_captured.values())) avg_blues = list(map(self._avg_robots, blue_captured.values())) fused = {("yellow", r.id): r for r in avg_yellows} | {("blue", r.id): r for r in avg_blues} @@ -540,11 +504,46 @@ def combine_cameras( for r in robots: if self._bounds_check(r.x, r.y, bounds): seen_by[(colour, r.id)][frame.camera_id] = (r.x, r.y) + + # Keep single-camera ticks in the fused frame using the last multi-camera bias. + for key, detections in seen_by.items(): + f = fused[key] + if len(detections) >= 2: + self._remembered[key] = (f, dict(detections), frame_ts) + elif len(detections) == 1: + cam_id, (x, y) = next(iter(detections.items())) + rem = self._remembered.get(key) + if rem is None or frame_ts - rem[2] > BALL_ROBOT_OFFSET_MEMORY: + for (other, last_key), (ox, oy, ot, o_pos) in list(self._last_seen.items()): + if last_key == key and other != cam_id and abs(frame_ts - ot) <= 0.05: + captured = yellow_captured[key[1]] if key[0] == "yellow" else blue_captured[key[1]] + f = self._avg_robots(captured + [(RawRobotData(key[1], ox, oy, f.orientation, 1.0), o_pos)]) + self._remembered[key] = (f, {cam_id: (x, y), other: (ox, oy)}, frame_ts) + self._gaps[(cam_id, other, key)] = (x - ox, y - oy, frame_ts) + self._gaps[(other, cam_id, key)] = (ox - x, oy - y, frame_ts) + rem = self._remembered[key] + break + if rem is not None and frame_ts - rem[2] <= BALL_ROBOT_OFFSET_MEMORY and cam_id in rem[1]: + rx, ry = rem[1][cam_id] + fused[key] = VisionRobotData(f.id, x + rem[0].x - rx, y + rem[0].y - ry, f.orientation) + + for frame in frames: + for colour, robots in (("yellow", frame.yellow_robots), ("blue", frame.blue_robots)): + for r in robots: + self._last_seen[(frame.camera_id, (colour, r.id))] = (r.x, r.y, frame.ts, frame.camera_position) + + avg_yellows = [fused[("yellow", r.id)] for r in avg_yellows] + avg_blues = [fused[("blue", r.id)] for r in avg_blues] + shifted = [] + all_keys = set(fused) | { + k for k, rem in self._remembered.items() if frame_ts - rem[2] <= BALL_ROBOT_OFFSET_MEMORY + } for frame in frames: own = [] # (where this camera puts a robot, the fused robot there) - for key, f in fused.items(): - detections = seen_by[key] + for key in all_keys: + f = fused.get(key) or self._remembered[key][0] + detections = seen_by.get(key, {}) if frame.camera_id in detections: x, y = detections[frame.camera_id] for other, (ox, oy) in detections.items(): @@ -557,6 +556,11 @@ def combine_cameras( if gap is not None and frame.ts - gap[2] <= BALL_ROBOT_OFFSET_MEMORY: own.append((ox + gap[0], oy + gap[1], f)) break + else: + rem = self._remembered.get(key) + if rem is not None and frame.ts - rem[2] <= BALL_ROBOT_OFFSET_MEMORY and frame.camera_id in rem[1]: + rx, ry = rem[1][frame.camera_id] + own.append((f.x - (rem[0].x - rx), f.y - (rem[0].y - ry), f)) for b in frame.balls: if not self._bounds_check(b.x, b.y, bounds): continue @@ -567,7 +571,7 @@ def combine_cameras( shifted.append((b, frame.camera_position, frame.camera_id)) balls = self._combine_balls_by_proximity(shifted, avg_yellows + avg_blues) - return VisionData(sum(ts) / len(ts), avg_yellows, avg_blues, balls) + return VisionData(frame_ts, avg_yellows, avg_blues, balls) @staticmethod def _nearby_robot_duplicates(robots: List[RawRobotData]) -> List[RawRobotData]: diff --git a/utama_core/tests/refiners/test_local_camera_consistency.py b/utama_core/tests/refiners/test_local_camera_consistency.py index 4605da2c..567478ec 100644 --- a/utama_core/tests/refiners/test_local_camera_consistency.py +++ b/utama_core/tests/refiners/test_local_camera_consistency.py @@ -7,6 +7,7 @@ import pytest from utama_core.config.field_params import FieldDimensions +from utama_core.config.physical_constants import ROBOT_RADIUS from utama_core.data_processing.refiners import PositionRefiner, VelocityRefiner from utama_core.data_processing.refiners.position import CameraCombiner, VisionBounds from utama_core.entities.data.raw_vision import RawBallData, RawRobotData, RawVisionData @@ -287,3 +288,61 @@ def test_a_second_camera_outside_candidate_cannot_pull_a_valid_ball_out_of_field ] result = position.refine(GameFrame(0, False, False, {}, {}, None), frames) assert result.ball.p.x == pytest.approx(1.4) + + +def test_single_camera_tick_after_two_camera_fusion_does_not_jump_robot_onto_ball(): + cam_positions = {0: (-1.1, 0.0, 1.73), 1: (1.1, 0.0, 1.73)} + + cam0_t1 = RawVisionData( + ts=1.0, + yellow_robots=[], + blue_robots=[RawRobotData(2, -0.554, 1.084, 0.0, 1.0)], + balls=[], + camera_id=0, + camera_position=cam_positions[0], + ) + cam1_t1 = RawVisionData( + ts=1.0, + yellow_robots=[], + blue_robots=[RawRobotData(2, -0.638, 1.093, 0.0, 1.0)], + balls=[RawBallData(-0.521, 1.076, 0.0, 1.0)], + camera_id=1, + camera_position=cam_positions[1], + ) + + cam0_t2 = RawVisionData( + ts=1.016, + yellow_robots=[], + blue_robots=[RawRobotData(2, -0.554, 1.084, 0.0, 1.0)], + balls=[], + camera_id=0, + camera_position=cam_positions[0], + ) + + combiner = CameraCombiner() + fused_t1 = combiner.combine_cameras([cam0_t1, cam1_t1], BOUNDS) + assert len(fused_t1.blue_robots) == 1 + assert fused_t1.blue_robots[0].x == pytest.approx(-0.577, abs=0.01) + assert fused_t1.blue_robots[0].y == pytest.approx(1.086, abs=0.01) + + fused_t2 = combiner.combine_cameras([cam0_t2], BOUNDS) + assert len(fused_t2.blue_robots) == 1 + assert fused_t2.blue_robots[0].x == pytest.approx(-0.577, abs=0.01) + assert fused_t2.blue_robots[0].y == pytest.approx(1.086, abs=0.01) + assert abs(fused_t2.blue_robots[0].x - (-0.554)) > 0.015 + + position = PositionRefiner( + FieldDimensions(1.5, 1.125, 0.25, 0.5, 0.4, 0.4), + allowed_blue_ids=frozenset({2}), + allowed_yellow_ids=frozenset(), + ) + current = GameFrame(0, False, False, {}, {}, None) + frame1 = position.refine(current, [cam0_t1, cam1_t1]) + position.start_filtering() + + frame2 = position.refine(frame1, [cam0_t2]) + assert frame2.ball is not None + assert 2 in frame2.friendly_robots + dist = math.hypot(frame2.ball.p.x - frame2.friendly_robots[2].p.x, frame2.ball.p.y - frame2.friendly_robots[2].p.y) + assert dist >= ROBOT_RADIUS + assert dist > 0.11