diff --git a/utama_core/data_processing/refiners/position.py b/utama_core/data_processing/refiners/position.py index 6e31ffae..1a443258 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,14 +112,24 @@ 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, @@ -235,7 +252,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 = {} @@ -336,19 +353,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 +429,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,10 +439,15 @@ 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]] = {} + # (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, @@ -448,6 +464,18 @@ 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( + [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 + ] ts = [] # maps robot id to the (detection, its camera's position) of every camera that saw it yellow_captured = defaultdict(list) @@ -464,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} @@ -475,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(): @@ -492,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 @@ -499,10 +568,33 @@ 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) + return VisionData(frame_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: @@ -539,20 +631,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..567478ec --- /dev/null +++ b/utama_core/tests/refiners/test_local_camera_consistency.py @@ -0,0 +1,348 @@ +"""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.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 +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) + + +@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} + + +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) + + +@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)] + 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) + + +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