mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 21:31:13 +00:00
[AZ-358] [AZ-361] C4 OpenCVGtsamPoseEstimator + Jacobian thermal hybrid
Implement the single production-default C4 PoseEstimator strategy. AZ-358 — Marginals path: OpenCV solvePnPRansac (SOLVEPNP_IPPE) on best-candidate inliers, PriorFactorPose3 with Jacobian-derived initial covariance, flushed into C5's iSAM2 graph via the widened ISam2GraphHandle.update(graph, values, None) (Option B). Posterior covariance from compute_marginals().marginalCovariance(pose_key) with SPD-defensive Cholesky check. Tile pixel -> ENU world conversion via the shared WgsConverter + a configurable tile_size_px. Two spec deviations now documented in the AZ-358 task file: PriorFactorPose3 over GenericProjectionFactorCal3DS2 (avoids unbounded landmark variables; same Fisher information on the pose marginal) and explicit (graph, values, timestamps) update args (aligns with C5's impl). AZ-361 — Jacobian + thermal hybrid: per-frame dispatch on thermal_state.thermal_throttle_active selects the cv2.projectPoints- derived 6x6 information matrix (with ridge regularisation) as the emitted covariance. Skips the iSAM2 factor add under throttle (Invariant 12). Emits CovarianceDegradedWarning via warnings.warn (never raised); paired WARN log + FDR record rate-limited per covariance_degraded_warn_window_ns (default 60 s) via an injected monotonic Clock. Supersedes the AZ-358 NotImplementedError stub. Widens ISam2GraphHandle from get_pose_key only to all five C4-facing methods (add_factor, update, compute_marginals, last_anchor_age_ms); C5's existing ISam2GraphHandleImpl already satisfies the superset, so no C5 source change this batch. Threads fdr_client + clock through pose_factory composition. Registers two new FDR payload kinds: pose.frame_done (per-call telemetry; both success and PnpFailureError paths) and pose.covariance_degraded (per-window throttle exposure). Tests: 21 new (AZ-358 AC-1..11 + AZ-361 AC-1..10/12/13; AZ-361 AC-11 RMSE-ratio informational per spec, not asserted). Updates 2 existing test files for Protocol widening and the FDR-schema round trip. Code review verdict: PASS_WITH_WARNINGS (5 findings: Medium x2, Low x3; none blocking). Full suite: 1958 passed, 1 unrelated host-dependent perf failure (c12 CLI cold-start, pre-existing). Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
@@ -465,6 +465,41 @@ KNOWN_PAYLOAD_KEYS: Final[dict[str, frozenset[str]]] = {
|
||||
"error",
|
||||
}
|
||||
),
|
||||
# AZ-358 / AZ-361 / E-C4: emitted by ``OpenCVGtsamPoseEstimator``
|
||||
# on every ``estimate(...)`` call (both success and PnpFailureError
|
||||
# paths). ``mode`` is the path actually taken — ``"marginals"`` on
|
||||
# the production path, ``"jacobian"`` on the AZ-361 thermal-throttle
|
||||
# fallback; readers correlate against the FDR rolling cursor to
|
||||
# bin AC-1.3 thermal-throttle exposure. ``error`` is True ONLY when
|
||||
# the call ended in ``PnpFailureError``; default-absent otherwise.
|
||||
# ``position_wgs84`` is a 3-tuple ``[lat_deg, lon_deg, alt_m]`` so
|
||||
# the orjson serialiser stays on plain lists (no DTO leakage).
|
||||
"pose.frame_done": frozenset(
|
||||
{
|
||||
"frame_id",
|
||||
"inliers",
|
||||
"residual_px",
|
||||
"mode",
|
||||
"covariance_norm",
|
||||
"position_wgs84",
|
||||
"error",
|
||||
}
|
||||
),
|
||||
# AZ-361 / E-C4: emitted at MOST once per 60 s window (rolling)
|
||||
# whenever the AZ-361 Jacobian path engages. Provides the
|
||||
# post-flight FDR forensics needed to bin AC-NEW-5 thermal-throttle
|
||||
# exposure without per-frame log spam. ``thermal_throttle_active``
|
||||
# echoes the input flag for symmetry with the WARN log that
|
||||
# accompanies the same record. ``window_start_ns`` is the
|
||||
# monotonic_ns reading the rate limiter used to anchor the
|
||||
# current 60 s window so consumers can verify the rate limit.
|
||||
"pose.covariance_degraded": frozenset(
|
||||
{
|
||||
"frame_id",
|
||||
"thermal_throttle_active",
|
||||
"window_start_ns",
|
||||
}
|
||||
),
|
||||
}
|
||||
|
||||
KNOWN_KINDS: Final[frozenset[str]] = frozenset(KNOWN_PAYLOAD_KEYS.keys())
|
||||
|
||||
Reference in New Issue
Block a user