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_causal_image_rate/image_rate.patch

141 lines
6.7 KiB
Diff

--- a/postprocess.py
+++ b/postprocess.py
@@ -761,6 +761,7 @@
edge_margin_min: float = 0.30
center_max_norm: float = 0.12
image_rate_max_norm_s: float = 0.04
+ image_rate_min_interval_s: float = 0.04
_PTZ17_ZOOM = _PTZ17ZoomTuning()
@@ -3320,6 +3321,55 @@
# TRACK/ACQUIRE boundary.
+def _update_image_rate_ema(st: dict, dx_norm: float, dy_norm: float, dt_s: float) -> None:
+ """Update X-right/Y-down rates in full frame fractions per second."""
+ image_rate_max = max(0.1, float(getattr(config, "AIM_IMAGE_RATE_MAX", 2.0)))
+ vx_i = clipf(dx_norm / dt_s, -image_rate_max, image_rate_max)
+ vy_i = clipf(dy_norm / dt_s, -image_rate_max, image_rate_max)
+ vel_smooth = clipf(float(getattr(config, "VEL_SMOOTH", 0.85)), 0.0, 0.99)
+ st["vx"] = vel_smooth * st["vx"] + (1.0 - vel_smooth) * vx_i
+ st["vy"] = vel_smooth * st["vy"] + (1.0 - vel_smooth) * vy_i
+ fast_smooth = clipf(float(getattr(config, "AIM_IMAGE_RATE_FAST_SMOOTH", 0.45)), 0.0, 0.95)
+ st["vx_fast"] = fast_smooth * float(st.get("vx_fast", 0.0) or 0.0) + (1.0 - fast_smooth) * vx_i
+ st["vy_fast"] = fast_smooth * float(st.get("vy_fast", 0.0) or 0.0) + (1.0 - fast_smooth) * vy_i
+
+
+@dataclass(frozen=True, slots=True)
+class _ImageRateSample:
+ latest_at_s: float
+ baseline_at_s: float
+ x_norm: float
+ y_norm: float
+ have_rate: bool
+
+
+def _update_ptz17_image_rate(st: dict, measured_at_s: float, x_norm: float, y_norm: float) -> str:
+ """Use a causal baseline and accumulate short samples without inventing elapsed time."""
+ if not all(map(math.isfinite, (measured_at_s, x_norm, y_norm))):
+ return "invalid"
+ previous = st.get("ptz17_image_rate_ref")
+ if previous is None:
+ st["ptz17_image_rate_ref"] = _ImageRateSample(measured_at_s, measured_at_s, x_norm, y_norm, False)
+ return "seed"
+ if measured_at_s <= previous.latest_at_s:
+ return "noncausal"
+ dt_s = measured_at_s - previous.baseline_at_s
+ dx_norm, dy_norm = x_norm - previous.x_norm, y_norm - previous.y_norm
+ if dt_s < _PTZ17_ZOOM.image_rate_min_interval_s:
+ # Advance the ordering barrier without moving the derivative baseline.
+ st["ptz17_image_rate_ref"] = _ImageRateSample(
+ measured_at_s, previous.baseline_at_s, previous.x_norm, previous.y_norm, previous.have_rate,
+ )
+ # Use the prior rate only if the whole displacement fits the motion gate.
+ rate_max = min(0.08, float(getattr(config, "ZOOM_STEP_USEFUL_IMAGE_RATE_MAX", 0.08)))
+ if previous.have_rate and abs(dx_norm) + abs(dy_norm) <= rate_max * dt_s:
+ return "short_checked"
+ return "short"
+ _update_image_rate_ema(st, dx_norm, dy_norm, dt_s)
+ st["ptz17_image_rate_ref"] = _ImageRateSample(measured_at_s, measured_at_s, x_norm, y_norm, True)
+ return "updated"
+
+
def _reset_aim_motion_feedforward(st: dict, now: float) -> None:
"""Clear stale target-motion estimates without disturbing TRACK/lens state.
@@ -3333,6 +3383,7 @@
its feedback barrier instead of erasing the lens transaction.
"""
st["prev_t"] = None
+ st.pop("ptz17_image_rate_ref", None)
st["prev_cx"] = 0.5
st["prev_cy"] = 0.5
st["vx"] = 0.0
@@ -3416,6 +3467,7 @@
) -> None:
"""Discard motion history that must not cross a new TRACK boundary."""
st["prev_t"] = None
+ st.pop("ptz17_image_rate_ref", None)
st["prev_cx"] = 0.5
st["prev_cy"] = 0.5
st["vx"] = 0.0
@@ -8907,18 +8959,12 @@
min_margin = min(left, right, top, bottom)
prev_t = st.get("prev_t")
- dt = None
- if prev_t is not None:
- dt = max(now - float(prev_t), 1e-3)
- image_rate_max = max(0.1, float(getattr(config, "AIM_IMAGE_RATE_MAX", 2.0)))
- vx_i = clipf((cxn - st["prev_cx"]) / dt, -image_rate_max, image_rate_max)
- vy_i = clipf((cyn - st["prev_cy"]) / dt, -image_rate_max, image_rate_max)
- vel_smooth = clipf(float(getattr(config, "VEL_SMOOTH", 0.85)), 0.0, 0.99)
- st["vx"] = vel_smooth * st["vx"] + (1.0 - vel_smooth) * vx_i
- st["vy"] = vel_smooth * st["vy"] + (1.0 - vel_smooth) * vy_i
- fast_smooth = clipf(float(getattr(config, "AIM_IMAGE_RATE_FAST_SMOOTH", 0.45)), 0.0, 0.95)
- st["vx_fast"] = fast_smooth * float(st.get("vx_fast", 0.0) or 0.0) + (1.0 - fast_smooth) * vx_i
- st["vy_fast"] = fast_smooth * float(st.get("vy_fast", 0.0) or 0.0) + (1.0 - fast_smooth) * vy_i
+ dt = max(now - float(prev_t), 1e-3) if prev_t is not None else None
+ image_rate_sample = "legacy"
+ if cam_idx == _PTZ17_CAM_IDX:
+ image_rate_sample = _update_ptz17_image_rate(st, now, cxn, cyn)
+ elif dt is not None:
+ _update_image_rate_ema(st, cxn - st["prev_cx"], cyn - st["prev_cy"], dt)
# Convert image displacement to a true line-of-sight angle before any
# steering decisions. Normalized pixels are not a physical motion unit:
@@ -9705,6 +9751,7 @@
)
zoom_rearm_ready = _zoom_revalidation_ready(st)
stable_ok = stable_ok or predictive_entry or predictive_continuing
+ image_rate_ready = image_rate_sample in ("legacy", "updated", "short_checked")
can_zoom_in = (
zoom_safe
and zoom_angular_safe
@@ -9716,6 +9763,7 @@
or bounded_identification_request
)
and fast_drift <= float(getattr(config, "ZOOM_IN_FAST_DRIFT_MAX", 0.45))
+ and image_rate_ready
and now >= float(st.get("focus_settle_until", 0.0) or 0.0)
)
@@ -10739,6 +10787,7 @@
("edge", near_edge or edge_recovery_active),
("lock", st.get("zoom_lock", False) and not bounded_identification_request),
("drift", fast_drift > float(getattr(config, "ZOOM_IN_FAST_DRIFT_MAX", 0.45))),
+ ("image_rate_sample", not image_rate_ready),
("focus", now < float(st.get("focus_settle_until", 0.0) or 0.0)),
("coast", visual_control_coast),
("stale", visual_control_stale),
@@ -10760,6 +10809,8 @@
center_error_norm=max(abs(float(measured_dx)), abs(float(measured_dy))),
edge_margin_norm=float(edge_risk_margin), approach_margin_norm=approach_margin,
image_rate_norm_s=max(float(vel), float(fast_drift)),
+ image_rate_sample=image_rate_sample,
+ frame_clock="visual_receipt" if visual_only else "control_entry",
zoom_requested=bool(raw_zoom_needed), zoom_before_gate=ptz17_zoom_before_gate,
zoom_command=float(zoom_cmd), proposed_native=int(timing_native),
reason=zoom_zero_reason if zoom_cmd <= 0.0 else "positive_command",