You cannot select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
Chameleon/integration/ptz17_yolo_steering/server.patch

363 lines
17 KiB
Diff

This file contains ambiguous Unicode characters!

This file contains ambiguous Unicode characters that may be confused with others in your current locale. If your use case is intentional and legitimate, you can safely ignore this warning. Use the Escape button to highlight these characters.

--- a/postprocess.py
+++ b/postprocess.py
@@ -3,6 +3,7 @@
import math
import logging
import json
+from dataclasses import dataclass
from typing import Any, List, Tuple
import numpy as np
@@ -16,6 +17,7 @@
from . import motion_detector
from .utils import (
clipf,
+ quantf,
deg_to_compass,
hfov_ptz_from_zoom_ratio,
vfov_from_hfov,
@@ -6613,8 +6615,149 @@
return_to_preset(cam_idx, reason=reason)
return True
-def _consume_paired_detector_evidence(cam_idx, st, sample, detector_targets,
- validation_targets, width, height):
+_PTZ17_CAM_IDX = 6
+_PTZ17_STEERING_MIN_CONF = 0.45
+
+
+@dataclass(frozen=True, slots=True)
+class _SteeringEvidence:
+ owner: tuple[int, int, float, int]
+ source_seq: int
+ captured_at: float
+ target_class: int
+
+
+@dataclass(frozen=True, slots=True)
+class _SteeringTimeout:
+ track_owner: tuple[int, int, float]
+ last_normal_validation_at: float
+
+
+def _ptz17_remember_steering_evidence(
+ st: dict,
+ sample: visual_control.VisualSample,
+ candidates: list[tuple[list[float], float, int]],
+ width: int,
+ height: int,
+ wall_now: float,
+) -> None:
+ if not visual_control.current(_PTZ17_CAM_IDX, sample, wall_now, max_age=.65):
+ return
+ bbox = sample.result.bbox
+ target_class = int(sample.result.target_class)
+ if not sample.result.ok or bbox is None or target_class < 0:
+ return
+ min_overlap = float(getattr(config, "VISUAL_TRACKER_MATCH_IOU", .08))
+ overlapping = [
+ (box, confidence, cls)
+ for box, confidence, cls in candidates
+ if cls == target_class and 0.0 <= confidence <= 1.0
+ and all(math.isfinite(value) for value in box)
+ and 0 <= box[0] < box[2] <= width and 0 <= box[1] < box[3] <= height
+ and bbox_iou(bbox, box) >= min_overlap
+ ]
+ matched = _find_visual_validation_target(bbox, overlapping, width, height,
+ target_class=target_class)
+ if matched is not None:
+ st["ptz17_steering_evidence"] = _SteeringEvidence(
+ sample.owner, sample.source_seq, sample.captured_at, target_class,
+ )
+
+
+def _ptz17_validation_timeout_steering(
+ cam_idx: int,
+ st: dict,
+ sample: visual_control.VisualSample | None,
+ width: int,
+ height: int,
+ wall_now: float,
+) -> tuple[float, float] | None:
+ """Return bounded logical pan/tilt from a fresh box, right/down positive.
+
+ Box coordinates are in pixels from the upper left. Position feedback is
+ scaled by tan(FOV/2) / tan(wide FOV/2) before the normal transport clamps.
+ """
+ if cam_idx != _PTZ17_CAM_IDX or sample is None:
+ return None
+ evidence = st.get("ptz17_steering_evidence")
+ if (
+ not isinstance(evidence, _SteeringEvidence)
+ or not visual_control.current(cam_idx, sample, wall_now, max_age=.25)
+ or not sample.result.ok or sample.result.bbox is None
+ or sample.owner != evidence.owner or sample.source_seq <= evidence.source_seq
+ or int(sample.result.target_class) != evidence.target_class
+ or st.get("visual_tracker_mismatch_frames")
+ or st.get("aim_final_pan_recovering") or st.get("aim_final_tilt_recovering")
+ or float(st.get("aim_final_pan_predict_cooldown_until", 0.0) or 0.0) > wall_now
+ or float(st.get("aim_final_tilt_predict_cooldown_until", 0.0) or 0.0) > wall_now
+ or st.get("zoom_edge_recovery_active")
+ or st.get("zoom_step_pending") or st.get("lens_move_put_inflight")
+ or float(st.get("zoom_step_until", 0.0) or 0.0) > wall_now
+ or float(st.get("zoom_prev_cmd", 0.0) or 0.0) > 0.0
+ or (st.get("last_ptz_physical_cmd") or (0.0, 0.0, 0.0))[2] != 0.0
+ ):
+ return None
+ max_age_s = float(getattr(config, "VISUAL_TRACKER_CONTROL_MAX_UNVALIDATED_SEC", 2.0))
+ if not 0.0 <= sample.captured_at - evidence.captured_at <= wall_now - evidence.captured_at <= max_age_s:
+ return None
+ x1_px, y1_px, x2_px, y2_px = map(float, sample.result.bbox)
+ margin_x = float(getattr(config, "EDGE_LOSS_MARGIN", .015))
+ margin_y = float(getattr(config, "VERT_LOSS_MARGIN", .05))
+ if not (
+ width * margin_x <= x1_px < x2_px <= width * (1.0 - margin_x)
+ and height * margin_y <= y1_px < y2_px <= height * (1.0 - margin_y)
+ ):
+ return None
+ hfov_deg = float(st.get("hfov_deg", 0.0) or 0.0)
+ optics_at = float(st.get("zoom_status_at", 0.0) or 0.0)
+ optics_max_age_s = float(getattr(config, "AIM_FINAL_PREDICT_STATUS_MAX_AGE_SEC", .65))
+ wide_hfov_deg = float(config.CAMERA_CONFIG[cam_idx].get(
+ "ptz_hfov_wide_deg", getattr(config, "PTZ_HFOV_WIDE_DEG", 59.0),
+ ))
+ if not (
+ 0.0 < hfov_deg < 180.0 and 0.0 < wide_hfov_deg < 180.0
+ and optics_at > 0.0 and 0.0 <= wall_now - optics_at <= optics_max_age_s
+ ):
+ return None
+ dx_norm = (x1_px + x2_px) / (2.0 * width) - .5
+ dy_norm = (y1_px + y2_px) / (2.0 * height) - .5
+ gain, _, _, _, dead_zone = _aim_schedule(
+ max(abs(dx_norm), abs(dy_norm)), 0.0, (x2_px - x1_px) / width,
+ )
+ fov_scale = min(1.0, math.tan(math.radians(hfov_deg / 2.0))
+ / math.tan(math.radians(wide_hfov_deg / 2.0)))
+ dead_zone = max(dead_zone, float(getattr(config, "AIM_HOLD_ENTER_ERR", .020)))
+ cap = float(getattr(config, "TRACK_LOSS_LAST_COMMAND_MAX", .18))
+ quantum = float(getattr(config, "PTZ_QUANT", .01))
+ transport_min = max(.01, float(getattr(config, "PTZ_EPS", .01)))
+ if (
+ not all(math.isfinite(value) and value >= 0.0
+ for value in (gain, dead_zone, cap, quantum, transport_min))
+ or gain <= 0.0
+ ):
+ return None
+ # ISAPI encodes each axis as integer hundredths. Apply one encodable step
+ # outside the centre deadband so small telephoto corrections reach the queue.
+ minimum = math.ceil(transport_min / quantum) * quantum if quantum else transport_min
+ if quantf(minimum, quantum) < transport_min:
+ minimum += quantum
+ if minimum > cap:
+ return None
+
+ def axis(error_norm: float) -> float:
+ if abs(error_norm) <= dead_zone:
+ return 0.0
+ return math.copysign(clipf(abs(error_norm) * gain * fov_scale, minimum, cap), error_norm)
+
+ return axis(dx_norm), axis(dy_norm)
+
+
+def _consume_paired_detector_evidence(
+ cam_idx: int, st: dict, sample: visual_control.VisualSample,
+ detector_targets: list[tuple[list[int], float, int]],
+ validation_targets: list[tuple[list[int], float, int]], width: int, height: int,
+ steering_targets: list[tuple[list[float], float, int]] | None = None,
+) -> None:
"""Validate the same image without rewinding live control or loss recovery."""
result = sample.result
bbox = result.bbox
@@ -6629,6 +6772,8 @@
conflict = matched is None and _has_nearby_detector_identity_conflict(
bbox, candidates, width, height, target_class=target_class)
if conflict:
+ if cam_idx == _PTZ17_CAM_IDX:
+ st.pop("ptz17_steering_evidence", None)
votes = int(st.get('visual_tracker_mismatch_frames', 0) or 0) + 1
_arm_zoom_revalidation(st, time.time(), 'identity_conflict', cam_idx=cam_idx)
st['visual_tracker_mismatch_frames'] = votes
@@ -6641,8 +6786,13 @@
return
st['visual_tracker_mismatch_frames'] = 0
if matched is not None and now >= float(st.get('visual_tracker_last_detector_at', 0.) or 0.):
+ if cam_idx == _PTZ17_CAM_IDX:
+ st.pop("ptz17_steering_evidence", None)
+ st.pop("ptz17_steering_timeout", None)
visual_tracker_manager.record_detector_validation(cam_idx, matched[0])
_record_visual_detector_validation(st, now, float(matched[1]), source_seq=sample.source_seq)
+ elif matched is None and cam_idx == _PTZ17_CAM_IDX and steering_targets:
+ _ptz17_remember_steering_evidence(st, sample, steering_targets, width, height, time.time())
alarm_eligible, alarm_conf = _alarm_eligible_for_target(
bbox, target_class, detector_targets, width, height)
identity_conf = float(matched[1]) if matched is not None and int(matched[2]) == target_class else 0.
@@ -6946,6 +7096,7 @@
max_model_conf = 0.0
target_max_conf = 0.0
cls_hist_diag = ""
+ steering_targets: list[tuple[list[float], float, int]] = []
if boxes_t is not None and len(boxes_t) > 0:
raw_box_count = int(len(boxes_t))
@@ -6999,6 +7150,10 @@
continue
conf_thr = torch.where(clss == cls_id, torch.as_tensor(thr, device=confs.device), conf_thr)
+ steering_mask = (
+ mask & torch.isfinite(confs) & (confs >= _PTZ17_STEERING_MIN_CONF) & (confs < conf_thr)
+ if cam_idx == _PTZ17_CAM_IDX and evidence_only else None
+ )
# === Motion-aided detection: boost conf для боксов внутри motion ROI ===
# Только для PTZ-камер с включённым motion detector и статичной позицией.
# Это позволяет пропустить мелкие/неуверенные NN-детекции, которые в обычном
@@ -7045,6 +7200,8 @@
for drone_id in drone_ids:
drone_mask = drone_mask | (clss == int(drone_id))
mask = mask & ((~drone_mask) | size_ok)
+ if steering_mask is not None:
+ steering_mask &= (~drone_mask) | size_ok
if getattr(config, "ANTI_CLOUD_FILTER", True):
xyxy = boxes_t.xyxy
@@ -7058,6 +7215,14 @@
| (h_fr > float(getattr(config, "ANTI_CLOUD_MAX_H_FRAC", 0.24)))
)
mask = mask & (~(low_conf & cloud_like))
+ if steering_mask is not None:
+ steering_mask &= ~(low_conf & cloud_like)
+ if steering_mask is not None and steering_mask.any():
+ steering_targets = list(zip(
+ boxes_t.xyxy[steering_mask].detach().cpu().tolist(),
+ confs[steering_mask].detach().cpu().tolist(),
+ clss[steering_mask].detach().cpu().to(dtype=torch.int64).tolist(),
+ ))
try:
pass_box_count = int(mask.sum().detach().cpu().item())
except Exception:
@@ -7095,7 +7260,7 @@
if evidence_only:
_consume_paired_detector_evidence(cam_idx, st, visual_sample, detector_targets,
- validation_targets, w, h)
+ validation_targets, w, h, steering_targets)
return
if bt_active:
@@ -7377,6 +7542,7 @@
# measurement is not sufficient for initial acquisition.
best_target = None
+ ptz17_validation_timeout = False
# PTZ target ownership after acquisition:
# YOLO/ByteTrack -> confirm/reacquire; OSTrack -> frame-to-frame control.
# This branch intentionally runs after weak-candidate confirmation so a
@@ -7730,6 +7896,8 @@
)
)
if nearby_identity_conflict:
+ if cam_idx == _PTZ17_CAM_IDX:
+ st.pop("ptz17_steering_evidence", None)
mismatch_frames = int(st.get("visual_tracker_mismatch_frames", 0) or 0) + 1
st["visual_tracker_mismatch_frames"] = mismatch_frames
st["visual_tracker_last_zoom_detector_at"] = 0.0
@@ -7867,7 +8035,13 @@
max_unvalidated > strong_unvalidated
and validation_age > strong_unvalidated
)
- if validation_age <= max_unvalidated:
+ steering_timeout = st.get("ptz17_steering_timeout") if cam_idx == _PTZ17_CAM_IDX else None
+ steering_only_after_timeout = (
+ isinstance(steering_timeout, _SteeringTimeout)
+ and steering_timeout.track_owner == visual_control.owner(st)[:3]
+ and last_validation <= steering_timeout.last_normal_validation_at
+ )
+ if validation_age <= max_unvalidated and not steering_only_after_timeout:
visual_conf = max(
float(visual_result.score),
float(matched_validation[1]) if matched_validation is not None else 0.0,
@@ -7889,19 +8063,36 @@
visual_track_label_index = len(det_labels)
det_labels.append("target track")
else:
- logger.info(
- "[OSTRACK/VALIDATION] cam=%s timeout age=%.2fs "
- "limit=%.2fs score=%.2f zoomed_grace=%s",
- cam_idx,
- validation_age,
- max_unvalidated,
- float(visual_result.score),
- bool(st.get("visual_tracker_validation_grace_active", False)),
+ ptz17_validation_timeout = bool(
+ visual_only and _ptz17_validation_timeout_steering(
+ cam_idx, st, visual_sample, w, h, time.time(),
+ ) is not None
)
- visual_tracker_manager.reset(cam_idx, reason="detector validation timeout")
+ if ptz17_validation_timeout:
+ # A recovered OSTrack score alone must not undo the
+ # normal detector timeout for the lens or alarm path.
+ if not steering_only_after_timeout:
+ st["ptz17_steering_timeout"] = _SteeringTimeout(
+ visual_control.owner(st)[:3], last_validation,
+ )
+ st["visual_tracker_last_reason"] = "detector timeout: PTZ17 steering only"
+ else:
+ logger.info(
+ "[OSTRACK/VALIDATION] cam=%s timeout age=%.2fs "
+ "limit=%.2fs score=%.2f zoomed_grace=%s",
+ cam_idx,
+ validation_age,
+ max_unvalidated,
+ float(visual_result.score),
+ bool(st.get("visual_tracker_validation_grace_active", False)),
+ )
+ visual_tracker_manager.reset(cam_idx, reason="detector validation timeout")
+ if cam_idx == _PTZ17_CAM_IDX:
+ st.pop("ptz17_steering_timeout", None)
+ st.pop("ptz17_steering_evidence", None)
+ st["visual_tracker_last_reason"] = "detector validation timeout"
best_target = None
target_source = "none"
- st["visual_tracker_last_reason"] = "detector validation timeout"
st["visual_tracker_validation_limit_sec"] = 0.0
st["visual_tracker_validation_grace_active"] = False
else:
@@ -10308,6 +10499,20 @@
decay,
)
+ if ptz17_validation_timeout:
+ steering = (
+ _ptz17_validation_timeout_steering(cam_idx, st, visual_sample, w, h, time.time())
+ if zoom_cmd_track == 0.0 else None
+ )
+ if steering is not None:
+ base_pan, base_tilt = steering
+ else:
+ logger.info("[PTZ17/STEERING] cam=%s end reason=%s", cam_idx,
+ "loss_zoom" if zoom_cmd_track != 0.0 else "steering_veto")
+ visual_tracker_manager.reset(cam_idx, reason="detector validation timeout")
+ st.pop("ptz17_steering_timeout", None)
+ st.pop("ptz17_steering_evidence", None)
+
pan_cmd = _PAN_SIGN * base_pan
tilt_cmd = _TILT_SIGN * base_tilt
st["track_loss_active"] = True
@@ -10327,6 +10532,16 @@
dmax_s = abs(ang_diff_signed(float(smax), float(az)))
if min(dmin_s, dmax_s) > soft_leash:
pan_cmd = 0.0 # не толкаем дальше от сектора
+
+ if ptz17_validation_timeout and steering is not None:
+ last_log = float(st.get("ptz17_steering_log_at", 0.0) or 0.0)
+ if not steering_only_after_timeout or now - last_log >= .25:
+ st["ptz17_steering_log_at"] = now
+ logger.info(
+ "[PTZ17/STEERING] cam=%s %s frame=%s pan=%.4f tilt=%.4f zoom=%.2f",
+ cam_idx, "continue" if steering_only_after_timeout else "start",
+ source_seq, pan_cmd, tilt_cmd, zoom_cmd_track,
+ )
st["track_loss_pan_cmd"] = float(pan_cmd)
st["track_loss_tilt_cmd"] = float(tilt_cmd)