azaion/gps-denied-onboard
1
0
Fork 0
mirror of https://github.com/azaion/gps-denied-onboard.git synced 2026-06-22 16:41:13 +00:00
Code Issues Packages Projects Releases Wiki Activity

[AZ-358] [AZ-361] C4 OpenCVGtsamPoseEstimator + Jacobian thermal hybrid

Browse Source 
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
Download Patch File Download Diff File Expand all files Collapse all files
src/gps_denied_onboard/components/c4_pose/_isam2_handle.py
+73 -9
View File
@@ -1,16 +1,34 @@
"""C4 ``ISam2GraphHandle`` Protocol stub (AZ-355).
"""C4 ``ISam2GraphHandle`` Protocol stub (AZ-355; extended by AZ-358).
The C5 iSAM2 graph is the shared GTSAM substrate (ADR-003). C4
NEVER owns the graph — it receives a typed handle from the runtime
root and uses ONLY the minimum surface it needs to attach pose
factors against C5's key namespace.
Per the C4 contract this Protocol exposes ONLY
``get_pose_key(frame_id) -> int`` — the C5 concrete handle
(``components.c5_state._isam2_handle.ISam2GraphHandleImpl``) is
strictly a superset of this surface, so a duck-typed satisfaction
check via ``isinstance(handle, ISam2GraphHandle)`` succeeds without
C4 importing C5 internals.
AZ-355 originally shipped a single-method stub (``get_pose_key``).
AZ-358 widens the surface to five methods so the production
:class:`OpenCVGtsamPoseEstimator` can dispatch its per-frame factor
add + covariance recovery without importing C5 internals:
* :meth:`get_pose_key` — map ``frame_id`` to a GTSAM ``Key``.
* :meth:`add_factor` — staged-buffer factor append. Kept on the
Protocol for symmetry with the C5-side superset; C4 in Option B
(per the AZ-358 batch-58 design discussion) does NOT call this
itself — C4 builds a local ``NonlinearFactorGraph`` and passes it
to :meth:`update` directly.
* :meth:`update` — apply a per-call (graph, values, timestamps)
diff to iSAM2. The canonical C4 entry point in Option B.
* :meth:`compute_marginals` — return a ``gtsam.Marginals`` snapshot
for posterior covariance recovery.
* :meth:`last_anchor_age_ms` — milliseconds since C5 last accepted
a satellite-anchored pose; broadcast to C4 so the
:attr:`PoseEstimate.last_satellite_anchor_age_ms` field reflects
the C5-owned freshness counter without C4 computing it.
The C5-side ``components.c5_state._isam2_handle.ISam2GraphHandleImpl``
exposes the SAME five methods (and is a strict superset — it adds
no extra C4-facing surface), so any production instance satisfies
both the C4 and the C5 Protocol declarations without an adapter.
Risk-1 mitigation per the AZ-355 task spec: if C5's graph design
grows, this stub grows ONLY if C4's needs grow. Otherwise the two
@@ -19,14 +37,21 @@ Protocols diverge cleanly along the producer/consumer split.
from __future__ import annotations
from typing import Protocol, runtime_checkable
from typing import Any, Protocol, runtime_checkable
__all__ = ["ISam2GraphHandle"]
@runtime_checkable
class ISam2GraphHandle(Protocol):
"""Read-only view of C5's iSAM2 graph for C4's pose-factor adds."""
"""C4 ↔ C5 seam over the shared iSAM2 graph (ADR-003).
Owned by C5; held by reference inside ``OpenCVGtsamPoseEstimator``.
The handle is passed during composition (``state_factory``
returns the tuple; ``pose_factory`` accepts it as a positional
argument) and never crosses thread boundaries — see Invariant 1
of both the C4 and C5 contracts.
"""
def get_pose_key(self, frame_id: int) -> int:
"""Map a C4 frame_id to the corresponding GTSAM pose key.
@@ -35,3 +60,42 @@ class ISam2GraphHandle(Protocol):
``gtsam.symbol('x', frame_id)``); C4 only needs the integer
key to construct a ``PriorFactorPose3``.
"""
def add_factor(self, factor: Any) -> None:
"""Append ``factor`` to the C5-owned staging graph.
C4 in Option B does NOT call this — see the module docstring.
Kept on the Protocol for the producer/consumer symmetry
invariant with the C5-side superset Protocol.
"""
def update(self, graph: Any, values: Any, timestamps: Any | None = None) -> None:
"""Apply a per-call ``(graph, values, timestamps)`` diff to iSAM2.
C4's canonical Option B entry point: builds a local
``NonlinearFactorGraph`` carrying its single
``PriorFactorPose3``, inserts the initial pose ``Pose3``
into a local ``Values`` (skip-if-exists, defensively),
passes both to this method with ``timestamps=None``.
Implementations MAY treat ``timestamps=None`` as "use the
C5-side default" (e.g. an empty ``FixedLagSmootherKeyTimestampMap``).
"""
def compute_marginals(self) -> Any:
"""Return a ``gtsam.Marginals`` snapshot of the current iSAM2 state.
C4 calls ``marginals.marginalCovariance(pose_key)`` against
the returned object. The C5-side impl builds the Marginals
on demand; do NOT cache the returned object across frames
— the graph state evolves between calls.
"""
def last_anchor_age_ms(self) -> int:
"""Return ms since C5 last committed a satellite-anchored pose.
Sourced from C5's ``_last_anchor_ns`` watermark. C4 emits
the value as :attr:`PoseEstimate.last_satellite_anchor_age_ms`
(Invariant 8 of the C4 contract — C4 NEVER computes this
metric independently).
"""
src/gps_denied_onboard/components/c4_pose/config.py
+35
View File
@@ -36,12 +36,33 @@ class C4PoseConfig:
* ``thermal_throttle_threshold_celsius`` — informational only;
the actual ``ThermalState.thermal_throttle_active`` decision is owned by C7
(AZ-302). Default 75.0 °C.
* ``covariance_degraded_warn_window_ns`` — AZ-361 rate-limit
window for ``CovarianceDegradedWarning`` emissions AND the
paired ``c4.pose.covariance_degraded`` WARN log + FDR record.
Default 60 s. Set to 0 to disable rate-limiting (useful in
tests that want to see every warning).
* ``ridge_regularisation_epsilon`` — AZ-361 ridge added to
``JᵀJ/σ²`` before inversion to stabilise near-singular
Jacobians on degenerate inlier sets. Default 1e-9; raise
to 1e-6 if Jacobian-path SPD failures begin spiking in
forensics.
* ``tile_size_px`` — AZ-358 satellite-tile pixel dimensions
(square). Used to map ``MatchResult`` inlier tile-pixel
coordinates back to WGS84 lat/lon → local-ENU world points
consumed by ``solvePnPRansac``. Default 256 matches the OSM /
C6 tile-cache convention. If the upstream tile source
provides a different square size, override at composition
time; the spec assumes a square tile (any non-square tile
handling would land in a future config extension).
"""
strategy: str = "opencv_gtsam"
ransac_iterations: int = 200
ransac_reprojection_threshold_px: float = 4.0
thermal_throttle_threshold_celsius: float = 75.0
covariance_degraded_warn_window_ns: int = 60_000_000_000
ridge_regularisation_epsilon: float = 1e-9
tile_size_px: int = 256
def __post_init__(self) -> None:
if self.strategy not in KNOWN_POSE_STRATEGIES:
@@ -62,3 +83,17 @@ class C4PoseConfig:
"C4PoseConfig.thermal_throttle_threshold_celsius must be > 0; "
f"got {self.thermal_throttle_threshold_celsius}"
)
if self.covariance_degraded_warn_window_ns < 0:
raise ConfigError(
"C4PoseConfig.covariance_degraded_warn_window_ns must be >= 0; "
f"got {self.covariance_degraded_warn_window_ns}"
)
if self.ridge_regularisation_epsilon <= 0.0:
raise ConfigError(
"C4PoseConfig.ridge_regularisation_epsilon must be > 0; "
f"got {self.ridge_regularisation_epsilon}"
)
if self.tile_size_px <= 0:
raise ConfigError(
f"C4PoseConfig.tile_size_px must be > 0; got {self.tile_size_px}"
)
src/gps_denied_onboard/components/c4_pose/opencv_gtsam_estimator.py
+972
View File
@@ -0,0 +1,972 @@
"""Production-default OpenCV+GTSAM ``PoseEstimator`` (AZ-358 + AZ-361).
Implements the single concrete ``PoseEstimator`` strategy registered
under ``opencv_gtsam``. Two per-frame execution paths share PnP +
WGS84 conversion + FDR emission but diverge on covariance recovery:
* :meth:`_estimate_marginals_path` (AZ-358) — production default.
After PnP, build a single ``PriorFactorPose3`` with a Jacobian-
derived initial 6x6 covariance, flush it into C5's iSAM2 graph
via ``handle.update(graph, values, None)``, then read the
*posterior* 6x6 covariance via ``handle.compute_marginals()``.
The factor add is a per-call diff (Option B per the AZ-358
batch-58 design discussion) — C4 builds a local
``NonlinearFactorGraph`` + ``Values`` and passes them to update,
leaving the C5-internal staging buffer untouched.
* :meth:`_estimate_jacobian_path` (AZ-361) — thermal-throttle
fallback. After PnP, compute a 6x6 covariance from
``cv2.projectPoints``-derived Jacobians + reprojection-residual
variance, with a ridge regulariser for numerical stability.
Skips the iSAM2 factor add entirely (Invariant 3 of the
hybrid task — under throttle, the graph stops growing).
Emits ``CovarianceDegradedWarning`` via ``warnings.warn`` (NOT
raise) once per ``covariance_degraded_warn_window_ns`` window;
the paired WARN log + FDR record are rate-limited on the same
cadence.
Implementation deviations from the original task wording (tracked
in the batch-58 implementation report):
1. The task spec named ``GenericProjectionFactorCal3DS2`` for the
factor add. We use ``PriorFactorPose3`` instead — it is
mathematically equivalent on the pose marginal in expectation
(same Fisher information), avoids unbounded landmark variable
growth in iSAM2, and requires no tile-pixel-to-3D-world-coord
georef infrastructure beyond what ``WgsConverter`` already
ships.
2. The task spec implied ``isam2_graph_handle.update()`` with no
args. The C5-side ``ISam2GraphHandleImpl`` requires explicit
``(graph, values, timestamps)`` arguments (Option B). C4
accordingly builds the per-call diff itself.
3. The task spec implied calibration-supplied georef. The actual
``CameraCalibration`` DTO does not carry tile metadata; we
derive 3D world points from ``MatchResult.per_candidate[best].tile_id``
+ ``WgsConverter.tile_xy_to_latlon_bounds`` + a configurable
``tile_size_px`` (default 256). The first-frame tile centre
doubles as the local-ENU origin until the composition root
overrides it via :meth:`set_enu_origin`.
"""
from __future__ import annotations
import logging
import warnings
from datetime import datetime, timezone
from typing import TYPE_CHECKING, Any, Final
from uuid import uuid4
import cv2
import gtsam
import numpy as np
from gps_denied_onboard._types.geo import LatLonAlt
from gps_denied_onboard._types.pose import (
CovarianceMode,
PoseEstimate,
PoseSourceLabel,
Quat,
)
from gps_denied_onboard.clock.wall_clock import WallClock
from gps_denied_onboard.components.c4_pose._isam2_handle import ISam2GraphHandle
from gps_denied_onboard.components.c4_pose.config import C4PoseConfig
from gps_denied_onboard.components.c4_pose.errors import (
CovarianceDegradedWarning,
PnpFailureError,
PoseEstimatorConfigError,
)
from gps_denied_onboard.components.c4_pose.interface import PoseEstimator
from gps_denied_onboard.fdr_client.records import FdrRecord
from gps_denied_onboard.helpers.wgs_converter import WgsConverter
from gps_denied_onboard.logging import get_logger
if TYPE_CHECKING:
from gps_denied_onboard._types.calibration import CameraCalibration
from gps_denied_onboard._types.matcher import MatchResult
from gps_denied_onboard._types.thermal import ThermalState
from gps_denied_onboard.clock import Clock
from gps_denied_onboard.config.schema import Config
__all__ = ["OpenCVGtsamPoseEstimator", "create", "register"]
_STRATEGY: Final[str] = "opencv_gtsam"
_PRODUCER_ID: Final[str] = "c4_pose"
_FDR_KIND_FRAME_DONE: Final[str] = "pose.frame_done"
_FDR_KIND_COV_DEGRADED: Final[str] = "pose.covariance_degraded"
_LOG_KIND_FRAME_DONE: Final[str] = "c4.pose.frame_done"
_LOG_KIND_READY: Final[str] = "c4.pose.ready"
_LOG_KIND_COV_DEGRADED: Final[str] = "c4.pose.covariance_degraded"
_LOG_KIND_PNP_FAILURE: Final[str] = "c4.pose.pnp_failure"
# Default ENU origin used until the composition root or the first
# successful frame supplies a real one. Matches the C5 estimator's
# equator/prime-meridian default so cross-component tests don't
# diverge.
_DEFAULT_ENU_ORIGIN: Final[LatLonAlt] = LatLonAlt(lat_deg=0.0, lon_deg=0.0, alt_m=0.0)
class OpenCVGtsamPoseEstimator(PoseEstimator):
"""OpenCV ``solvePnPRansac`` + GTSAM ``Marginals`` pose estimator.
Single concrete implementation behind the
:class:`PoseEstimator` Protocol. Per-frame thermal-throttle
dispatch selects between the production Marginals path and the
AZ-361 Jacobian fallback; both paths share the PnP front-end and
WGS84 / FDR back-end.
Constructor dependencies:
* ``config`` — top-level :class:`Config`; the ``c4_pose`` block
drives RANSAC settings + rate-limit window + ridge epsilon.
* ``ransac_filter`` — shared :class:`RansacFilter` for
consistency with C3 / C3.5; held by reference but not used
directly on the steady-state pose path (the inliers reach C4
already filtered).
* ``wgs_converter`` — shared :class:`WgsConverter` (one
instance across the runtime).
* ``se3_utils`` — shared SE(3) helpers; constructor-injected for
symmetry with the other strategies.
* ``isam2_graph_handle`` — typed handle into C5's iSAM2 graph
(ADR-003 shared substrate). The estimator NEVER imports C5
internals — every graph touch goes through this handle.
* ``fdr_client`` — optional :class:`FdrClient`; ``None`` causes
every FDR enqueue to silently no-op (composition tests that
do not wire C13 should pass ``None``).
* ``clock`` — optional :class:`Clock` for rate-limiting the
AZ-361 ``CovarianceDegradedWarning`` cadence; defaults to
:class:`WallClock`.
"""
def __init__(
self,
config: Config,
*,
ransac_filter: Any,
wgs_converter: Any,
se3_utils: Any,
isam2_graph_handle: ISam2GraphHandle,
fdr_client: Any | None = None,
clock: Clock | None = None,
logger: logging.Logger | None = None,
) -> None:
block = self._extract_block(config)
self._config = config
self._block = block
self._ransac_filter = ransac_filter
self._wgs_converter = wgs_converter
self._se3_utils = se3_utils
self._handle = isam2_graph_handle
self._fdr_client = fdr_client
self._clock: Clock = clock if clock is not None else WallClock()
self._log = logger or get_logger("c4_pose.opencv_gtsam")
self._last_covariance_mode: CovarianceMode = CovarianceMode.MARGINALS
# AZ-361 rolling-window rate-limit watermark. Stores the
# monotonic_ns of the LAST emission; ``0`` means "never
# emitted". A window of 0 disables rate-limiting.
self._last_cov_degraded_emit_ns: int = 0
# ENU origin used by ``solvePnPRansac`` world-point
# conversion + by :meth:`_pose_to_wgs84`. Set lazily to the
# first frame's tile centre; the composition root MAY
# pre-seed via :meth:`set_enu_origin`.
self._enu_origin: LatLonAlt | None = None
self._log.info(
_LOG_KIND_READY,
extra={
"kind": _LOG_KIND_READY,
"kv": {
"strategy": _STRATEGY,
"default_covariance": CovarianceMode.MARGINALS.value,
"ransac_iterations": block.ransac_iterations,
"ransac_reprojection_threshold_px": block.ransac_reprojection_threshold_px,
"covariance_degraded_warn_window_ns": (
block.covariance_degraded_warn_window_ns
),
"ridge_regularisation_epsilon": block.ridge_regularisation_epsilon,
"tile_size_px": block.tile_size_px,
},
},
)
@staticmethod
def _extract_block(config: Config) -> C4PoseConfig:
components = getattr(config, "components", None) or {}
block = components.get("c4_pose") if isinstance(components, dict) else None
if block is None:
return C4PoseConfig()
if isinstance(block, C4PoseConfig):
return block
raise PoseEstimatorConfigError(
f"config.components['c4_pose'] must be a C4PoseConfig; "
f"got {type(block).__name__}"
)
# ------------------------------------------------------------------
# Public composition hooks.
def set_enu_origin(self, origin: LatLonAlt) -> None:
"""Set the local-ENU origin used for WGS84 conversion.
SHOULD be called once by the composition root after C5
determines its anchor origin. When omitted, C4 lazily
adopts the first frame's tile centre.
"""
self._enu_origin = origin
# ------------------------------------------------------------------
# PoseEstimator Protocol.
def estimate(
self,
match_result: MatchResult,
calibration: CameraCalibration,
thermal_state: ThermalState,
) -> PoseEstimate:
"""Run PnP → factor add (steady state) → posterior covariance.
Per-frame dispatch on ``thermal_state.thermal_throttle_active``:
``True`` → :meth:`_estimate_jacobian_path`; ``False`` →
:meth:`_estimate_marginals_path`. Invariant 4: the decision
is made on EVERY call independently — no hysteresis.
"""
if thermal_state.thermal_throttle_active:
return self._estimate_jacobian_path(match_result, calibration, thermal_state)
return self._estimate_marginals_path(match_result, calibration, thermal_state)
def current_covariance_mode(self) -> CovarianceMode:
return self._last_covariance_mode
# ------------------------------------------------------------------
# Marginals path (AZ-358 production default).
def _estimate_marginals_path(
self,
match_result: MatchResult,
calibration: CameraCalibration,
thermal_state: ThermalState,
) -> PoseEstimate:
try:
pnp = self._run_pnp(match_result, calibration)
except PnpFailureError as exc:
self._emit_pose_frame_done_fdr_error(
match_result.frame_id, CovarianceMode.MARGINALS
)
self._log.error(
_LOG_KIND_PNP_FAILURE,
extra={
"kind": _LOG_KIND_PNP_FAILURE,
"kv": {
"frame_id": match_result.frame_id,
"mode": CovarianceMode.MARGINALS.value,
"stage": "pnp",
"error": str(exc),
},
},
)
raise
pose_key = self._handle.get_pose_key(match_result.frame_id)
gtsam_pose = self._pose_matrix_to_gtsam(pnp.pose_T_world_cam)
initial_cov_6x6 = self._jacobian_covariance(
pnp.pose_T_world_cam,
pnp.image_pts,
pnp.world_pts,
pnp.residuals,
calibration,
)
noise = gtsam.noiseModel.Gaussian.Covariance(initial_cov_6x6)
factor = gtsam.PriorFactorPose3(pose_key, gtsam_pose, noise)
local_graph = gtsam.NonlinearFactorGraph()
local_graph.add(factor)
local_values = gtsam.Values()
try:
local_values.insert(pose_key, gtsam_pose)
except Exception:
# AC-7 caveat — the pose key may already carry a value
# initialised by a prior VIO/IMU keyframe insert. iSAM2
# raises on duplicate insert; we swallow it because the
# prior factor still applies the same constraint.
pass
try:
self._handle.update(local_graph, local_values, None)
marginals = self._handle.compute_marginals()
posterior_cov_6x6 = np.asarray(
marginals.marginalCovariance(pose_key), dtype=np.float64
)
except Exception as exc:
self._emit_pose_frame_done_fdr_error(
match_result.frame_id, CovarianceMode.MARGINALS
)
self._log.error(
_LOG_KIND_PNP_FAILURE,
extra={
"kind": _LOG_KIND_PNP_FAILURE,
"kv": {
"frame_id": match_result.frame_id,
"mode": CovarianceMode.MARGINALS.value,
"stage": "marginals",
"error": str(exc),
},
},
)
raise PnpFailureError(
f"marginals recovery failed for frame {match_result.frame_id}: {exc}"
) from exc
posterior_cov_6x6 = _symmetrise(posterior_cov_6x6)
try:
np.linalg.cholesky(posterior_cov_6x6)
except np.linalg.LinAlgError as exc:
self._emit_pose_frame_done_fdr_error(
match_result.frame_id, CovarianceMode.MARGINALS
)
self._log.error(
_LOG_KIND_PNP_FAILURE,
extra={
"kind": _LOG_KIND_PNP_FAILURE,
"kv": {
"frame_id": match_result.frame_id,
"mode": CovarianceMode.MARGINALS.value,
"stage": "spd_check",
"error": str(exc),
},
},
)
raise PnpFailureError(
"non-SPD covariance from Marginals; numerical instability"
) from exc
pose_estimate = self._assemble_pose_estimate(
pose_T_world_cam=pnp.pose_T_world_cam,
covariance_6x6=posterior_cov_6x6,
mode=CovarianceMode.MARGINALS,
)
self._last_covariance_mode = CovarianceMode.MARGINALS
self._emit_pose_frame_done_fdr(
frame_id=pose_estimate.frame_id,
inliers=pnp.inlier_count,
residual_px=pnp.median_residual_px,
mode=CovarianceMode.MARGINALS,
covariance_norm=float(np.linalg.norm(posterior_cov_6x6, ord="fro")),
position_wgs84=pose_estimate.position_wgs84,
)
self._log.debug(
_LOG_KIND_FRAME_DONE,
extra={
"kind": _LOG_KIND_FRAME_DONE,
"kv": {
"frame_id": match_result.frame_id,
"inliers": pnp.inlier_count,
"residual_px": pnp.median_residual_px,
"mode": CovarianceMode.MARGINALS.value,
"covariance_norm": float(
np.linalg.norm(posterior_cov_6x6, ord="fro")
),
},
},
)
return pose_estimate
# ------------------------------------------------------------------
# Jacobian path (AZ-361 thermal-throttle fallback).
def _estimate_jacobian_path(
self,
match_result: MatchResult,
calibration: CameraCalibration,
thermal_state: ThermalState,
) -> PoseEstimate:
try:
pnp = self._run_pnp(match_result, calibration)
except PnpFailureError as exc:
self._emit_pose_frame_done_fdr_error(
match_result.frame_id, CovarianceMode.JACOBIAN
)
self._log.error(
_LOG_KIND_PNP_FAILURE,
extra={
"kind": _LOG_KIND_PNP_FAILURE,
"kv": {
"frame_id": match_result.frame_id,
"mode": CovarianceMode.JACOBIAN.value,
"stage": "pnp",
"error": str(exc),
},
},
)
raise
cov_6x6 = self._jacobian_covariance(
pnp.pose_T_world_cam,
pnp.image_pts,
pnp.world_pts,
pnp.residuals,
calibration,
)
cov_6x6 = _symmetrise(cov_6x6)
try:
np.linalg.cholesky(cov_6x6)
except np.linalg.LinAlgError as exc:
self._emit_pose_frame_done_fdr_error(
match_result.frame_id, CovarianceMode.JACOBIAN
)
self._log.error(
_LOG_KIND_PNP_FAILURE,
extra={
"kind": _LOG_KIND_PNP_FAILURE,
"kv": {
"frame_id": match_result.frame_id,
"mode": CovarianceMode.JACOBIAN.value,
"stage": "spd_check",
"error": str(exc),
},
},
)
raise PnpFailureError(
"non-SPD Jacobian covariance; numerical instability"
) from exc
pose_estimate = self._assemble_pose_estimate(
pose_T_world_cam=pnp.pose_T_world_cam,
covariance_6x6=cov_6x6,
mode=CovarianceMode.JACOBIAN,
)
self._last_covariance_mode = CovarianceMode.JACOBIAN
self._emit_pose_frame_done_fdr(
frame_id=pose_estimate.frame_id,
inliers=pnp.inlier_count,
residual_px=pnp.median_residual_px,
mode=CovarianceMode.JACOBIAN,
covariance_norm=float(np.linalg.norm(cov_6x6, ord="fro")),
position_wgs84=pose_estimate.position_wgs84,
)
self._maybe_emit_covariance_degraded(pose_estimate.frame_id, thermal_state)
self._log.debug(
_LOG_KIND_FRAME_DONE,
extra={
"kind": _LOG_KIND_FRAME_DONE,
"kv": {
"frame_id": match_result.frame_id,
"inliers": pnp.inlier_count,
"residual_px": pnp.median_residual_px,
"mode": CovarianceMode.JACOBIAN.value,
"covariance_norm": float(np.linalg.norm(cov_6x6, ord="fro")),
},
},
)
return pose_estimate
# ------------------------------------------------------------------
# PnP front-end shared by both paths.
def _run_pnp(
self,
match_result: MatchResult,
calibration: CameraCalibration,
) -> _PnpResult:
"""Run ``cv2.solvePnPRansac`` against the best-candidate inliers.
Raises:
PnpFailureError: degenerate geometry, RANSAC convergence
failure, OR insufficient inliers (< 4 — IPPE needs
at least 4 non-collinear points).
"""
if not match_result.per_candidate:
raise PnpFailureError(
f"PnP no-input: frame={match_result.frame_id} has empty per_candidate"
)
best = match_result.per_candidate[match_result.best_candidate_idx]
inliers = np.asarray(best.inlier_correspondences, dtype=np.float64)
if inliers.ndim != 2 or inliers.shape[1] != 4:
raise PnpFailureError(
f"PnP shape error: frame={match_result.frame_id} "
f"inlier_correspondences has shape {inliers.shape}; expected (N, 4)"
)
if inliers.shape[0] < 4:
raise PnpFailureError(
f"PnP convergence failure: frame={match_result.frame_id} "
f"has {inliers.shape[0]} inliers; IPPE requires >= 4"
)
image_pts = inliers[:, :2].astype(np.float64, copy=False)
world_pts = self._tile_pixels_to_enu_world(
tile_pixels=inliers[:, 2:].astype(np.float64, copy=False),
tile_id=best.tile_id,
)
K = np.asarray(calibration.intrinsics_3x3, dtype=np.float64)
dist = np.asarray(calibration.distortion, dtype=np.float64).reshape(-1)
try:
success, rvec, tvec, inlier_idx = cv2.solvePnPRansac(
objectPoints=world_pts.reshape(-1, 1, 3),
imagePoints=image_pts.reshape(-1, 1, 2),
cameraMatrix=K,
distCoeffs=dist,
flags=cv2.SOLVEPNP_IPPE,
iterationsCount=int(self._block.ransac_iterations),
reprojectionError=float(self._block.ransac_reprojection_threshold_px),
)
except cv2.error as exc:
raise PnpFailureError(
f"PnP convergence failure: frame={match_result.frame_id} "
f"OpenCV solvePnPRansac raised: {exc}"
) from exc
if not success or inlier_idx is None or len(inlier_idx) < 4:
raise PnpFailureError(
f"PnP convergence failure: frame={match_result.frame_id} "
f"success={success!r} inlier_idx={len(inlier_idx) if inlier_idx is not None else None}"
)
# Reduce to the RANSAC inlier subset for the Jacobian path
# AND for residual computation. The match_result inliers
# were filtered at the homography stage; ``solvePnPRansac``
# refines further.
idx_flat = np.asarray(inlier_idx, dtype=np.int64).reshape(-1)
image_pts_inliers = image_pts[idx_flat]
world_pts_inliers = world_pts[idx_flat]
pose_T_world_cam = _rvec_tvec_to_pose_matrix(rvec, tvec)
residuals = _compute_reprojection_residuals(
world_pts_inliers, image_pts_inliers, rvec, tvec, K, dist
)
return _PnpResult(
pose_T_world_cam=pose_T_world_cam,
image_pts=image_pts_inliers,
world_pts=world_pts_inliers,
residuals=residuals,
inlier_count=int(idx_flat.size),
median_residual_px=(
float(np.median(residuals)) if residuals.size else 0.0
),
)
# ------------------------------------------------------------------
# Jacobian-derived covariance (shared by both paths).
def _jacobian_covariance(
self,
pose_T_world_cam: np.ndarray,
image_pts: np.ndarray,
world_pts: np.ndarray,
residuals: np.ndarray,
calibration: CameraCalibration,
) -> np.ndarray:
"""Compute a 6x6 covariance from PnP residuals + Jacobian.
The Jacobian comes from ``cv2.projectPoints`` (analytical
for the IPPE pose at the converged solution); the residual
variance is the isotropic-noise scalar ``mean(residuals^2)``
as documented in ADR-006. ``Σ = (JᵀJ/σ² + ε·I)^{-1}``.
"""
K = np.asarray(calibration.intrinsics_3x3, dtype=np.float64)
dist = np.asarray(calibration.distortion, dtype=np.float64).reshape(-1)
rvec, _ = cv2.Rodrigues(pose_T_world_cam[:3, :3])
tvec = pose_T_world_cam[:3, 3].reshape(3, 1)
try:
_projected, jacobian = cv2.projectPoints(
objectPoints=world_pts.reshape(-1, 1, 3),
rvec=rvec,
tvec=tvec,
cameraMatrix=K,
distCoeffs=dist,
)
except cv2.error as exc:
raise PnpFailureError(
f"Jacobian computation failed: cv2.projectPoints raised: {exc}"
) from exc
# cv2.projectPoints returns the Jacobian in the layout
# documented in the OpenCV wiki: 2N rows × ≥ 6 cols, where
# the first 3 cols are d(uv)/d(rvec) and the next 3 cols
# are d(uv)/d(tvec). Cols beyond 6 are intrinsics / dist
# partials we ignore — pose-only Jacobian here.
jacobian = np.asarray(jacobian, dtype=np.float64)
if jacobian.ndim != 2 or jacobian.shape[0] != 2 * world_pts.shape[0] or jacobian.shape[1] < 6:
raise PnpFailureError(
f"Jacobian shape unexpected: {jacobian.shape}; "
f"expected (2N, >=6) where N={world_pts.shape[0]}"
)
J_pose = jacobian[:, :6]
sigma_sq = float(np.mean(residuals * residuals)) if residuals.size else 1.0
if not np.isfinite(sigma_sq) or sigma_sq <= 0.0:
# Defensive: zero residuals on a noise-free synthetic
# fixture would produce a singular information matrix.
# Fall back to a 1-pixel isotropic prior so the
# Cholesky check downstream still has a chance.
sigma_sq = 1.0
info = (J_pose.T @ J_pose) / sigma_sq
info += self._block.ridge_regularisation_epsilon * np.eye(6, dtype=np.float64)
cov = np.linalg.inv(info)
return cov
# ------------------------------------------------------------------
# Tile-pixel-to-ENU-world-point conversion.
def _tile_pixels_to_enu_world(
self,
tile_pixels: np.ndarray,
tile_id: tuple[int, float, float],
) -> np.ndarray:
"""Map ``(I, 2)`` tile pixel coords to ``(I, 3)`` ENU world points.
Walks ``WgsConverter.latlon_to_tile_xy`` → ``tile_xy_to_latlon_bounds``
to get the tile's WGS84 bounding box, bilinearly interpolates
each pixel inside it (Web-Mercator pixel (0, 0) = tile NW
corner), then projects ``(lat, lon, 0)`` into the local ENU
frame anchored at :attr:`_enu_origin`. Lazily seeds the origin
to the tile centre on the first call.
"""
zoom, lat_center, lon_center = tile_id
tile_x, tile_y = WgsConverter.latlon_to_tile_xy(zoom, lat_center, lon_center)
bounds = WgsConverter.tile_xy_to_latlon_bounds(zoom, tile_x, tile_y)
tile_size_px = float(self._block.tile_size_px)
if self._enu_origin is None:
self._enu_origin = LatLonAlt(
lat_deg=lat_center, lon_deg=lon_center, alt_m=0.0
)
origin = self._enu_origin
px = tile_pixels[:, 0]
py = tile_pixels[:, 1]
lons = bounds.min_lon_deg + (px / tile_size_px) * (
bounds.max_lon_deg - bounds.min_lon_deg
)
lats = bounds.max_lat_deg - (py / tile_size_px) * (
bounds.max_lat_deg - bounds.min_lat_deg
)
world = np.empty((tile_pixels.shape[0], 3), dtype=np.float64)
for i in range(tile_pixels.shape[0]):
world[i] = WgsConverter.latlonalt_to_local_enu(
origin,
LatLonAlt(lat_deg=float(lats[i]), lon_deg=float(lons[i]), alt_m=0.0),
)
return world
# ------------------------------------------------------------------
# Pose-matrix assembly.
def _assemble_pose_estimate(
self,
*,
pose_T_world_cam: np.ndarray,
covariance_6x6: np.ndarray,
mode: CovarianceMode,
) -> PoseEstimate:
position_wgs84 = self._enu_translation_to_wgs84(pose_T_world_cam[:3, 3])
orientation = _rotation_matrix_to_quat(pose_T_world_cam[:3, :3])
try:
last_anchor_age_ms = int(self._handle.last_anchor_age_ms())
except Exception:
# The handle MAY raise on early-flight reads; per
# Invariant 8 C4 passes the value through and never
# computes it itself. Default to 0 so the consumer sees
# a fresh-anchor sentinel rather than garbage.
last_anchor_age_ms = 0
return PoseEstimate(
frame_id=uuid4(),
position_wgs84=position_wgs84,
orientation_world_T_body=orientation,
covariance_6x6=covariance_6x6,
covariance_mode=mode,
source_label=PoseSourceLabel.SATELLITE_ANCHORED,
last_satellite_anchor_age_ms=last_anchor_age_ms,
emitted_at=self._clock.monotonic_ns(),
)
def _enu_translation_to_wgs84(self, translation_3: np.ndarray) -> LatLonAlt:
origin = self._enu_origin if self._enu_origin is not None else _DEFAULT_ENU_ORIGIN
translation = np.asarray(translation_3, dtype=np.float64).reshape(3)
return self._wgs_converter.local_enu_to_latlonalt(origin, translation)
@staticmethod
def _pose_matrix_to_gtsam(pose_T_world_cam: np.ndarray) -> gtsam.Pose3:
return gtsam.Pose3(np.ascontiguousarray(pose_T_world_cam, dtype=np.float64))
# ------------------------------------------------------------------
# FDR emission.
def _emit_pose_frame_done_fdr(
self,
*,
frame_id: Any,
inliers: int,
residual_px: float,
mode: CovarianceMode,
covariance_norm: float,
position_wgs84: LatLonAlt,
) -> None:
if self._fdr_client is None:
return
payload: dict[str, Any] = {
"frame_id": str(frame_id),
"inliers": int(inliers),
"residual_px": float(residual_px),
"mode": mode.value,
"covariance_norm": float(covariance_norm),
"position_wgs84": [
float(position_wgs84.lat_deg),
float(position_wgs84.lon_deg),
float(position_wgs84.alt_m),
],
}
self._enqueue_fdr_record(_FDR_KIND_FRAME_DONE, payload)
def _emit_pose_frame_done_fdr_error(
self, frame_id: Any, mode: CovarianceMode
) -> None:
if self._fdr_client is None:
return
payload = {
"frame_id": str(frame_id),
"inliers": 0,
"residual_px": 0.0,
"mode": mode.value,
"covariance_norm": 0.0,
"position_wgs84": [0.0, 0.0, 0.0],
"error": True,
}
self._enqueue_fdr_record(_FDR_KIND_FRAME_DONE, payload)
def _enqueue_fdr_record(self, kind: str, payload: dict[str, Any]) -> None:
record = FdrRecord(
schema_version=1,
ts=datetime.now(tz=timezone.utc).isoformat(),
producer_id=_PRODUCER_ID,
kind=kind,
payload=payload,
)
try:
self._fdr_client.enqueue(record)
except Exception as exc:
self._log.warning(
f"{kind}_fdr_enqueue_failed",
extra={
"kind": f"{kind}_fdr_enqueue_failed",
"kv": {"error": repr(exc)},
},
)
# ------------------------------------------------------------------
# AZ-361 rate-limited covariance-degraded emission.
def _maybe_emit_covariance_degraded(
self,
frame_id: Any,
thermal_state: ThermalState,
) -> None:
"""Emit ``CovarianceDegradedWarning`` + paired WARN log + FDR.
Rate-limited to one emission per
``covariance_degraded_warn_window_ns``. A window of 0
disables rate-limiting (useful in tests). The warning is
emitted via :func:`warnings.warn` (NOT raised) — callers
observe it via :func:`warnings.catch_warnings`.
"""
now_ns = self._clock.monotonic_ns()
window_ns = int(self._block.covariance_degraded_warn_window_ns)
last_ns = self._last_cov_degraded_emit_ns
emit = (
window_ns <= 0
or last_ns == 0
or (now_ns - last_ns) >= window_ns
)
if not emit:
return
self._last_cov_degraded_emit_ns = now_ns
warnings.warn(
CovarianceDegradedWarning(
"Jacobian covariance engaged; thermal_throttle_active=true"
),
stacklevel=2,
)
self._log.warning(
_LOG_KIND_COV_DEGRADED,
extra={
"kind": _LOG_KIND_COV_DEGRADED,
"kv": {
"frame_id": str(frame_id),
"thermal_throttle_active": bool(
thermal_state.thermal_throttle_active
),
"window_start_ns": int(now_ns),
},
},
)
if self._fdr_client is None:
return
self._enqueue_fdr_record(
_FDR_KIND_COV_DEGRADED,
{
"frame_id": str(frame_id),
"thermal_throttle_active": bool(
thermal_state.thermal_throttle_active
),
"window_start_ns": int(now_ns),
},
)
# ----------------------------------------------------------------------
# Module-level helpers.
class _PnpResult:
"""Internal carrier for PnP outputs threaded between path methods."""
__slots__ = (
"pose_T_world_cam",
"image_pts",
"world_pts",
"residuals",
"inlier_count",
"median_residual_px",
)
def __init__(
self,
pose_T_world_cam: np.ndarray,
image_pts: np.ndarray,
world_pts: np.ndarray,
residuals: np.ndarray,
inlier_count: int,
median_residual_px: float,
) -> None:
self.pose_T_world_cam = pose_T_world_cam
self.image_pts = image_pts
self.world_pts = world_pts
self.residuals = residuals
self.inlier_count = inlier_count
self.median_residual_px = median_residual_px
def _symmetrise(cov: np.ndarray) -> np.ndarray:
"""Force-symmetrise a 6x6 numerical covariance via ``(C + Cᵀ) / 2``."""
arr = np.asarray(cov, dtype=np.float64)
return 0.5 * (arr + arr.T)
def _rvec_tvec_to_pose_matrix(rvec: np.ndarray, tvec: np.ndarray) -> np.ndarray:
"""Convert OpenCV ``(rvec, tvec)`` to a 4x4 SE(3) matrix.
The convention follows OpenCV: ``[R | t]`` maps world points
into the camera frame. The returned matrix is consequently
``T_cam_world`` and we invert to ``T_world_cam`` so the caller
can read the camera pose directly.
"""
R, _ = cv2.Rodrigues(rvec)
T_cam_world = np.eye(4, dtype=np.float64)
T_cam_world[:3, :3] = R
T_cam_world[:3, 3] = np.asarray(tvec, dtype=np.float64).reshape(3)
return np.linalg.inv(T_cam_world)
def _compute_reprojection_residuals(
world_pts: np.ndarray,
image_pts: np.ndarray,
rvec: np.ndarray,
tvec: np.ndarray,
K: np.ndarray,
dist: np.ndarray,
) -> np.ndarray:
"""Per-point pixel residual ``||reproject(P_w) - p_img||``."""
if world_pts.size == 0:
return np.zeros((0,), dtype=np.float64)
projected, _ = cv2.projectPoints(
objectPoints=world_pts.reshape(-1, 1, 3),
rvec=rvec,
tvec=tvec,
cameraMatrix=K,
distCoeffs=dist,
)
return np.linalg.norm(
projected.reshape(-1, 2).astype(np.float64) - image_pts, axis=1
)
def _rotation_matrix_to_quat(R: np.ndarray) -> Quat:
"""Convert a 3x3 rotation matrix to a scalar-first unit quaternion."""
arr = np.asarray(R, dtype=np.float64)
trace = float(np.trace(arr))
if trace > 0:
s = (trace + 1.0) ** 0.5 * 2.0
w = 0.25 * s
x = (arr[2, 1] - arr[1, 2]) / s
y = (arr[0, 2] - arr[2, 0]) / s
z = (arr[1, 0] - arr[0, 1]) / s
elif (arr[0, 0] > arr[1, 1]) and (arr[0, 0] > arr[2, 2]):
s = ((1.0 + arr[0, 0] - arr[1, 1] - arr[2, 2]) ** 0.5) * 2.0
w = (arr[2, 1] - arr[1, 2]) / s
x = 0.25 * s
y = (arr[0, 1] + arr[1, 0]) / s
z = (arr[0, 2] + arr[2, 0]) / s
elif arr[1, 1] > arr[2, 2]:
s = ((1.0 + arr[1, 1] - arr[0, 0] - arr[2, 2]) ** 0.5) * 2.0
w = (arr[0, 2] - arr[2, 0]) / s
x = (arr[0, 1] + arr[1, 0]) / s
y = 0.25 * s
z = (arr[1, 2] + arr[2, 1]) / s
else:
s = ((1.0 + arr[2, 2] - arr[0, 0] - arr[1, 1]) ** 0.5) * 2.0
w = (arr[1, 0] - arr[0, 1]) / s
x = (arr[0, 2] + arr[2, 0]) / s
y = (arr[1, 2] + arr[2, 1]) / s
z = 0.25 * s
norm = (w * w + x * x + y * y + z * z) ** 0.5
if norm < 1e-12:
return Quat(w=1.0, x=0.0, y=0.0, z=0.0)
return Quat(w=w / norm, x=x / norm, y=y / norm, z=z / norm)
# ----------------------------------------------------------------------
# Factory.
def create(
config: Config,
*,
ransac_filter: Any,
wgs_converter: Any,
se3_utils: Any,
isam2_graph_handle: ISam2GraphHandle,
fdr_client: Any | None = None,
clock: Clock | None = None,
) -> PoseEstimator:
"""Composition-root factory for ``opencv_gtsam`` strategy."""
return OpenCVGtsamPoseEstimator(
config,
ransac_filter=ransac_filter,
wgs_converter=wgs_converter,
se3_utils=se3_utils,
isam2_graph_handle=isam2_graph_handle,
fdr_client=fdr_client,
clock=clock,
)
def register() -> None:
"""Register :func:`create` under the ``opencv_gtsam`` strategy slug.
Deferred to per-binary bootstrap modules under the
``BUILD_POSE_OPENCV_GTSAM`` flag (ADR-002). Tests that exercise
the factory path call this directly so the registry lookup
succeeds without depending on the lazy-import fallback.
"""
from gps_denied_onboard.runtime_root.pose_factory import (
register_pose_estimator,
)
register_pose_estimator(_STRATEGY, create)