[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:
Oleksandr Bezdieniezhnykh
2026-05-14 05:01:14 +03:00
parent 360aece7a6
commit 4eac24f37a
13 changed files with 2452 additions and 35 deletions
@@ -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())