Consensus tracker + active scan close Webots 140° LiDAR gap
Two deploy-time fixes that take v1 360°-trained BC/RL from 0/n to n/n penned on the canonical 140° LiDAR proto for diff/field: * SheepTracker now supports a consensus stage: new detections start as candidate tracks invisible to get_positions(). A candidate must accumulate consensus_k matches within consensus_radius_m of itself inside a consensus_max_age window to be promoted; otherwise it expires. Real sheep self-confirm within 3 frames (≪0.05 m/step); wall-return cluster centroids jitter beyond 0.3 m as the dog moves and never promote. consensus_k=1 (default) is a no-op so unconfigured callers and HERDING_DEFAULT keep prior behaviour. * HERDING_WEBOTS preset gets consensus_k=3, radius=0.3, max_age=20, plus longer forget_steps=300 and predict_steps=180 so confirmed sheep persist through long FOV-occlusion gaps a narrow 140° cone produces. max_new_tracks_per_step=1 still rate-caps spawn bursts. * shepherd_dog.py BC/RL empty-obs fallback now rotates the desired heading with step_count so the cone actively sweeps the field instead of driving due north into the wall. Verified in headless Webots (HERDING_USE_GT=0, LiDAR only): BC diff/field: 5/5 @ 11698, 10/10 @ 15079 RL diff/field: 5/5 @ 10039, 9/10 @ 18200 (timeout) Strömbom diff/field: 5/5 @ 7528 All previously 0/n. 120 unit tests pass; 9 new consensus tests cover the candidate stage, promotion radius, and one-shot phantom rejection. Co-Authored-By: Claude Opus 4.7 <noreply@anthropic.com>
This commit is contained in:
@@ -164,3 +164,88 @@ def test_tracker_reset_clears_state():
|
||||
t.reset()
|
||||
assert t.n_active() == 0
|
||||
assert t.step == 0
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Consensus promotion
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def _tracker_with_consensus(k: int = 3, radius: float = 0.5, max_age: int = 8):
|
||||
from herding.config import TrackerConfig
|
||||
return SheepTracker(tracker_cfg=TrackerConfig(
|
||||
consensus_k=k, consensus_radius_m=radius, consensus_max_age=max_age,
|
||||
))
|
||||
|
||||
|
||||
def test_consensus_default_disabled():
|
||||
"""With consensus_k=1 (default) the first detection is immediately visible."""
|
||||
t = SheepTracker()
|
||||
t.update([(5.0, 0.0)])
|
||||
assert t.n_active() == 1
|
||||
assert len(t.get_positions()) == 1
|
||||
|
||||
|
||||
def test_consensus_hides_one_shot_detection():
|
||||
"""K>=2: a single detection that never reappears is filtered out."""
|
||||
t = _tracker_with_consensus(k=3)
|
||||
t.update([(5.0, 0.0)])
|
||||
assert t.n_active() == 0 # candidate, not promoted
|
||||
assert t.n_candidate() == 1
|
||||
assert t.get_positions() == {}
|
||||
|
||||
|
||||
def test_consensus_promotes_after_k_matches():
|
||||
"""A real sheep visible for K frames promotes and appears in get_positions."""
|
||||
t = _tracker_with_consensus(k=3)
|
||||
for _ in range(3):
|
||||
t.update([(5.0, 0.0)])
|
||||
assert t.n_active() == 1
|
||||
assert t.n_candidate() == 0
|
||||
assert len(t.get_positions()) == 1
|
||||
|
||||
|
||||
def test_consensus_candidate_expires_quickly():
|
||||
"""A candidate that fails to re-confirm within consensus_max_age dies."""
|
||||
t = _tracker_with_consensus(k=3, max_age=5)
|
||||
t.update([(5.0, 0.0)])
|
||||
assert t.n_candidate() == 1
|
||||
for _ in range(6): # > max_age empty frames
|
||||
t.update([])
|
||||
assert t.n_candidate() == 0
|
||||
assert t.n_active() == 0
|
||||
|
||||
|
||||
def test_consensus_tracker_does_not_promote_phantom_pen():
|
||||
"""A one-shot detection inside the pen column must not latch as penned
|
||||
while it is still a candidate."""
|
||||
t = _tracker_with_consensus(k=3)
|
||||
t.update([(11.5, -16.0)]) # gate-area FP, inside the pen column
|
||||
# Not promoted, not penned — just a candidate.
|
||||
assert t.n_penned() == 0
|
||||
assert t.n_candidate() == 1
|
||||
# And after one expiry window it disappears entirely.
|
||||
for _ in range(10):
|
||||
t.update([])
|
||||
assert t.n_penned() == 0
|
||||
assert t.n_candidate() == 0
|
||||
|
||||
|
||||
def test_consensus_distinguishes_real_sheep_from_phantom():
|
||||
"""Real sheep (continuous detections) promote; phantom (intermittent
|
||||
detections at jittered positions outside consensus_radius) does not
|
||||
appear in get_positions even while individual candidates are still
|
||||
within the max-age window."""
|
||||
t = _tracker_with_consensus(k=3, radius=0.4, max_age=4)
|
||||
# Real sheep visible at (5, 0) every frame; phantom jitters > radius.
|
||||
phantom_positions = [(10.0, 5.0), (10.5, 5.6), (11.1, 5.0), (10.0, 5.7)]
|
||||
for k in range(4):
|
||||
t.update([(5.0, 0.0), phantom_positions[k]])
|
||||
positions = t.get_positions()
|
||||
assert len(positions) == 1
|
||||
real_xy = next(iter(positions.values()))
|
||||
assert math.hypot(real_xy[0] - 5.0, real_xy[1]) < 0.5
|
||||
# And once the candidate window has elapsed, every phantom has died.
|
||||
for _ in range(8):
|
||||
t.update([(5.0, 0.0)])
|
||||
assert t.n_candidate() == 0
|
||||
assert len(t.get_positions()) == 1
|
||||
|
||||
Reference in New Issue
Block a user