359 lines
13 KiB
Python
359 lines
13 KiB
Python
"""Multi-feature person-to-door assignment and active-CAN risk gating."""
|
|
|
|
from __future__ import annotations
|
|
|
|
import json
|
|
import math
|
|
from collections import Counter, defaultdict, deque
|
|
from dataclasses import dataclass
|
|
from pathlib import Path
|
|
from statistics import median
|
|
|
|
|
|
DOORS = ("front", "rear")
|
|
POSE_FEATURES = {
|
|
"shoulder_center_x",
|
|
"shoulder_center_y",
|
|
"shoulder_width",
|
|
"torso_height",
|
|
"hip_center_y",
|
|
}
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class FeatureCalibration:
|
|
rear: float
|
|
front: float
|
|
weight: float
|
|
|
|
def frontness(self, value: float) -> float:
|
|
span = self.front - self.rear
|
|
if span == 0:
|
|
return 0.5
|
|
return min(1.0, max(0.0, (value - self.rear) / span))
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class DoorZoneConfig:
|
|
features: dict[str, FeatureCalibration]
|
|
front_probability_min: float
|
|
rear_probability_max: float
|
|
minimum_person_height: float
|
|
minimum_pose_features: int
|
|
history_frames: int
|
|
stale_track_frames: int
|
|
source_path: Path | None = None
|
|
|
|
@classmethod
|
|
def load(cls, path: Path) -> "DoorZoneConfig":
|
|
source = Path(path).expanduser().resolve()
|
|
data = json.loads(source.read_text(encoding="utf-8-sig"))
|
|
features = {
|
|
name: FeatureCalibration(
|
|
rear=float(values["rear"]),
|
|
front=float(values["front"]),
|
|
weight=float(values["weight"]),
|
|
)
|
|
for name, values in data["features"].items()
|
|
}
|
|
return cls(
|
|
features=features,
|
|
front_probability_min=float(data.get("front_probability_min", 0.72)),
|
|
rear_probability_max=float(data.get("rear_probability_max", 0.28)),
|
|
minimum_person_height=float(data.get("minimum_person_height", 0.40)),
|
|
minimum_pose_features=int(data.get("minimum_pose_features", 4)),
|
|
history_frames=int(data.get("history_frames", 5)),
|
|
stale_track_frames=int(data.get("stale_track_frames", 120)),
|
|
source_path=source,
|
|
)
|
|
|
|
def classify(self, probability: float) -> str:
|
|
if probability >= self.front_probability_min:
|
|
return "front"
|
|
if probability <= self.rear_probability_max:
|
|
return "rear"
|
|
return "transition"
|
|
|
|
|
|
def _point(keypoints: list, index: int) -> tuple[float, float] | None:
|
|
if index >= len(keypoints) or len(keypoints[index]) < 2:
|
|
return None
|
|
x, y = float(keypoints[index][0]), float(keypoints[index][1])
|
|
return (x, y) if math.isfinite(x) and math.isfinite(y) else None
|
|
|
|
|
|
def extract_person_features(
|
|
record: dict,
|
|
position: int,
|
|
frame_shape: tuple[int, int],
|
|
) -> tuple[dict[str, float], int]:
|
|
"""Extract fixed-camera pose features, with box geometry as low-weight fallback."""
|
|
|
|
height, width = frame_shape
|
|
box = record.get("boxes", [])[position]
|
|
x1, y1, x2, y2 = [float(value) for value in box[:4]]
|
|
features = {
|
|
"box_center_x": ((x1 + x2) * 0.5) / max(width, 1),
|
|
"box_height": (y2 - y1) / max(height, 1),
|
|
}
|
|
|
|
all_keypoints = record.get("temporal_selected_keypoints", [])
|
|
if position >= len(all_keypoints):
|
|
return features, 0
|
|
keypoints = all_keypoints[position]
|
|
left_shoulder = _point(keypoints, 5)
|
|
right_shoulder = _point(keypoints, 6)
|
|
left_hip = _point(keypoints, 11)
|
|
right_hip = _point(keypoints, 12)
|
|
if None in (left_shoulder, right_shoulder, left_hip, right_hip):
|
|
return features, 0
|
|
|
|
shoulder_x = (left_shoulder[0] + right_shoulder[0]) * 0.5
|
|
shoulder_y = (left_shoulder[1] + right_shoulder[1]) * 0.5
|
|
hip_y = (left_hip[1] + right_hip[1]) * 0.5
|
|
features.update(
|
|
{
|
|
"shoulder_center_x": shoulder_x / max(width, 1),
|
|
"shoulder_center_y": shoulder_y / max(height, 1),
|
|
"shoulder_width": math.hypot(
|
|
left_shoulder[0] - right_shoulder[0],
|
|
left_shoulder[1] - right_shoulder[1],
|
|
)
|
|
/ max(width, 1),
|
|
"torso_height": abs(hip_y - shoulder_y) / max(height, 1),
|
|
"hip_center_y": hip_y / max(height, 1),
|
|
}
|
|
)
|
|
return features, len(POSE_FEATURES)
|
|
|
|
|
|
class DoorZoneAssigner:
|
|
"""Fuse pose-dominant evidence and smooth front probability per track."""
|
|
|
|
def __init__(self, config: DoorZoneConfig):
|
|
self.config = config
|
|
self.history: dict[int, deque[float]] = defaultdict(
|
|
lambda: deque(maxlen=self.config.history_frames)
|
|
)
|
|
self.last_seen: dict[int, int] = {}
|
|
self.zone_counts: Counter = Counter()
|
|
|
|
def _score(self, features: dict[str, float]) -> tuple[float, dict[str, float]]:
|
|
weighted_sum = 0.0
|
|
total_weight = 0.0
|
|
contributions = {}
|
|
for name, calibration in self.config.features.items():
|
|
if name not in features:
|
|
continue
|
|
frontness = calibration.frontness(float(features[name]))
|
|
contributions[name] = round(frontness, 6)
|
|
weighted_sum += frontness * calibration.weight
|
|
total_weight += calibration.weight
|
|
probability = weighted_sum / total_weight if total_weight else 0.5
|
|
return probability, contributions
|
|
|
|
def update(
|
|
self,
|
|
frame_index: int,
|
|
record: dict,
|
|
frame_shape: tuple[int, int],
|
|
) -> list[dict]:
|
|
track_ids = [int(value) for value in record.get("track_ids", [])]
|
|
assignments = []
|
|
for position, _box in enumerate(record.get("boxes", [])):
|
|
track_id = track_ids[position] if position < len(track_ids) else position + 1
|
|
features, pose_feature_count = extract_person_features(
|
|
record, position, frame_shape
|
|
)
|
|
probability, contributions = self._score(features)
|
|
enough_pose = pose_feature_count >= self.config.minimum_pose_features
|
|
if features["box_height"] < self.config.minimum_person_height:
|
|
raw_zone = "unassigned"
|
|
door_zone = "unassigned"
|
|
elif not enough_pose:
|
|
raw_zone = "transition"
|
|
door_zone = "transition"
|
|
else:
|
|
raw_zone = self.config.classify(probability)
|
|
values = self.history[track_id]
|
|
values.append(probability)
|
|
door_zone = self.config.classify(float(median(values)))
|
|
|
|
values = self.history.get(track_id, ())
|
|
smoothed_probability = (
|
|
float(median(values)) if values else float(probability)
|
|
)
|
|
self.last_seen[track_id] = int(frame_index)
|
|
self.zone_counts[door_zone] += 1
|
|
assignments.append(
|
|
{
|
|
"track_id": track_id,
|
|
"person_index": position,
|
|
"door_zone": door_zone,
|
|
"raw_door_zone": raw_zone,
|
|
"front_probability": round(float(probability), 6),
|
|
"smoothed_front_probability": round(smoothed_probability, 6),
|
|
"confidence": round(abs(smoothed_probability - 0.5) * 2.0, 6),
|
|
"evidence_source": "pose_primary_bbox_secondary",
|
|
"pose_feature_count": pose_feature_count,
|
|
"features": {name: round(value, 6) for name, value in features.items()},
|
|
"feature_frontness": contributions,
|
|
"history_frames": len(values),
|
|
}
|
|
)
|
|
|
|
expired = [
|
|
track_id
|
|
for track_id, seen_at in self.last_seen.items()
|
|
if frame_index - seen_at > self.config.stale_track_frames
|
|
]
|
|
for track_id in expired:
|
|
self.history.pop(track_id, None)
|
|
self.last_seen.pop(track_id, None)
|
|
return assignments
|
|
|
|
def summary(self) -> dict:
|
|
return {
|
|
"policy": "pose-primary weighted front probability with causal median",
|
|
"config": str(self.config.source_path) if self.config.source_path else None,
|
|
"history_frames": self.config.history_frames,
|
|
"front_probability_min": self.config.front_probability_min,
|
|
"rear_probability_max": self.config.rear_probability_max,
|
|
"minimum_person_height": self.config.minimum_person_height,
|
|
"minimum_pose_features": self.config.minimum_pose_features,
|
|
"features": {
|
|
name: {
|
|
"rear": calibration.rear,
|
|
"front": calibration.front,
|
|
"weight": calibration.weight,
|
|
}
|
|
for name, calibration in self.config.features.items()
|
|
},
|
|
"zone_counts": dict(self.zone_counts),
|
|
}
|
|
|
|
|
|
def _inactive_risk(door: str) -> dict:
|
|
return {
|
|
"door_id": door,
|
|
"status": "inactive",
|
|
"risk_score": None,
|
|
"risk_level": "inactive",
|
|
"decision": "DOOR_CAN_NOT_ACTIVE",
|
|
"risk_type": f"{door.upper()}_INACTIVE",
|
|
"source": None,
|
|
"track_id": None,
|
|
"side": None,
|
|
"regions": [],
|
|
}
|
|
|
|
|
|
def _clear_risk(door: str) -> dict:
|
|
return {
|
|
"door_id": door,
|
|
"status": "calculated",
|
|
"risk_score": 0.0,
|
|
"risk_level": "clear",
|
|
"decision": "CLEAR_NO_PERSON_ASSIGNED_TO_ACTIVE_DOOR",
|
|
"risk_type": f"{door.upper()}_CLEAR",
|
|
"source": None,
|
|
"track_id": None,
|
|
"side": None,
|
|
"regions": [],
|
|
}
|
|
|
|
|
|
def _unknown_risk(door: str, transition_regions: list[dict]) -> dict:
|
|
return {
|
|
"door_id": door,
|
|
"status": "door_zone_ambiguous",
|
|
"risk_score": None,
|
|
"risk_level": "unknown",
|
|
"decision": "EVIDENCE_UNAVAILABLE_TRANSITION_ZONE",
|
|
"risk_type": f"{door.upper()}_UNKNOWN",
|
|
"source": None,
|
|
"track_id": None,
|
|
"side": None,
|
|
"regions": transition_regions,
|
|
}
|
|
|
|
|
|
def build_door_risks(
|
|
raw_risk: dict,
|
|
assignments: list[dict],
|
|
active_door: str,
|
|
) -> tuple[dict[str, dict], dict]:
|
|
"""Build per-door risk and select the CAN-active door result."""
|
|
|
|
zone_by_track = {
|
|
int(assignment["track_id"]): str(assignment["door_zone"])
|
|
for assignment in assignments
|
|
}
|
|
annotated_regions = []
|
|
for original in raw_risk.get("regions", []):
|
|
region = dict(original)
|
|
region["door_zone"] = zone_by_track.get(int(region.get("track_id", -1)), "unknown")
|
|
annotated_regions.append(region)
|
|
|
|
active_doors = set(DOORS) if active_door == "both" else {active_door}
|
|
if active_door == "none":
|
|
active_doors.clear()
|
|
|
|
door_risks: dict[str, dict] = {}
|
|
for door in DOORS:
|
|
if door not in active_doors:
|
|
door_risks[door] = _inactive_risk(door)
|
|
continue
|
|
exact = [region for region in annotated_regions if region["door_zone"] == door]
|
|
transition = [
|
|
region for region in annotated_regions if region["door_zone"] == "transition"
|
|
]
|
|
calculated = [region for region in exact if region.get("risk_score") is not None]
|
|
if calculated:
|
|
nearest = max(calculated, key=lambda item: float(item["risk_score"]))
|
|
level = str(nearest["risk_level"])
|
|
if transition and level in ("clear", "watch"):
|
|
door_risks[door] = _unknown_risk(door, transition)
|
|
else:
|
|
door_risks[door] = {
|
|
"door_id": door,
|
|
"status": "calculated",
|
|
"risk_score": nearest["risk_score"],
|
|
"risk_level": level,
|
|
"decision": nearest["decision"],
|
|
"risk_type": f"{door.upper()}_{level.upper()}",
|
|
"source": nearest["source"],
|
|
"track_id": nearest["track_id"],
|
|
"side": nearest["side"],
|
|
"regions": exact,
|
|
}
|
|
elif transition:
|
|
door_risks[door] = _unknown_risk(door, transition)
|
|
else:
|
|
door_risks[door] = _clear_risk(door)
|
|
|
|
if len(active_doors) == 1:
|
|
selected = dict(door_risks[next(iter(active_doors))])
|
|
elif not active_doors:
|
|
selected = {
|
|
"door_id": None,
|
|
"status": "inactive",
|
|
"risk_score": None,
|
|
"risk_level": "inactive",
|
|
"decision": "NO_DOOR_CAN_ACTIVE",
|
|
"risk_type": "NO_ACTIVE_DOOR",
|
|
"source": None,
|
|
"track_id": None,
|
|
"side": None,
|
|
"regions": [],
|
|
}
|
|
else:
|
|
severity = {"danger": 4, "warning": 3, "watch": 2, "unknown": 1, "clear": 0}
|
|
selected = dict(
|
|
max(door_risks.values(), key=lambda item: severity.get(item["risk_level"], -1))
|
|
)
|
|
selected["door_mask"] = raw_risk.get("door_mask")
|
|
selected["warning_distance_relative"] = raw_risk.get("warning_distance_relative")
|
|
selected["release_distance_relative"] = raw_risk.get("release_distance_relative")
|
|
return door_risks, selected
|