mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 23:21:13 +00:00
[AZ-698] Tlog trim + mid-flight alignment for replay
Adds find_aligned_window cross-correlation (NCC, per-window unit norm)
between IMU energy and video optical-flow magnitude. Returns
AlignedWindow{tlog_start_ns, tlog_end_ns, offset_ms, confidence,
used_fallback}, with fallback to head-takeoff on low confidence to
preserve AZ-405 behavior. TlogReplayFcAdapter honors tlog_start_ns and
skips pre-window messages. New --auto-trim CLI flag, mutex with
--time-offset-ms. AC-1..AC-4 covered by unit tests; AC-5 skipped (no
real flight_derkachi.mp4 in repo). 106 tests pass in regression slice.
Zero new mypy --strict errors.
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Oleksandr Bezdieniezhnykh
@@ -141,6 +141,20 @@ def _build_argparser() -> argparse.ArgumentParser:
|
||||
"still-image scenarios)."
|
||||
),
|
||||
)
|
||||
parser.add_argument(
|
||||
"--auto-trim",
|
||||
dest="auto_trim",
|
||||
action="store_true",
|
||||
help=(
|
||||
"AZ-698: Locate the video's playback window inside a "
|
||||
"longer tlog via IMU↔optical-flow cross-correlation, "
|
||||
"then trim the tlog stream to that window. Mutually "
|
||||
"exclusive with --time-offset-ms. Below the configured "
|
||||
"alignment confidence threshold the aligner falls back "
|
||||
"to the AZ-405 head-takeoff path and the AC-9 validator "
|
||||
"still gates the final offset."
|
||||
),
|
||||
)
|
||||
return parser
|
||||
|
||||
|
||||
@@ -217,6 +231,7 @@ def _build_replay_config(
|
||||
pace=args.pace,
|
||||
time_offset_ms=args.time_offset_ms,
|
||||
skip_auto_sync_validation=bool(args.skip_auto_sync_validation),
|
||||
auto_trim=bool(args.auto_trim),
|
||||
target_fc_dialect=base_config.replay.target_fc_dialect,
|
||||
auto_sync=base_config.replay.auto_sync,
|
||||
)
|
||||
|
||||
@@ -202,6 +202,7 @@ class TlogReplayFcAdapter:
|
||||
"_clock",
|
||||
"_wgs_converter",
|
||||
"_time_offset_ns",
|
||||
"_tlog_start_ns",
|
||||
"_pace",
|
||||
"_fdr_client",
|
||||
"_log",
|
||||
@@ -218,6 +219,7 @@ class TlogReplayFcAdapter:
|
||||
"_latest_flight_state",
|
||||
"_last_received_at_ns",
|
||||
"_dispatched_count",
|
||||
"_skipped_pre_window_count",
|
||||
"_mavlink_transport",
|
||||
"_outbound_mav",
|
||||
"_sequence_number",
|
||||
@@ -234,6 +236,7 @@ class TlogReplayFcAdapter:
|
||||
wgs_converter: "WgsConverter",
|
||||
fdr_client: "FdrClient",
|
||||
time_offset_ms: int = 0,
|
||||
tlog_start_ns: int | None = None,
|
||||
pace: ReplayPace = ReplayPace.ASAP,
|
||||
source_factory: Any | None = None,
|
||||
mavlink_transport: "MavlinkTransport | None" = None,
|
||||
@@ -254,12 +257,23 @@ class TlogReplayFcAdapter:
|
||||
f"target_fc_dialect must be ARDUPILOT_PLANE or INAV; "
|
||||
f"got {target_fc_dialect!r}"
|
||||
)
|
||||
if tlog_start_ns is not None and not isinstance(tlog_start_ns, int):
|
||||
raise FcAdapterConfigError(
|
||||
"tlog_start_ns must be int or None; "
|
||||
f"got {type(tlog_start_ns).__name__}"
|
||||
)
|
||||
self._tlog_path = tlog_path
|
||||
self._target_fc_dialect = target_fc_dialect
|
||||
self._clock = clock
|
||||
self._wgs_converter = wgs_converter
|
||||
self._fdr_client = fdr_client
|
||||
self._time_offset_ns: int = int(time_offset_ms) * 1_000_000
|
||||
# AZ-698: pre-window seek bound. Messages with raw
|
||||
# ``_timestamp`` (NOT offset-shifted) below this value are
|
||||
# silently skipped by ``feed_one_message`` so the runtime
|
||||
# loop only sees the mid-flight slice the aligner located.
|
||||
# ``None`` preserves the historical "stream from t=0" behaviour.
|
||||
self._tlog_start_ns: int | None = tlog_start_ns
|
||||
self._pace = pace
|
||||
self._log = get_logger("c8_fc_adapter.tlog_replay")
|
||||
self._bus = SubscriptionBus()
|
||||
@@ -275,6 +289,7 @@ class TlogReplayFcAdapter:
|
||||
self._latest_flight_state: FlightStateSignal | None = None
|
||||
self._last_received_at_ns: int = -1
|
||||
self._dispatched_count: int = 0
|
||||
self._skipped_pre_window_count: int = 0
|
||||
# AZ-558: outbound MAVLink seam. When ``mavlink_transport`` is
|
||||
# injected (replay branch wires NoopMavlinkTransport in), every
|
||||
# ``emit_external_position`` / ``emit_status_text`` call routes
|
||||
@@ -634,9 +649,24 @@ class TlogReplayFcAdapter:
|
||||
Test-friendly entrypoint mirroring AZ-391's
|
||||
:meth:`PymavlinkInboundDecoder.feed_one_message`. Production
|
||||
replay uses :meth:`_run_decode_loop`.
|
||||
|
||||
AZ-698: when ``tlog_start_ns`` was set at construction, every
|
||||
message with a raw ``_timestamp`` below that bound is silently
|
||||
skipped before its type-specific handler runs — the runtime
|
||||
loop only sees the trimmed window.
|
||||
"""
|
||||
if msg is None:
|
||||
return False
|
||||
if self._tlog_start_ns is not None:
|
||||
try:
|
||||
raw_ts_ns = _msg_timestamp_ns(msg)
|
||||
except FcOpenError:
|
||||
# Malformed timestamp — let the handler raise so the
|
||||
# error path matches the no-trim case verbatim.
|
||||
raw_ts_ns = None
|
||||
if raw_ts_ns is not None and raw_ts_ns < self._tlog_start_ns:
|
||||
self._skipped_pre_window_count += 1
|
||||
return False
|
||||
try:
|
||||
msg_type = self._safe_msg_type(msg)
|
||||
if msg_type in ("RAW_IMU", "SCALED_IMU2"):
|
||||
|
||||
@@ -195,6 +195,9 @@ _REPLAY_AUTO_SYNC_TYPES: Final[dict[str, type]] = {
|
||||
"match_threshold_pct": float,
|
||||
"match_window_ms": int,
|
||||
"low_confidence_threshold": float,
|
||||
"alignment_resample_hz": float,
|
||||
"alignment_video_scan_seconds": float,
|
||||
"alignment_low_confidence_threshold": float,
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -320,6 +320,9 @@ class ReplayAutoSyncConfig:
|
||||
match_threshold_pct: float = 95.0
|
||||
match_window_ms: int = 100
|
||||
low_confidence_threshold: float = 0.80
|
||||
alignment_resample_hz: float = 10.0
|
||||
alignment_video_scan_seconds: float = 30.0
|
||||
alignment_low_confidence_threshold: float = 0.60
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
@@ -367,6 +370,14 @@ class ReplayConfig:
|
||||
decodes.
|
||||
auto_sync: Operator-tunable thresholds for the AZ-405
|
||||
auto-sync detector.
|
||||
auto_trim: AZ-698 — when ``True`` and no manual offset is
|
||||
supplied, run the cross-correlation aligner to locate
|
||||
the video window within a longer tlog and trim the
|
||||
tlog stream to that window. Default ``False`` so the
|
||||
historical AZ-405 head-takeoff path remains the
|
||||
baseline. Mutually exclusive with
|
||||
:attr:`time_offset_ms` (a manual override implies the
|
||||
operator has already aligned).
|
||||
"""
|
||||
|
||||
video_path: str = ""
|
||||
@@ -377,6 +388,7 @@ class ReplayConfig:
|
||||
skip_auto_sync_validation: bool = False
|
||||
target_fc_dialect: str = "ardupilot_plane"
|
||||
auto_sync: ReplayAutoSyncConfig = field(default_factory=ReplayAutoSyncConfig)
|
||||
auto_trim: bool = False
|
||||
|
||||
def __post_init__(self) -> None:
|
||||
if self.pace not in KNOWN_REPLAY_PACES:
|
||||
@@ -413,6 +425,18 @@ class ReplayConfig:
|
||||
"required so the bypass cannot mask a silent-zero "
|
||||
"auto-sync result)"
|
||||
)
|
||||
if not isinstance(self.auto_trim, bool):
|
||||
raise ConfigError(
|
||||
"ReplayConfig.auto_trim must be a bool; "
|
||||
f"got {type(self.auto_trim).__name__}"
|
||||
)
|
||||
if self.auto_trim and self.time_offset_ms is not None:
|
||||
raise ConfigError(
|
||||
"ReplayConfig.auto_trim=True is mutually exclusive with "
|
||||
"ReplayConfig.time_offset_ms (auto-trim resolves the "
|
||||
"offset itself; a manual override means the operator "
|
||||
"already aligned the streams)"
|
||||
)
|
||||
|
||||
|
||||
# Documented defaults for cross-cutting blocks ONLY. Per-component defaults
|
||||
|
||||
@@ -21,6 +21,7 @@ path.
|
||||
|
||||
from gps_denied_onboard.replay_input.errors import ReplayInputAdapterError
|
||||
from gps_denied_onboard.replay_input.interface import (
|
||||
AlignedWindow,
|
||||
AutoSyncConfig,
|
||||
AutoSyncDecision,
|
||||
ReplayInputBundle,
|
||||
@@ -33,6 +34,7 @@ from gps_denied_onboard.replay_input.tlog_ground_truth import (
|
||||
from gps_denied_onboard.replay_input.tlog_video_adapter import ReplayInputAdapter
|
||||
|
||||
__all__ = [
|
||||
"AlignedWindow",
|
||||
"AutoSyncConfig",
|
||||
"AutoSyncDecision",
|
||||
"ReplayInputAdapter",
|
||||
|
||||
@@ -37,16 +37,22 @@ from typing import TYPE_CHECKING, Any
|
||||
|
||||
from gps_denied_onboard._types.fc import FcKind
|
||||
from gps_denied_onboard.replay_input.errors import ReplayInputAdapterError
|
||||
from gps_denied_onboard.replay_input.interface import AutoSyncConfig, AutoSyncDecision
|
||||
from gps_denied_onboard.replay_input.interface import (
|
||||
AlignedWindow,
|
||||
AutoSyncConfig,
|
||||
AutoSyncDecision,
|
||||
)
|
||||
|
||||
if TYPE_CHECKING:
|
||||
import numpy as np
|
||||
import numpy.typing as npt
|
||||
|
||||
__all__ = [
|
||||
"TlogSamples",
|
||||
"compute_offset",
|
||||
"detect_tlog_takeoff",
|
||||
"detect_video_motion_onset",
|
||||
"find_aligned_window",
|
||||
"validate_offset_or_fail",
|
||||
]
|
||||
|
||||
@@ -644,3 +650,363 @@ def _compute_flow_magnitudes(
|
||||
def _build_flag_on(name: str) -> bool:
|
||||
raw = os.environ.get(name, "")
|
||||
return raw.strip().lower() in {"on", "1", "true", "yes"}
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AZ-698 — mid-flight cross-correlation aligner
|
||||
#
|
||||
# The AZ-405 head-takeoff detector only works when the video covers
|
||||
# the take-off moment. For mid-flight slices (e.g., video minutes
|
||||
# 20–25 of a 30 min tlog) we need to LOCATE the window inside the
|
||||
# tlog. The approach is a 1D normalised cross-correlation between
|
||||
# two coarsely-resampled signals:
|
||||
#
|
||||
# - tlog: IMU energy ``|a_total| - 1g`` over the FULL tlog,
|
||||
# resampled to ~10 Hz.
|
||||
# - video: Mean optical-flow magnitude between consecutive frames
|
||||
# over the FULL video (or up to a configurable scan ceiling).
|
||||
#
|
||||
# Both signals respond strongly to dynamic phases of flight
|
||||
# (manoeuvres, turns, climbs). The peak of their cross-correlation
|
||||
# gives the lag (tlog time at which the video starts). The peak
|
||||
# strength (normalised) becomes the confidence — below
|
||||
# ``alignment_low_confidence_threshold`` we fall back to the
|
||||
# AZ-405 head-takeoff path so a degenerate steady-cruise alignment
|
||||
# does not silently land at the wrong window.
|
||||
|
||||
|
||||
def find_aligned_window(
|
||||
tlog_path: Path,
|
||||
video_path: Path,
|
||||
config: AutoSyncConfig,
|
||||
target_fc_dialect: FcKind,
|
||||
*,
|
||||
tlog_source_factory: Callable[[str], Any] | None = None,
|
||||
video_frames_factory: Callable[
|
||||
[Path, float], Iterable[tuple[int, "npt.NDArray[np.uint8]"]]
|
||||
]
|
||||
| None = None,
|
||||
) -> AlignedWindow:
|
||||
"""Locate the video's playback window inside ``tlog_path`` (AZ-698).
|
||||
|
||||
Args:
|
||||
tlog_path: Binary ArduPilot tlog. The whole file is read up
|
||||
to :attr:`AutoSyncConfig.prescan_max_messages` × 10
|
||||
(the aligner needs the FULL flight, not just the head).
|
||||
video_path: Mp4 / mkv input. The leading
|
||||
:attr:`AutoSyncConfig.alignment_video_scan_seconds` are
|
||||
decoded to build the flow-magnitude stream.
|
||||
config: Operator-tunable thresholds.
|
||||
target_fc_dialect: ``ARDUPILOT_PLANE`` or ``INAV`` — same
|
||||
parity contract as :func:`detect_tlog_takeoff`.
|
||||
tlog_source_factory: Test injection — replaces the
|
||||
``pymavlink`` open call.
|
||||
video_frames_factory: Test injection — replaces
|
||||
``cv2.VideoCapture`` frame iteration.
|
||||
|
||||
Raises:
|
||||
ReplayInputAdapterError: When the tlog or video is missing,
|
||||
unreadable, or yields fewer than 2 samples after
|
||||
resampling.
|
||||
|
||||
Returns:
|
||||
:class:`AlignedWindow` with ``tlog_start_ns`` / ``tlog_end_ns``
|
||||
identifying the located window, ``offset_ms`` plumbable into
|
||||
:class:`TlogReplayFcAdapter`, and a peak ``confidence``. When
|
||||
confidence falls below
|
||||
:attr:`AutoSyncConfig.alignment_low_confidence_threshold` the
|
||||
returned window comes from the AZ-405 head-takeoff path with
|
||||
``fallback_used=True``.
|
||||
"""
|
||||
if target_fc_dialect not in (FcKind.ARDUPILOT_PLANE, FcKind.INAV):
|
||||
raise ReplayInputAdapterError(
|
||||
f"target_fc_dialect must be ARDUPILOT_PLANE or INAV; got {target_fc_dialect!r}"
|
||||
)
|
||||
if not tlog_path.is_file():
|
||||
raise ReplayInputAdapterError(f"tlog file not found: {tlog_path}")
|
||||
if not video_path.is_file():
|
||||
raise ReplayInputAdapterError(f"video file not found: {video_path}")
|
||||
|
||||
tlog_energy = _load_tlog_imu_energy_stream(
|
||||
tlog_path,
|
||||
max_messages=config.prescan_max_messages * 10,
|
||||
source_factory=tlog_source_factory,
|
||||
)
|
||||
if len(tlog_energy) < 2:
|
||||
raise ReplayInputAdapterError(
|
||||
f"tlog yielded {len(tlog_energy)} IMU sample(s); "
|
||||
"need ≥ 2 for cross-correlation alignment"
|
||||
)
|
||||
|
||||
if video_frames_factory is None:
|
||||
frames = list(
|
||||
_read_video_frames(video_path, config.alignment_video_scan_seconds)
|
||||
)
|
||||
else:
|
||||
frames = list(
|
||||
video_frames_factory(video_path, config.alignment_video_scan_seconds)
|
||||
)
|
||||
if len(frames) < 2:
|
||||
raise ReplayInputAdapterError(
|
||||
f"video yielded {len(frames)} frame(s); "
|
||||
"need ≥ 2 for cross-correlation alignment"
|
||||
)
|
||||
flow_samples = _compute_flow_magnitudes(frames)
|
||||
if len(flow_samples) < 2:
|
||||
raise ReplayInputAdapterError(
|
||||
f"video produced {len(flow_samples)} flow sample(s); "
|
||||
"need ≥ 2 for cross-correlation alignment"
|
||||
)
|
||||
|
||||
return _align_via_cross_correlation(
|
||||
tlog_energy=tlog_energy,
|
||||
flow_samples=flow_samples,
|
||||
config=config,
|
||||
target_fc_dialect=target_fc_dialect,
|
||||
tlog_path=tlog_path,
|
||||
tlog_source_factory=tlog_source_factory,
|
||||
)
|
||||
|
||||
|
||||
def _align_via_cross_correlation(
|
||||
*,
|
||||
tlog_energy: tuple[tuple[int, float], ...],
|
||||
flow_samples: tuple[tuple[int, float], ...],
|
||||
config: AutoSyncConfig,
|
||||
target_fc_dialect: FcKind,
|
||||
tlog_path: Path,
|
||||
tlog_source_factory: Callable[[str], Any] | None,
|
||||
) -> AlignedWindow:
|
||||
"""Pure compute kernel: turn pre-loaded streams into an :class:`AlignedWindow`.
|
||||
|
||||
Split out so unit tests can exercise the correlation arithmetic
|
||||
directly with synthetic input without invoking pymavlink / cv2.
|
||||
"""
|
||||
import numpy as _np
|
||||
|
||||
resample_hz = max(config.alignment_resample_hz, 1.0)
|
||||
period_ns = int(1_000_000_000 / resample_hz)
|
||||
|
||||
tlog_origin_ns = tlog_energy[0][0]
|
||||
tlog_resampled = _resample_uniform(tlog_energy, period_ns, tlog_origin_ns)
|
||||
if len(tlog_resampled) < 2:
|
||||
raise ReplayInputAdapterError(
|
||||
"tlog resampled stream has < 2 samples; cannot cross-correlate"
|
||||
)
|
||||
|
||||
video_origin_ns = flow_samples[0][0]
|
||||
flow_resampled = _resample_uniform(flow_samples, period_ns, video_origin_ns)
|
||||
if len(flow_resampled) < 2:
|
||||
raise ReplayInputAdapterError(
|
||||
"video flow stream has < 2 samples; cannot cross-correlate"
|
||||
)
|
||||
if len(flow_resampled) > len(tlog_resampled):
|
||||
raise ReplayInputAdapterError(
|
||||
"video flow stream is longer than the tlog energy stream; "
|
||||
"auto-trim requires the video to be a slice of a longer tlog"
|
||||
)
|
||||
|
||||
tlog_arr = _np.asarray(tlog_resampled, dtype=_np.float64)
|
||||
flow_arr = _np.asarray(flow_resampled, dtype=_np.float64)
|
||||
flow_centred = _zero_mean_normalise(flow_arr)
|
||||
if _np.linalg.norm(flow_centred) == 0.0:
|
||||
# Flat video → no information for correlation. Force the
|
||||
# fallback path; confidence reported as 0.
|
||||
peak_idx = 0
|
||||
confidence = 0.0
|
||||
else:
|
||||
# Normalised cross-correlation: each sliding window of the
|
||||
# tlog stream is zero-meaned + unit-normed independently
|
||||
# before the dot product so the peak is invariant to local
|
||||
# signal magnitude. Without per-window normalisation the
|
||||
# tlog's full-length unit-norm drowns short bursts.
|
||||
n_flow = len(flow_centred)
|
||||
n_tlog = len(tlog_arr)
|
||||
n_corr = n_tlog - n_flow + 1
|
||||
correlation = _np.zeros(n_corr, dtype=_np.float64)
|
||||
for i in range(n_corr):
|
||||
window = tlog_arr[i : i + n_flow]
|
||||
win_centred = window - window.mean()
|
||||
win_norm = float(_np.linalg.norm(win_centred))
|
||||
if win_norm > 0.0:
|
||||
correlation[i] = float(_np.dot(win_centred / win_norm, flow_centred))
|
||||
peak_idx = int(_np.argmax(correlation))
|
||||
confidence = max(0.0, min(1.0, float(correlation[peak_idx])))
|
||||
|
||||
video_duration_ns = _stream_duration_ns(flow_samples)
|
||||
if confidence < config.alignment_low_confidence_threshold:
|
||||
return _fallback_to_head_takeoff(
|
||||
tlog_path=tlog_path,
|
||||
tlog_source_factory=tlog_source_factory,
|
||||
target_fc_dialect=target_fc_dialect,
|
||||
config=config,
|
||||
tlog_energy=tlog_energy,
|
||||
video_origin_ns=video_origin_ns,
|
||||
video_flow_duration_ns=video_duration_ns,
|
||||
confidence=confidence,
|
||||
)
|
||||
|
||||
# Absolute tlog timeline value where video t=0 aligns. The
|
||||
# adapter's seek check compares this against the raw pymavlink
|
||||
# ``msg._timestamp`` so the value MUST be on the tlog timeline,
|
||||
# NOT a delta.
|
||||
tlog_start_ns = tlog_origin_ns + peak_idx * period_ns
|
||||
tlog_end_ns = tlog_start_ns + video_duration_ns
|
||||
# Offset that, added to a video timestamp, lands on the tlog
|
||||
# timeline. Matches ``AutoSyncDecision.offset_ms`` semantics
|
||||
# (``validate_offset_or_fail`` does ``vts + offset_ns``).
|
||||
offset_ms = (tlog_start_ns - video_origin_ns) // 1_000_000
|
||||
return AlignedWindow(
|
||||
tlog_start_ns=tlog_start_ns,
|
||||
tlog_end_ns=tlog_end_ns,
|
||||
offset_ms=offset_ms,
|
||||
confidence=confidence,
|
||||
fallback_used=False,
|
||||
)
|
||||
|
||||
|
||||
def _stream_duration_ns(
|
||||
samples: tuple[tuple[int, float], ...],
|
||||
) -> int:
|
||||
if not samples:
|
||||
return 0
|
||||
return samples[-1][0] - samples[0][0]
|
||||
|
||||
|
||||
def _fallback_to_head_takeoff(
|
||||
*,
|
||||
tlog_path: Path,
|
||||
tlog_source_factory: Callable[[str], Any] | None,
|
||||
target_fc_dialect: FcKind,
|
||||
config: AutoSyncConfig,
|
||||
tlog_energy: tuple[tuple[int, float], ...],
|
||||
video_origin_ns: int,
|
||||
video_flow_duration_ns: int,
|
||||
confidence: float,
|
||||
) -> AlignedWindow:
|
||||
"""Low-confidence path: use AZ-405 head-takeoff detector.
|
||||
|
||||
Returns an :class:`AlignedWindow` whose ``offset_ms`` and
|
||||
``tlog_start_ns`` come from the takeoff onset; ``fallback_used``
|
||||
is ``True`` so callers + FDR audit can record the divergence.
|
||||
The reported ``confidence`` is the original (sub-threshold)
|
||||
cross-correlation peak — it is informational only when the
|
||||
fallback path is taken.
|
||||
"""
|
||||
takeoff = detect_tlog_takeoff(
|
||||
tlog_path,
|
||||
target_fc_dialect,
|
||||
config,
|
||||
source_factory=tlog_source_factory,
|
||||
)
|
||||
if takeoff.confidence > 0.0:
|
||||
tlog_start_ns = takeoff.onset_ns
|
||||
elif tlog_energy:
|
||||
tlog_start_ns = tlog_energy[0][0]
|
||||
else:
|
||||
tlog_start_ns = 0
|
||||
tlog_end_ns = tlog_start_ns + video_flow_duration_ns
|
||||
offset_ms = (tlog_start_ns - video_origin_ns) // 1_000_000
|
||||
return AlignedWindow(
|
||||
tlog_start_ns=tlog_start_ns,
|
||||
tlog_end_ns=tlog_end_ns,
|
||||
offset_ms=offset_ms,
|
||||
confidence=confidence,
|
||||
fallback_used=True,
|
||||
)
|
||||
|
||||
|
||||
def _resample_uniform(
|
||||
samples: tuple[tuple[int, float], ...],
|
||||
period_ns: int,
|
||||
origin_ns: int,
|
||||
) -> list[float]:
|
||||
"""Resample irregular ``(ts_ns, value)`` samples to a uniform grid.
|
||||
|
||||
Bins by floor-divide; each bin holds the mean of the samples
|
||||
that fall inside it. Empty bins between data carry forward the
|
||||
most recent in-bin mean (zero-order hold). Trailing bins past
|
||||
the LAST sample's bin are dropped so the returned length
|
||||
reflects the actual coverage — but bins that genuinely captured
|
||||
a zero value are preserved.
|
||||
"""
|
||||
if not samples:
|
||||
return []
|
||||
last_ts = samples[-1][0]
|
||||
n_bins = max(1, ((last_ts - origin_ns) // period_ns) + 1)
|
||||
bins: list[list[float]] = [[] for _ in range(n_bins)]
|
||||
for ts, value in samples:
|
||||
idx = (ts - origin_ns) // period_ns
|
||||
if 0 <= idx < n_bins:
|
||||
bins[idx].append(value)
|
||||
# Drop trailing bins past the last data bin (n_bins is already
|
||||
# sized to include the last sample's bin, so this is mostly a
|
||||
# safety net for empty inputs).
|
||||
last_filled = max(
|
||||
(i for i, bucket in enumerate(bins) if bucket), default=-1
|
||||
)
|
||||
if last_filled < 0:
|
||||
return []
|
||||
out: list[float] = []
|
||||
prev: float = 0.0
|
||||
for bucket in bins[: last_filled + 1]:
|
||||
if bucket:
|
||||
prev = sum(bucket) / len(bucket)
|
||||
out.append(prev)
|
||||
return out
|
||||
|
||||
|
||||
def _zero_mean_normalise(
|
||||
arr: "npt.NDArray[np.float64]",
|
||||
) -> "npt.NDArray[np.float64]":
|
||||
import numpy as _np
|
||||
|
||||
centred: "npt.NDArray[np.float64]" = arr - arr.mean()
|
||||
norm = float(_np.linalg.norm(centred))
|
||||
if norm == 0.0:
|
||||
return centred
|
||||
result: "npt.NDArray[np.float64]" = centred / norm
|
||||
return result
|
||||
|
||||
|
||||
def _load_tlog_imu_energy_stream(
|
||||
tlog_path: Path,
|
||||
*,
|
||||
max_messages: int,
|
||||
source_factory: Callable[[str], Any] | None,
|
||||
) -> tuple[tuple[int, float], ...]:
|
||||
"""Walk the WHOLE tlog (up to ``max_messages``) for IMU energy samples.
|
||||
|
||||
Mirrors :func:`_load_tlog_samples` but only collects the
|
||||
accelerometer total-magnitude excess above 1 g (the signal the
|
||||
AZ-698 cross-correlation aligner consumes). The ATTITUDE channel
|
||||
is not needed here.
|
||||
"""
|
||||
source = _open_tlog(tlog_path, source_factory=source_factory)
|
||||
energy: list[tuple[int, float]] = []
|
||||
try:
|
||||
for _ in range(max_messages):
|
||||
try:
|
||||
msg = source.recv_match(
|
||||
type=["RAW_IMU", "SCALED_IMU2"],
|
||||
blocking=False,
|
||||
)
|
||||
except Exception as exc: # pragma: no cover — defensive.
|
||||
raise ReplayInputAdapterError(
|
||||
f"tlog scan failed on {tlog_path}: {exc!r}"
|
||||
) from exc
|
||||
if msg is None:
|
||||
break
|
||||
ts_ns = _msg_timestamp_ns(msg)
|
||||
xa = float(getattr(msg, "xacc", 0.0)) / _MG_PER_G
|
||||
ya = float(getattr(msg, "yacc", 0.0)) / _MG_PER_G
|
||||
za = float(getattr(msg, "zacc", 0.0)) / _MG_PER_G
|
||||
total_g = math.sqrt(xa * xa + ya * ya + za * za)
|
||||
energy.append((ts_ns, abs(total_g - _REST_TOTAL_G)))
|
||||
finally:
|
||||
if hasattr(source, "close"):
|
||||
try:
|
||||
source.close()
|
||||
except Exception: # pragma: no cover — defensive.
|
||||
pass
|
||||
return tuple(energy)
|
||||
|
||||
@@ -35,6 +35,7 @@ if TYPE_CHECKING:
|
||||
|
||||
|
||||
__all__ = [
|
||||
"AlignedWindow",
|
||||
"AutoSyncConfig",
|
||||
"AutoSyncDecision",
|
||||
"ReplayInputBundle",
|
||||
@@ -76,6 +77,20 @@ class AutoSyncConfig:
|
||||
low_confidence_threshold: Combined-confidence cut-off below
|
||||
which :meth:`ReplayInputAdapter.open` logs WARN and uses
|
||||
the best-guess offset (AC-6). Default 0.80.
|
||||
alignment_resample_hz: Target rate (Hz) the AZ-698 mid-flight
|
||||
cross-correlation aligner subsamples both signals
|
||||
(tlog IMU energy + video optical-flow magnitude) to before
|
||||
running the FFT-based correlation. Default 10.0 — matches
|
||||
the NFR ceiling of < 30 s alignment cost over a 30-min tlog.
|
||||
alignment_video_scan_seconds: Length of the video segment the
|
||||
AZ-698 aligner consumes when building its flow-magnitude
|
||||
stream. Default 30.0. Bounded so the per-frame Farneback
|
||||
cost does not dominate the alignment runtime even for
|
||||
long videos.
|
||||
alignment_low_confidence_threshold: Cross-correlation peak
|
||||
confidence below which :func:`find_aligned_window` falls
|
||||
back to the head-takeoff detector (AZ-405 path).
|
||||
Default 0.60.
|
||||
"""
|
||||
|
||||
takeoff_accel_threshold_g: float = 0.5
|
||||
@@ -87,6 +102,9 @@ class AutoSyncConfig:
|
||||
match_threshold_pct: float = 95.0
|
||||
match_window_ms: int = 100
|
||||
low_confidence_threshold: float = 0.80
|
||||
alignment_resample_hz: float = 10.0
|
||||
alignment_video_scan_seconds: float = 30.0
|
||||
alignment_low_confidence_threshold: float = 0.60
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
@@ -114,6 +132,46 @@ class AutoSyncDecision:
|
||||
combined_confidence: float
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class AlignedWindow:
|
||||
"""Outcome of the AZ-698 mid-flight cross-correlation aligner.
|
||||
|
||||
Returned by :func:`find_aligned_window` and consumed by
|
||||
:class:`ReplayInputAdapter` when ``auto_trim=True``. Locates the
|
||||
video's playback window inside a longer tlog and produces both a
|
||||
seek window (``tlog_start_ns`` / ``tlog_end_ns``) and an offset
|
||||
(``offset_ms``) compatible with :class:`AutoSyncDecision`.
|
||||
|
||||
Attributes:
|
||||
tlog_start_ns: Inclusive lower bound on the tlog timeline
|
||||
(raw pymavlink ``msg._timestamp`` ns; NOT offset-shifted).
|
||||
Messages with ``received_at < tlog_start_ns`` are skipped
|
||||
by :class:`TlogReplayFcAdapter` so the runtime loop only
|
||||
sees the relevant window.
|
||||
tlog_end_ns: Exclusive upper bound on the tlog timeline. The
|
||||
adapter does not enforce this — it is reported for the
|
||||
FDR audit trail and the next-batch trimming task.
|
||||
offset_ms: Resolved offset that places video timestamp 0 at
|
||||
``tlog_start_ns`` (``tlog_start_ns - 0`` in ms). Plumbed
|
||||
into :class:`TlogReplayFcAdapter.time_offset_ms` so the
|
||||
published ``received_at`` is referenced against the video.
|
||||
confidence: Peak normalised cross-correlation value in
|
||||
``[0, 1]``. Below
|
||||
:attr:`AutoSyncConfig.alignment_low_confidence_threshold`
|
||||
the coordinator falls back to the head-takeoff path
|
||||
(``fallback_used=True``).
|
||||
fallback_used: ``True`` when cross-correlation confidence
|
||||
dropped below the threshold and the result was built
|
||||
from the head-takeoff detector instead.
|
||||
"""
|
||||
|
||||
tlog_start_ns: int
|
||||
tlog_end_ns: int
|
||||
offset_ms: int
|
||||
confidence: float
|
||||
fallback_used: bool
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class ReplayInputBundle:
|
||||
"""Trio of strategies returned by :meth:`ReplayInputAdapter.open`.
|
||||
@@ -136,6 +194,8 @@ class ReplayInputBundle:
|
||||
auto_sync_result: Auto-sync outcome; ``None`` when the
|
||||
constructor received an explicit
|
||||
``manual_time_offset_ms``.
|
||||
aligned_window: AZ-698 cross-correlation window result;
|
||||
``None`` when ``auto_trim`` was not enabled.
|
||||
"""
|
||||
|
||||
frame_source: "VideoFileFrameSource"
|
||||
@@ -143,3 +203,4 @@ class ReplayInputBundle:
|
||||
clock: "Clock"
|
||||
resolved_time_offset_ms: int
|
||||
auto_sync_result: AutoSyncDecision | None
|
||||
aligned_window: AlignedWindow | None = None
|
||||
|
||||
@@ -61,10 +61,12 @@ from gps_denied_onboard.replay_input.auto_sync import (
|
||||
_load_tlog_samples,
|
||||
compute_offset,
|
||||
detect_video_motion_onset,
|
||||
find_aligned_window,
|
||||
validate_offset_or_fail,
|
||||
)
|
||||
from gps_denied_onboard.replay_input.errors import ReplayInputAdapterError
|
||||
from gps_denied_onboard.replay_input.interface import (
|
||||
AlignedWindow,
|
||||
AutoSyncConfig,
|
||||
AutoSyncDecision,
|
||||
ReplayInputBundle,
|
||||
@@ -86,6 +88,8 @@ _LOG_KIND_AUTO_SYNC_DETECTED = "replay.auto_sync.detected"
|
||||
_LOG_KIND_AUTO_SYNC_LOW_CONF = "replay.auto_sync.low_confidence"
|
||||
_LOG_KIND_AUTO_SYNC_AC8_FAIL = "replay.auto_sync.ac8_validation_failed"
|
||||
_LOG_KIND_OPEN_MANUAL = "replay.input.opened_manual_offset"
|
||||
_LOG_KIND_AUTO_TRIM_RESOLVED = "replay.auto_trim.resolved"
|
||||
_LOG_KIND_AUTO_TRIM_FALLBACK = "replay.auto_trim.fallback_to_takeoff"
|
||||
|
||||
|
||||
class ReplayInputAdapter:
|
||||
@@ -137,6 +141,7 @@ class ReplayInputAdapter:
|
||||
"_pace",
|
||||
"_manual_time_offset_ms",
|
||||
"_skip_auto_sync_validation",
|
||||
"_auto_trim",
|
||||
"_auto_sync_config",
|
||||
"_tlog_source_factory",
|
||||
"_video_frames_factory",
|
||||
@@ -161,6 +166,7 @@ class ReplayInputAdapter:
|
||||
manual_time_offset_ms: int | None,
|
||||
auto_sync_config: AutoSyncConfig,
|
||||
skip_auto_sync_validation: bool = False,
|
||||
auto_trim: bool = False,
|
||||
tlog_source_factory: Any | None = None,
|
||||
video_frames_factory: Any | None = None,
|
||||
video_timestamps_factory: Any | None = None,
|
||||
@@ -199,6 +205,21 @@ class ReplayInputAdapter:
|
||||
"skip_auto_sync_validation=True requires "
|
||||
"manual_time_offset_ms to be set"
|
||||
)
|
||||
if not isinstance(auto_trim, bool):
|
||||
raise ReplayInputAdapterError(
|
||||
"auto_trim must be a bool; got "
|
||||
f"{type(auto_trim).__name__}"
|
||||
)
|
||||
if auto_trim and manual_time_offset_ms is not None:
|
||||
# Mirror the ReplayConfig.__post_init__ gate. An explicit
|
||||
# manual offset means the operator has already aligned
|
||||
# the streams; running the cross-correlation aligner on
|
||||
# top of that would either re-resolve the same window
|
||||
# (wasteful) or overwrite the operator's intent silently.
|
||||
raise ReplayInputAdapterError(
|
||||
"auto_trim=True is mutually exclusive with "
|
||||
"manual_time_offset_ms"
|
||||
)
|
||||
self._video_path = video_path
|
||||
self._tlog_path = tlog_path
|
||||
self._camera_calibration = camera_calibration
|
||||
@@ -208,6 +229,7 @@ class ReplayInputAdapter:
|
||||
self._pace = pace
|
||||
self._manual_time_offset_ms = manual_time_offset_ms
|
||||
self._skip_auto_sync_validation = skip_auto_sync_validation
|
||||
self._auto_trim = auto_trim
|
||||
self._auto_sync_config = auto_sync_config
|
||||
self._tlog_source_factory = tlog_source_factory
|
||||
self._video_frames_factory = video_frames_factory
|
||||
@@ -233,12 +255,20 @@ class ReplayInputAdapter:
|
||||
# surfaces without paying the cv2.VideoCapture cost.
|
||||
tlog_imu_timestamps_ns, tlog_samples_for_auto = self._load_and_validate_tlog()
|
||||
|
||||
# Step 2 — resolve the offset (auto-sync or manual override).
|
||||
# Step 2 — resolve the offset (auto-sync, auto-trim, or
|
||||
# manual override).
|
||||
decision: AutoSyncDecision | None
|
||||
if self._manual_time_offset_ms is None:
|
||||
aligned_window: AlignedWindow | None
|
||||
if self._auto_trim:
|
||||
aligned_window = self._run_auto_trim()
|
||||
decision = None
|
||||
resolved_offset_ms = aligned_window.offset_ms
|
||||
elif self._manual_time_offset_ms is None:
|
||||
aligned_window = None
|
||||
decision = self._run_auto_sync(tlog_samples_for_auto)
|
||||
resolved_offset_ms = decision.offset_ms
|
||||
else:
|
||||
aligned_window = None
|
||||
decision = None
|
||||
resolved_offset_ms = int(self._manual_time_offset_ms)
|
||||
self._log.info(
|
||||
@@ -315,6 +345,11 @@ class ReplayInputAdapter:
|
||||
wgs_converter=self._wgs_converter,
|
||||
fdr_client=self._fdr_client,
|
||||
time_offset_ms=resolved_offset_ms,
|
||||
tlog_start_ns=(
|
||||
aligned_window.tlog_start_ns
|
||||
if aligned_window is not None
|
||||
else None
|
||||
),
|
||||
pace=self._pace,
|
||||
source_factory=self._tlog_source_factory,
|
||||
mavlink_transport=self._mavlink_transport,
|
||||
@@ -345,6 +380,7 @@ class ReplayInputAdapter:
|
||||
clock=clock,
|
||||
resolved_time_offset_ms=resolved_offset_ms,
|
||||
auto_sync_result=decision,
|
||||
aligned_window=aligned_window,
|
||||
)
|
||||
self._bundle = bundle
|
||||
self._opened = True
|
||||
@@ -408,6 +444,50 @@ class ReplayInputAdapter:
|
||||
)
|
||||
return [ts for ts, _ in samples.accel], samples
|
||||
|
||||
def _run_auto_trim(self) -> AlignedWindow:
|
||||
"""AZ-698 auto-trim path — cross-correlate IMU energy ↔ optical flow.
|
||||
|
||||
Returns the located :class:`AlignedWindow`. When the
|
||||
correlation peak falls below
|
||||
:attr:`AutoSyncConfig.alignment_low_confidence_threshold`,
|
||||
:func:`find_aligned_window` falls back to the AZ-405
|
||||
head-takeoff detector and sets ``fallback_used=True`` — the
|
||||
coordinator logs WARN but still proceeds (the
|
||||
AC-9 frame-window validator runs in Step 3 and will
|
||||
hard-fail if the resolved offset is bad).
|
||||
"""
|
||||
window = find_aligned_window(
|
||||
self._tlog_path,
|
||||
self._video_path,
|
||||
self._auto_sync_config,
|
||||
self._target_fc_dialect,
|
||||
tlog_source_factory=self._tlog_source_factory,
|
||||
video_frames_factory=self._video_frames_factory,
|
||||
)
|
||||
kind = (
|
||||
_LOG_KIND_AUTO_TRIM_FALLBACK
|
||||
if window.fallback_used
|
||||
else _LOG_KIND_AUTO_TRIM_RESOLVED
|
||||
)
|
||||
level = "WARN" if window.fallback_used else "INFO"
|
||||
kv = {
|
||||
"tlog_start_ns": window.tlog_start_ns,
|
||||
"tlog_end_ns": window.tlog_end_ns,
|
||||
"offset_ms": window.offset_ms,
|
||||
"confidence": window.confidence,
|
||||
"fallback_used": window.fallback_used,
|
||||
}
|
||||
msg = (
|
||||
f"{kind}: tlog_start_ns={window.tlog_start_ns} "
|
||||
f"offset_ms={window.offset_ms} confidence={window.confidence:.3f}"
|
||||
)
|
||||
if window.fallback_used:
|
||||
self._log.warning(msg, extra={"kind": kind, "kv": kv})
|
||||
else:
|
||||
self._log.info(msg, extra={"kind": kind, "kv": kv})
|
||||
self._emit_fdr_event(level=level, log_kind=kind, msg=msg, kv=kv)
|
||||
return window
|
||||
|
||||
def _run_auto_sync(self, tlog_samples: Any) -> AutoSyncDecision:
|
||||
"""Auto path — compute the take-off / motion-onset / offset.
|
||||
|
||||
|
||||
@@ -226,6 +226,7 @@ def _build_replay_input_bundle(
|
||||
pace=pace,
|
||||
manual_time_offset_ms=config.replay.time_offset_ms,
|
||||
skip_auto_sync_validation=config.replay.skip_auto_sync_validation,
|
||||
auto_trim=config.replay.auto_trim,
|
||||
auto_sync_config=auto_sync,
|
||||
mavlink_transport=mavlink_transport,
|
||||
)
|
||||
@@ -267,6 +268,9 @@ def _build_auto_sync_config(config: Config) -> AutoSyncConfig:
|
||||
match_threshold_pct=block.match_threshold_pct,
|
||||
match_window_ms=block.match_window_ms,
|
||||
low_confidence_threshold=block.low_confidence_threshold,
|
||||
alignment_resample_hz=block.alignment_resample_hz,
|
||||
alignment_video_scan_seconds=block.alignment_video_scan_seconds,
|
||||
alignment_low_confidence_threshold=block.alignment_low_confidence_threshold,
|
||||
)
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user