--- a/postprocess.py +++ b/postprocess.py @@ -690,6 +690,8 @@ def _focus_quality_score( frame_bgr: np.ndarray, bbox: Tuple[int, int, int, int] | list[int], + *, + roi_scale: float | None = None, ) -> float | None: """Return Laplacian focus energy from a bounded ROI around the target. @@ -708,7 +710,7 @@ if not np.isfinite([x1, y1, x2, y2]).all() or x2 <= x1 or y2 <= y1: return None - scale = max(1.0, float(getattr(config, "AUTOFOCUS_ROI_SCALE", 2.5))) + scale = max(1.0, float(getattr(config, "AUTOFOCUS_ROI_SCALE", 2.5) if roi_scale is None else roi_scale)) cx = (x1 + x2) * 0.5 cy = (y1 + y2) * 0.5 roi_w = max(16.0, (x2 - x1) * scale) @@ -741,6 +743,89 @@ return score if math.isfinite(score) else None +@dataclass(frozen=True, slots=True) +class _PTZ17ZoomTuning: + focus_max_hfov_deg: float = 18.0 + focus_min_side_px: int = 20 + frame_max_age_s: float = 0.25 + detector_max_age_s: float = 0.65 + first_pulse_s: float = 0.34 + hard_stop_s: float = 0.40 + startup_native: int = 10 + projected_margin_min: float = 0.16 + edge_margin_min: float = 0.30 + center_max_norm: float = 0.12 + image_rate_max_norm_s: float = 0.04 + + +_PTZ17_ZOOM = _PTZ17ZoomTuning() + + +def _ptz17_focus_hold( + st: dict, *, hfov_deg: float, side_px: int, frame_is_current: bool, +) -> str | None: + """Keep the lens at its current FOV while the settled target is blurry.""" + owner = visual_control.owner(st) + if owner[2] <= 0.0 or not 0.0 < hfov_deg <= _PTZ17_ZOOM.focus_max_hfov_deg: + st.pop("ptz17_focus_hold", None) + return None + previous = st.get("ptz17_focus_hold") + reason = previous[1] if previous is not None and previous[0] == owner else None + if not frame_is_current: + return reason + focus_state = st.get("focus_state") + if focus_state == "disabled": + reason = None + elif side_px >= _PTZ17_ZOOM.focus_min_side_px: + if focus_state == "sharp": + reason = None + elif focus_state in { + "blur_suspected", "blur_fast_suspected", "blur_cooldown", "blur_wait", + "unassessable", "requested", "settling", + }: + reason = str(focus_state) + if reason is not None: + st["ptz17_focus_hold"] = (owner, reason) + else: + st.pop("ptz17_focus_hold", None) + return reason + + +def _ptz17_first_useful_timing( + st: dict, timing: tuple[float, float], *, native: int, normal_onset: bool, + frame_age_s: float, detector_age_s: float, hfov_deg: float, wide_hfov_deg: float, + approach_margin: float | None, edge_margin: float, center_error: float, image_rate: float, +) -> tuple[float, float]: + """Select the longer measured-response candidate within existing deadlines.""" + tuning = _PTZ17_ZOOM + if ( + not normal_onset or timing[0] <= 0.0 or native != tuning.startup_native + or st.get("zoom_no_effect_blocked") + or st.get("zoom_stop_obligation_active") or st.get("lens_move_stop_required") + or not 0.0 <= frame_age_s <= tuning.frame_max_age_s + or not 0.0 <= detector_age_s <= tuning.detector_max_age_s + or not 0.0 < hfov_deg <= wide_hfov_deg < 180.0 + or hfov_deg < wide_hfov_deg * max(.85, float(getattr(config, "ZOOM_RESPONSE_WIDE_RATIO", .85))) + or approach_margin is None or not tuning.projected_margin_min <= approach_margin <= .5 + or not tuning.edge_margin_min <= edge_margin <= .5 + or not 0.0 <= center_error <= tuning.center_max_norm + or not 0.0 <= image_rate <= tuning.image_rate_max_norm_s + ): + return timing + if st.get("zoom_step_pending"): + # Revalidate only the original request created by the PTZ17 profile. + # The existing pulse gate owns the pending timer and expiry. + marker = st.get("ptz17_first_timing_owner") + if marker == (visual_control.owner(st), st.get("zoom_step_pending_id")): + return (float(st["zoom_step_pending_pulse_sec"]), float(st["zoom_step_pending_hard_sec"])) + return timing + if int(st.get("zoom_step_request_id", 0) or 0) != 0: + return timing + pulse = min(tuning.first_pulse_s, float(getattr(config, "ZOOM_RESPONSE_MAX_PULSE_SEC", .34))) + hard = min(tuning.hard_stop_s, float(getattr(config, "ZOOM_RESPONSE_MAX_HARD_STOP_SEC", .40))) + return (pulse, hard) if timing[0] < pulse <= hard and timing[1] <= hard else timing + + def _clear_focus_low_evidence(st: dict) -> None: st["focus_quality_low_frames"] = 0 st["focus_quality_low_since"] = None @@ -840,6 +925,16 @@ st["focus_state"] = "unassessable" _clear_focus_low_evidence(st) return False + if cam_idx == _PTZ17_CAM_IDX and float(hfov_deg) <= _PTZ17_ZOOM.focus_max_hfov_deg: + # Reject surrounding skyline detail as proof that the actual target + # is sharp. Keep the existing contrast and blur thresholds. + target_score = _focus_quality_score(frame_bgr, bbox, roi_scale=1.0) + if target_score is None: + st["focus_state"] = "unassessable" + _clear_focus_low_evidence(st) + return False + score = min(score, target_score) + st["focus_quality_score"] = score alpha = clipf(float(getattr(config, "AUTOFOCUS_SCORE_EMA_ALPHA", 0.35)), 0.01, 1.0) previous_ema = st.get("focus_quality_ema") @@ -9537,6 +9632,47 @@ max(0.0, float(getattr(config, min_key, 0.10))), ) zoom_cmd = min(max_effective, max(float(zoom_cmd), min_effective)) + ptz17_focus_reason = None + ptz17_focus_checked = False + if (cam_idx == _PTZ17_CAM_IDX and float(hfov_deg) <= _PTZ17_ZOOM.focus_max_hfov_deg + and not pulse_active and not st.get("zoom_step_pending")): + # Check the settled image before authorizing a fresh pulse. The + # existing autofocus worker and controller keep their own limits. + focus_wall_now = time.time() + lens_settled = ( + not st.get("lens_move_put_inflight") and not st.get("zoom_stop_obligation_active") + and not st.get("lens_move_stop_required") + and (st.get("last_ptz_physical_cmd") or (0.0, 0.0, 0.0))[2] == 0.0 + ) + if (lens_settled and zoom_cmd >= 0.0 and visual_only + and visual_control.current(cam_idx, visual_sample, focus_wall_now, + max_age=_PTZ17_ZOOM.frame_max_age_s)): + _maybe_request_autofocus( + cam_idx, st, frame_bgr, (x1, y1, x2, y2), now=float(now), + pan_cmd=float(pan_speed) + float(pan_rate_ff), + tilt_cmd=float(tilt_speed) + float(tilt_rate_ff), + zoom_cmd=0.0, center_err=float(center_err), hfov_deg=float(hfov_deg), + validation_age=float(visual_control_validation_age), + ) + ptz17_focus_checked = True + # A small but measurable crop can veto another zoom step. + # The higher size threshold for requesting autofocus still applies. + if (st.get("focus_state") == "target_too_small" + and min(x2 - x1, y2 - y1) >= _PTZ17_ZOOM.focus_min_side_px + and 0.0 <= visual_control_validation_age <= _PTZ17_ZOOM.detector_max_age_s): + target_score = _focus_quality_score(frame_bgr, (x1, y1, x2, y2), roi_scale=1.0) + threshold = float(getattr(config, "AUTOFOCUS_BLUR_THRESHOLD", 35.0)) + st["focus_state"] = ( + "unassessable" if target_score is None else + "sharp" if target_score >= threshold else "blur_suspected" + ) + ptz17_focus_reason = _ptz17_focus_hold( + st, hfov_deg=float(hfov_deg), side_px=min(x2 - x1, y2 - y1), + frame_is_current=ptz17_focus_checked, + ) + if ptz17_focus_reason is not None and zoom_cmd > 0.0: + zoom_cmd = 0.0 + zoom_zero_reason = "focus_hold" if probe_start_candidate and identify_start_candidate: pulse_kind = "probe_identify" elif probe_start_candidate: @@ -9645,6 +9781,27 @@ zoom_cmd = 0.0 st["zoom_useful_pulse_reason"] = "wait_response_framing" zoom_zero_reason = "response_framing" + ptz17_timing_changed = False + if cam_idx == _PTZ17_CAM_IDX: + original_response_timing = response_timing + response_timing = _ptz17_first_useful_timing( + st, response_timing, native=timing_native, + normal_onset=(zoom_cmd > 0.0 and useful_reason == "useful" and useful_optics_fresh + and pulse_family == "normal" + and fast_approach and normal_zoom_quality_ok + and current_track_source == "visual" and not predictive_start + and bool(visual_only and visual_control.current( + cam_idx, visual_sample, zoom_gate_now, + max_age=_PTZ17_ZOOM.frame_max_age_s, + ))), + frame_age_s=zoom_gate_now - now, + detector_age_s=zoom_gate_now - last_visual_zoom_validation, + hfov_deg=float(hfov_deg), wide_hfov_deg=float(shared_wide_hfov), + approach_margin=approach_margin, edge_margin=float(edge_risk_margin), + center_error=max(abs(float(measured_dx)), abs(float(measured_dy))), + image_rate=max(float(vel), float(fast_drift)), + ) + ptz17_timing_changed = response_timing != original_response_timing gate_stop = [] if decision_request else None continuation_trace = [] if decision_request else None zoom_cmd = _gate_zoom_in_pulse( @@ -9667,6 +9824,9 @@ ) if predictive_start and zoom_cmd > 0.0 and st["zoom_step_request_id"] > request_id_before_gate: st["predictive_zoom_request"] = st["zoom_step_pending_id"] + if (ptz17_timing_changed and zoom_cmd > 0.0 + and st["zoom_step_request_id"] > request_id_before_gate): + st["ptz17_first_timing_owner"] = (visual_control.owner(st), st["zoom_step_pending_id"]) if gate_stop: zoom_zero_reason = gate_stop[-1] decision_generation = st.get("ptz_generation") @@ -9693,23 +9853,24 @@ elif zoom_cmd < 0.0 and float(size_est) <= max(0.05, zoomed_size_thr * 0.75): st["was_zoomed_in"] = False - _maybe_request_autofocus( - cam_idx, - st, - frame_bgr, - (x1, y1, x2, y2), - now=float(now), - pan_cmd=float(pan_speed) + float(pan_rate_ff), - tilt_cmd=float(tilt_speed) + float(tilt_rate_ff), - zoom_cmd=float(zoom_cmd), - center_err=float(center_err), - hfov_deg=float(hfov_deg), - validation_age=( - float(visual_control_validation_age) - if current_track_source == "visual" - else 0.0 - ), - ) + if not ptz17_focus_checked: + _maybe_request_autofocus( + cam_idx, + st, + frame_bgr, + (x1, y1, x2, y2), + now=float(now), + pan_cmd=float(pan_speed) + float(pan_rate_ff), + tilt_cmd=float(tilt_speed) + float(tilt_rate_ff), + zoom_cmd=float(zoom_cmd), + center_err=float(center_err), + hfov_deg=float(hfov_deg), + validation_age=( + float(visual_control_validation_age) + if current_track_source == "visual" + else 0.0 + ), + ) st["fallback_done"] = False @@ -10222,6 +10383,8 @@ if getattr(config, "PREDICTIVE_ZOOM_TELEMETRY_DIR", "logs/ptz_zoom"): blocked = {} if zoom_cmd <= 0.0 and raw_zoom_needed: + if ptz17_focus_reason is not None: + blocked["focus_hold"] = {"state": ptz17_focus_reason} if not stable_ok: blocked["centering"] = {"error": center_err, "limit": lock_gate, "stable_frames": st.get("stable_frames")} if not zoom_safe: