Johnny Fernandes
127 lines
4.7 KiB
Python
127 lines
4.7 KiB
Python
"""Cluster a 2D LiDAR scan into world-frame sheep position estimates.
|
|
|
|
Pipeline:
|
|
|
|
ranges (N,) → hit mask → world-frame points
|
|
│
|
|
▼
|
|
adjacency clustering (gap > GAP_THRESHOLD
|
|
starts a new cluster, walking rays in
|
|
angular order)
|
|
│
|
|
▼
|
|
centroid + span + region + structure filters
|
|
│
|
|
▼
|
|
list of (x, y) detections
|
|
|
|
The downstream tracker handles association across frames.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import math
|
|
|
|
import numpy as np
|
|
|
|
from herding.world.geometry import FIELD_X, FIELD_Y, GATE_Y, PEN_X, PEN_Y
|
|
from herding.perception.lidar_sim import (
|
|
LIDAR_FOV, LIDAR_MAX_RANGE, LIDAR_N_RAYS, SHEEP_RADIUS, ray_angles,
|
|
)
|
|
|
|
|
|
GAP_THRESHOLD = 0.6 # m — adjacent ray-points farther apart start a new cluster
|
|
MAX_CLUSTER_SPAN = 1.5 # m — wider clusters are walls / structures
|
|
RANGE_HIT_EPS = 0.05 # m — hit if range < max_range - eps
|
|
WALL_REJECT = 0.5 # m — drop detections this close to a known wall line
|
|
|
|
# Sheep-sized static features (gate posts, corner pillars). A cluster
|
|
# centred within STATIC_REJECT of any of these is never a sheep.
|
|
_STATIC_FEATURES = (
|
|
( 10.0, -15.0), ( 13.0, -15.0), # gate posts
|
|
( 15.0, 15.0), ( 15.0, -15.0),
|
|
(-15.0, 15.0), (-15.0, -15.0), # field corners
|
|
)
|
|
STATIC_REJECT = 0.8
|
|
|
|
|
|
def detections_from_scan(
|
|
ranges: np.ndarray,
|
|
dog_x: float, dog_y: float, dog_heading: float,
|
|
max_range: float = LIDAR_MAX_RANGE,
|
|
) -> list[tuple[float, float]]:
|
|
"""Return list of (x, y) world-frame sheep position estimates."""
|
|
ranges = np.asarray(ranges, dtype=np.float32)
|
|
n_rays = ranges.shape[0]
|
|
if n_rays == 0:
|
|
return []
|
|
angles = ray_angles(n_rays, LIDAR_FOV)
|
|
hit = ranges < max_range - RANGE_HIT_EPS
|
|
|
|
world_a = dog_heading + angles
|
|
px = dog_x + ranges * np.cos(world_a)
|
|
py = dog_y + ranges * np.sin(world_a)
|
|
|
|
# Walk rays in angular order; a large jump between consecutive
|
|
# world-frame hit points closes the current cluster.
|
|
clusters: list[list[tuple[float, float]]] = []
|
|
current: list[tuple[float, float]] = []
|
|
prev: tuple[float, float] | None = None
|
|
for i in range(n_rays):
|
|
if not bool(hit[i]):
|
|
if current:
|
|
clusters.append(current)
|
|
current = []
|
|
prev = None
|
|
continue
|
|
pt = (float(px[i]), float(py[i]))
|
|
if prev is not None and math.hypot(pt[0] - prev[0], pt[1] - prev[1]) > GAP_THRESHOLD:
|
|
clusters.append(current)
|
|
current = []
|
|
current.append(pt)
|
|
prev = pt
|
|
if current:
|
|
clusters.append(current)
|
|
|
|
detections: list[tuple[float, float]] = []
|
|
for cluster in clusters:
|
|
xs = [p[0] for p in cluster]
|
|
ys = [p[1] for p in cluster]
|
|
cx, cy = sum(xs) / len(xs), sum(ys) / len(ys)
|
|
span = math.hypot(max(xs) - min(xs), max(ys) - min(ys))
|
|
if span > MAX_CLUSTER_SPAN:
|
|
continue
|
|
# Rays hit the front edge of the sheep; offset outward by
|
|
# SHEEP_RADIUS along the dog→cluster direction to estimate the
|
|
# centre.
|
|
dx, dy = cx - dog_x, cy - dog_y
|
|
d = math.hypot(dx, dy)
|
|
if d > 1e-3:
|
|
cx += SHEEP_RADIUS * dx / d
|
|
cy += SHEEP_RADIUS * dy / d
|
|
# Region filter: in-field clusters, plus a narrow strip south of
|
|
# the gate so sheep mid-crossing get latched penned. Detections
|
|
# deeper into the pen are dropped — pen posts and rails would
|
|
# otherwise generate phantom penned tracks.
|
|
in_main = (FIELD_X[0] - 0.2 < cx < FIELD_X[1] + 0.2 and
|
|
FIELD_Y[0] - 0.2 < cy < FIELD_Y[1] + 0.2)
|
|
in_gate_strip = (PEN_X[0] - 0.2 < cx < PEN_X[1] + 0.2 and
|
|
GATE_Y - 1.0 < cy < GATE_Y + 0.2)
|
|
if not (in_main or in_gate_strip):
|
|
continue
|
|
# Known sheep-sized static features.
|
|
if any(math.hypot(cx - fx, cy - fy) < STATIC_REJECT
|
|
for fx, fy in _STATIC_FEATURES):
|
|
continue
|
|
# Wall-proximity filter — sheep can't get this close to a wall,
|
|
# so detections right at the wall line are structure noise.
|
|
near_field_wall = (
|
|
cx > FIELD_X[1] - WALL_REJECT or cx < FIELD_X[0] + WALL_REJECT or
|
|
cy > FIELD_Y[1] - WALL_REJECT or
|
|
(cy < FIELD_Y[0] + WALL_REJECT and not (PEN_X[0] <= cx <= PEN_X[1]))
|
|
)
|
|
if near_field_wall:
|
|
continue
|
|
detections.append((cx, cy))
|
|
return detections
|