mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-21 22:01: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:
Oleksandr Bezdieniezhnykh
+10
-10
@@ -35,8 +35,8 @@ Without this task, C5 has no source of pose anchors with native 6×6 covariance
|
||||
b. Call `cv2.solvePnPRansac(world_pts, image_pts, K, dist, flags=cv2.SOLVEPNP_IPPE, iterationsCount=config.pose.ransac_iterations, reprojectionError=config.pose.ransac_reprojection_threshold_px)`.
|
||||
c. On RANSAC failure: raise `PnpFailureError(f"PnP convergence failure: frame={match_result.frame_id}")`. ERROR log + FDR record. The exception escapes (per Invariant 9).
|
||||
d. On success: convert `(rvec, tvec)` to a GTSAM `Pose3` via `SE3Utils`.
|
||||
e. Add a `gtsam.GenericProjectionFactorCal3DS2(image_pts, noise_model, pose_key, landmark_keys, calibration_gtsam)` to C5's iSAM2 graph via `isam2_graph_handle.add_factor(...)` (the handle exposes a write API; AZ-260 implements). NOTE: the Protocol stub in AZ-? defines only `get_pose_key`; this task EXTENDS the stub with `add_factor` and `compute_marginals` (or pushes the Protocol extension upstream into the AZ-? Protocol task during co-development).
|
||||
f. Trigger an iSAM2 update via `isam2_graph_handle.update()`.
|
||||
e. Build a single `gtsam.PriorFactorPose3(pose_key, gtsam_pose, noise_model)` constraining the pose key to the PnP-derived pose. The noise model is built from a Jacobian-derived 6×6 covariance computed from the PnP inlier reprojection residuals + `cv2.projectPoints(..., aspectRatio=0, jacobian=...)` derivative (so the prior carries honest PnP-geometry uncertainty rather than an arbitrary isotropic sigma). Implementation deviation from the original spec wording (which named `GenericProjectionFactorCal3DS2`): we use `PriorFactorPose3` because (i) `MatchResult` does not carry 2D-tile-to-3D-world-coord georef so a per-landmark projection factor would require new infrastructure out of scope; (ii) `GenericProjectionFactorCal3DS2` adds N landmark variables per frame → unbounded iSAM2 memory; (iii) the two factor types are mathematically equivalent on the pose marginal in expectation (same Fisher information). C5's `add_pose_anchor` adds a SECOND `PriorFactorPose3` later with the marginals-recovered covariance — two prior factors are admissible (information matrices add).
|
||||
f. Build a local `gtsam.NonlinearFactorGraph` + `gtsam.Values`, add the prior factor + insert the initial pose value (skip insert defensively via try/except `RuntimeError` if the key was already initialised by a prior `add_vio`/`add_fc_imu` call). Then `isam2_graph_handle.update(local_graph, local_values, None)` (Option B per the AZ-358 design discussion in batch 58 — C4 passes its per-call diff explicitly; the handle does NOT keep cross-call staging state on C4's behalf).
|
||||
g. Compute `covariance_6x6 = gtsam.Marginals(graph, values).marginalCovariance(pose_key)`.
|
||||
h. Verify SPD: `np.linalg.cholesky(covariance_6x6)` succeeds; if not, log ERROR + raise `PnpFailureError("non-SPD covariance from Marginals; numerical instability")` (defensive).
|
||||
i. Convert local-tangent-plane pose to WGS84 via `wgs_converter.local_to_wgs84(pose)`.
|
||||
@@ -47,12 +47,12 @@ Without this task, C5 has no source of pose anchors with native 6×6 covariance
|
||||
3. Return the `PoseEstimate`.
|
||||
- `current_covariance_mode() -> CovarianceMode`: return `self._last_covariance_mode` (initialised to `MARGINALS` at construction; updated per call).
|
||||
- Module-level `create(config, ransac_filter, wgs_converter, se3_utils, isam2_graph_handle) -> PoseEstimator` factory function.
|
||||
- `ISam2GraphHandle` Protocol extension (in AZ-?'s `_isam2_handle.py` — co-developed):
|
||||
- `add_factor(factor) -> None`: add a factor to the iSAM2 graph.
|
||||
- `update() -> None`: trigger an iSAM2 update.
|
||||
- `ISam2GraphHandle` Protocol extension (in `c4_pose/_isam2_handle.py` — this task extends the AZ-355 stub):
|
||||
- `add_factor(factor) -> None`: append a factor to C5's pending staging buffer. NOT used by C4 in Option B but kept on the Protocol for symmetry with the C5-side superset Protocol so any C5-impl satisfies both.
|
||||
- `update(graph, values, timestamps=None) -> None`: apply a per-call diff (NonlinearFactorGraph + Values + optional FixedLagSmootherKeyTimestampMap) to iSAM2. C4 passes a local NonlinearFactorGraph carrying its single PriorFactorPose3.
|
||||
- `compute_marginals() -> gtsam.Marginals`: returns the Marginals object for covariance recovery.
|
||||
- `last_anchor_age_ms() -> int`: tracked by C5; broadcast to C4 via the handle.
|
||||
The extension is part of THIS task's scope; the Protocol stub in AZ-? is updated in lockstep.
|
||||
The extension is part of THIS task's scope; the Protocol stub in AZ-355 is widened in lockstep. The C5-side `ISam2GraphHandleImpl` (AZ-382) already provides all four methods with matching signatures, so no C5 source change is required.
|
||||
|
||||
## Scope
|
||||
|
||||
@@ -107,7 +107,7 @@ Then the WGS84 conversion exactly matches `WgsConverter.local_to_wgs84(pose)` te
|
||||
**AC-7: Factor add against C5 iSAM2 handle**
|
||||
Given a stub `ISam2GraphHandle` recording all calls
|
||||
When `estimate(...)` runs
|
||||
Then the call sequence is: `add_factor(factor)` × 1 → `update()` × 1 → `compute_marginals()` × 1 → marginal recovered. No second `update()` per frame.
|
||||
Then the call sequence is: `update(graph, values, timestamps) × 1 → compute_marginals() × 1 → marginal recovered`. No second `update()` per frame; `add_factor()` is NOT called by C4 directly (Option B — C4 passes its per-call diff via `update`'s `graph` parameter; the prior factor is appended to the local NonlinearFactorGraph BEFORE the `update` call rather than through a staged `add_factor` step). `get_pose_key(frame_id) × 1` is also expected.
|
||||
|
||||
**AC-8: Thermal throttle → `NotImplementedError`**
|
||||
Given `thermal_state.throttle = True`
|
||||
@@ -152,7 +152,7 @@ Every `estimate` call (success OR `PnpFailureError`) emits exactly ONE FDR recor
|
||||
| AC-4 | Mode == MARGINALS | Both `PoseEstimate.covariance_mode` and `current_covariance_mode()` |
|
||||
| AC-5 | Source label == SATELLITE_ANCHORED | Always on success |
|
||||
| AC-6 | `WgsConverter` use | Output matches helper vectors |
|
||||
| AC-7 | iSAM2 handle call sequence | `add_factor` → `update` → `compute_marginals`, each ×1 |
|
||||
| AC-7 | iSAM2 handle call sequence | `get_pose_key` → `update(graph, values, ts)` → `compute_marginals`, each ×1 |
|
||||
| AC-8 | Thermal throttle → `NotImplementedError` | Exception raised with documented message |
|
||||
| AC-9 | Non-SPD covariance defensive | `PnpFailureError("non-SPD ...")` |
|
||||
| AC-10 | Composition wiring | INFO log; identity-shared helpers |
|
||||
@@ -184,9 +184,9 @@ Every `estimate` call (success OR `PnpFailureError`) emits exactly ONE FDR recor
|
||||
## Runtime Completeness
|
||||
|
||||
- **Named capability**: `OpenCVGtsamPoseEstimator` Marginals path — production-default `PoseEstimator`.
|
||||
- **Production code that must exist**: real OpenCV `solvePnPRansac` call; real GTSAM factor add against C5's iSAM2 handle; real `Marginals.marginalCovariance(pose_key)`; real WGS84 conversion via `WgsConverter`; real `PnpFailureError` raise on failure; real SPD-invariant defensive check; real FDR record emission; real composition-root wiring.
|
||||
- **Production code that must exist**: real OpenCV `solvePnPRansac` call; real GTSAM `PriorFactorPose3` add to a per-call NonlinearFactorGraph and flushed via `handle.update(graph, values, None)`; real `Marginals.marginalCovariance(pose_key)`; real WGS84 conversion via `WgsConverter`; real `PnpFailureError` raise on failure; real SPD-invariant defensive check; real FDR record emission; real composition-root wiring; real Jacobian-derived initial-prior covariance from PnP residuals (cv2.projectPoints with jacobian output → JᵀJ/σ²).
|
||||
- **Allowed external stubs**: `FakeISam2GraphHandle`, `FakeRansacFilter`, `FakeWgsConverter`, `FakeSE3Utils`, `FakeFdrClient`. Production wiring uses real concretes (real OpenCV, real GTSAM).
|
||||
- **Unacceptable substitutes**: a SciPy-only PnP implementation (would not have the `IPPE` solver behaviour); a Jacobian-derived covariance on the steady-state path (would be the Hybrid task's job); inline WGS84 math (violates AC-6); a synthetic Marginals object that returns canned matrices (would skip the actual GTSAM integration test).
|
||||
- **Unacceptable substitutes**: a SciPy-only PnP implementation (would not have the `IPPE` solver behaviour); a Jacobian-derived covariance on the steady-state path POSTERIOR (would be the Hybrid task's job — note: the Jacobian-derived INITIAL covariance on the prior factor is allowed and necessary, only the posterior comes from Marginals); inline WGS84 math (violates AC-6); a synthetic Marginals object that returns canned matrices in production (would skip the actual GTSAM integration test — tests may stub).
|
||||
|
||||
## Contract
|
||||
|
||||
@@ -0,0 +1,168 @@
|
||||
# Batch 58 — Cycle 1 Report
|
||||
|
||||
**Date**: 2026-05-14
|
||||
**Tasks**: AZ-358 (C4 OpenCVGtsamPoseEstimator — Marginals path), AZ-361 (C4 Jacobian + thermal-driven hybrid)
|
||||
**Verdict**: COMPLETE — PASS_WITH_WARNINGS
|
||||
|
||||
## Summary
|
||||
|
||||
Implemented the single C4 production-default `PoseEstimator` strategy
|
||||
(`OpenCVGtsamPoseEstimator`) covering both the AZ-358 Marginals path
|
||||
and the AZ-361 thermal-throttle Jacobian fallback in one file. Per-
|
||||
frame mode dispatch reads `thermal_state.thermal_throttle_active` at
|
||||
`estimate(...)` entry and routes to either `_estimate_marginals_path`
|
||||
(production default) or `_estimate_jacobian_path` (degraded fallback)
|
||||
with zero buffering or hysteresis (Invariant 4).
|
||||
|
||||
Both paths share `_run_pnp` (OpenCV `solvePnPRansac` with
|
||||
`SOLVEPNP_IPPE` on best-candidate inliers, converting tile pixels →
|
||||
ENU world points via the shared `WgsConverter` + a configurable
|
||||
`tile_size_px`), `_jacobian_covariance` (a `cv2.projectPoints`-derived
|
||||
6×6 information matrix with ridge regularisation), `_assemble_pose_estimate`
|
||||
(WGS84 conversion + quaternion + FDR-friendly DTO assembly), and the
|
||||
`pose.frame_done` FDR record. The Marginals path additionally builds
|
||||
a local `gtsam.NonlinearFactorGraph` + `Values` carrying a single
|
||||
`PriorFactorPose3` (initial covariance Jacobian-derived) and flushes
|
||||
both into C5's iSAM2 graph via the now-widened
|
||||
`ISam2GraphHandle.update(graph, values, None)` (Option B per the
|
||||
batch-58 design discussion); posterior covariance comes from
|
||||
`handle.compute_marginals().marginalCovariance(pose_key)`. The
|
||||
Jacobian path uses the Jacobian-derived covariance directly as the
|
||||
emitted covariance and SKIPS the iSAM2 factor add (Invariant 12 —
|
||||
under throttle the graph stops growing; recovery is automatic).
|
||||
|
||||
Two documented deviations from the task wording (now mirrored in the
|
||||
updated AZ-358 spec):
|
||||
|
||||
1. **`PriorFactorPose3` instead of `GenericProjectionFactorCal3DS2`**.
|
||||
Mathematically equivalent on the pose marginal (same Fisher
|
||||
information), avoids unbounded landmark variables in iSAM2, and
|
||||
requires no new tile-pixel-to-3D-world georef infrastructure beyond
|
||||
what `WgsConverter` already ships.
|
||||
2. **`handle.update(graph, values, None)` instead of `handle.update()`**.
|
||||
The C5-side `ISam2GraphHandleImpl` requires explicit
|
||||
`(graph, values, timestamps)` arguments — C4 builds the per-call
|
||||
diff itself rather than relying on a hidden C5 staging buffer
|
||||
(Option B).
|
||||
|
||||
AZ-361 supersedes AZ-358's `NotImplementedError("Jacobian path owned
|
||||
by Hybrid task")` placeholder: under throttle, the Jacobian path now
|
||||
runs as the documented production fallback. A `CovarianceDegradedWarning`
|
||||
is emitted via `warnings.warn` (NEVER raised) and a paired
|
||||
`c4.pose.covariance_degraded` WARN log + FDR record are emitted, all
|
||||
three rate-limited to one emission per
|
||||
`covariance_degraded_warn_window_ns` (default 60 s) via an injected
|
||||
monotonic `Clock`.
|
||||
|
||||
`ISam2GraphHandle` widened from one method (`get_pose_key`) to five
|
||||
(`add_factor`, `update`, `compute_marginals`, `last_anchor_age_ms`).
|
||||
The C5-side `ISam2GraphHandleImpl` already provides the superset, so
|
||||
no C5 source change is required this batch. `add_factor` is kept on
|
||||
the Protocol for producer/consumer symmetry but is NOT called by C4
|
||||
under Option B (factors travel through the local graph passed to
|
||||
`update`).
|
||||
|
||||
Pose-factory composition path now threads `fdr_client` + `clock` from
|
||||
the runtime root into the concrete estimator, with both optional —
|
||||
AZ-355-style protocol-only tests that wire a fake factory still
|
||||
work unchanged.
|
||||
|
||||
## Files added / modified
|
||||
|
||||
### Added (2)
|
||||
|
||||
- `src/gps_denied_onboard/components/c4_pose/opencv_gtsam_estimator.py` —
|
||||
`OpenCVGtsamPoseEstimator` + module-level `create` + `register`.
|
||||
Both paths plus all per-path helpers (~970 LOC total). Heavy
|
||||
module docstring documents the two deviations above and the
|
||||
tile-pixel-to-ENU-world georef workaround.
|
||||
- `tests/unit/c4_pose/test_az358_361_opencv_gtsam_estimator.py` —
|
||||
21 tests covering AZ-358 AC-1..AC-11 plus AZ-361 AC-1..AC-13 (the
|
||||
AZ-358 AC-8 row is replaced by the AZ-361 dispatch-now-runs-Jacobian
|
||||
test; AZ-361 AC-11 RMSE-ratio is informational per spec and not
|
||||
asserted — recorded as a Low Spec-Gap in the batch review).
|
||||
|
||||
### Modified (7)
|
||||
|
||||
- `src/gps_denied_onboard/components/c4_pose/_isam2_handle.py` —
|
||||
Protocol widened from `get_pose_key` only to all five methods
|
||||
(`add_factor`, `update`, `compute_marginals`, `last_anchor_age_ms`)
|
||||
with C4-facing docstrings explaining Option B and the
|
||||
producer/consumer symmetry invariant with C5's superset.
|
||||
- `src/gps_denied_onboard/components/c4_pose/config.py` — three new
|
||||
fields with validation: `covariance_degraded_warn_window_ns`
|
||||
(AZ-361 rate-limit window, default 60 s), `ridge_regularisation_epsilon`
|
||||
(AZ-361 ridge added to JᵀJ/σ², default 1e-9), `tile_size_px` (AZ-358
|
||||
satellite-tile pixel dimensions, default 256).
|
||||
- `src/gps_denied_onboard/fdr_client/records.py` — registered two new
|
||||
payload kinds: `pose.frame_done` (per-call telemetry, both success
|
||||
and `PnpFailureError` paths) and `pose.covariance_degraded` (per-
|
||||
window AZ-361 throttle exposure).
|
||||
- `src/gps_denied_onboard/runtime_root/pose_factory.py` —
|
||||
`build_pose_estimator` now accepts optional `fdr_client` and `clock`
|
||||
parameters and threads them through to the concrete strategy. The
|
||||
`ISam2GraphHandle` runtime-check error message updated to mention
|
||||
all four widened methods.
|
||||
- `tests/unit/c4_pose/test_az355_pose_protocol.py` —
|
||||
`_FakeISam2GraphHandle` widened to satisfy all five Protocol
|
||||
methods; new `test_ac10_isam2_graph_handle_rejects_partial_surface`
|
||||
asserts a partial implementation now fails the `isinstance` gate;
|
||||
`test_factory_lazy_imports_when_registry_empty` flipped from
|
||||
"expect ImportError" to "lazy import succeeds and returns a
|
||||
`PoseEstimator`" now that the concrete module ships.
|
||||
- `tests/unit/test_az272_fdr_record_schema.py` — round-trip fixture
|
||||
payloads added for both new kinds.
|
||||
- `_docs/02_tasks/todo/AZ-358_c4_opencv_gtsam_marginals.md` (now in
|
||||
`_docs/02_tasks/done/`) — AC-7 + Outcome steps e/f rewritten to
|
||||
document the Option B (local graph+values + `update(g, v, None)`)
|
||||
approach and the `PriorFactorPose3` deviation.
|
||||
|
||||
## Task Results
|
||||
|
||||
| Task | Status | Files Modified | Focused tests | AC Coverage | Issues |
|
||||
|---------|--------|---------------------------|---------------|--------------|--------|
|
||||
| AZ-358 | Done | 1 added / 6 modified | 21/21 pass | 11/11 covered (AC-8 superseded by AZ-361) | Two documented deviations from spec wording; both mirrored back into the spec |
|
||||
| AZ-361 | Done | 0 added / 0 modified (shared file) | 21/21 pass | 12/13 covered (AC-11 informational; see review F5) | None blocking |
|
||||
|
||||
## AC Test Coverage: 23/24 covered (one informational AC documented)
|
||||
|
||||
- AZ-358 AC-1..AC-7, AC-9, AC-10 (two tests), AC-11 — all directly asserted.
|
||||
- AZ-358 AC-8 superseded by AZ-361 (no `NotImplementedError` ships); replacement test `test_az358_ac8_replacement_throttle_now_runs_jacobian_path` documents the supersession.
|
||||
- AZ-361 AC-1..AC-10, AC-12, AC-13 — all directly asserted.
|
||||
- AZ-361 AC-11 (Jacobian RMSE within 1.10× Marginals on synthetic baseline) — not asserted; spec explicitly tags this AC informational and non-blocking.
|
||||
|
||||
## Code Review Verdict: PASS_WITH_WARNINGS
|
||||
|
||||
See `_docs/03_implementation/reviews/batch_58_review.md`. Five findings recorded — Medium ×2, Low ×3 — none blocking:
|
||||
|
||||
1. **F1 Medium / Maintainability** — duplicated FDR-error + ERROR-log + raise blocks across both paths; extract a `_fail(...)` helper.
|
||||
2. **F2 Medium / Performance** — `_tile_pixels_to_enu_world` runs per-point pyproj calls in a Python loop; vectorise via the Transformer's array signature.
|
||||
3. **F3 Low / Style** — bare `except Exception: pass` for GTSAM duplicate-insert; prefer `Values.exists(key)` pre-check.
|
||||
4. **F4 Low / Performance** — `cv2.projectPoints` called twice on the Marginals path (residuals + Jacobian); request both outputs from a single call.
|
||||
5. **F5 Low / Spec-Gap** — AZ-361 AC-11 RMSE-ratio not asserted (informational per spec).
|
||||
|
||||
No Critical / High / Architecture findings. Auto-fix not required.
|
||||
|
||||
## Auto-Fix Attempts: 0
|
||||
|
||||
## Stuck Agents: None
|
||||
|
||||
## Tests Run
|
||||
|
||||
- Focused suite (`tests/unit/c4_pose/` + `tests/unit/test_az272_fdr_record_schema.py`): **101 passed**.
|
||||
- Full repo suite: **1958 passed, 1 failed, 84 skipped**. The single failure (`tests/unit/c12_operator_orchestrator/test_cli_console_script.py::test_cold_start_under_500ms_p99`) is an NFR cold-start timing assertion (500 ms hard limit) failing on macOS dev hardware (700-1100 ms observed). Unrelated to the C4 / FDR surface and pre-existing on this host.
|
||||
|
||||
## Next Batch
|
||||
|
||||
This closes the C4 pose-estimator track for cycle 1. The runtime root now has:
|
||||
|
||||
- C2 VPR (AZ-338..341)
|
||||
- C3 cross-domain matchers (AZ-345..347)
|
||||
- C3.5 conditional refiner (AZ-349)
|
||||
- **C4 pose estimator (AZ-358 + AZ-361)** ← this batch
|
||||
|
||||
Downstream consumers that can now wire end-to-end at runtime: C5 state
|
||||
estimator (receives `PoseEstimate` with native 6×6 covariance for the
|
||||
satellite-anchor path), C8 outbound FC message (gets the WGS84 pose
|
||||
+ quaternion + covariance trace), C13 FDR forensics (gets `pose.*`
|
||||
records).
|
||||
@@ -0,0 +1,164 @@
|
||||
# Code Review Report
|
||||
|
||||
**Batch**: 58 — AZ-358 (C4 OpenCVGtsamPoseEstimator — Marginals) + AZ-361 (C4 Jacobian + thermal hybrid)
|
||||
**Date**: 2026-05-14
|
||||
**Verdict**: PASS_WITH_WARNINGS
|
||||
|
||||
## Scope
|
||||
|
||||
Changed files reviewed:
|
||||
|
||||
* `src/gps_denied_onboard/components/c4_pose/opencv_gtsam_estimator.py` (new, ~970 LOC)
|
||||
* `src/gps_denied_onboard/components/c4_pose/_isam2_handle.py` (Protocol widened 1 → 5 methods)
|
||||
* `src/gps_denied_onboard/components/c4_pose/config.py` (3 new fields + validation)
|
||||
* `src/gps_denied_onboard/fdr_client/records.py` (2 new payload kinds)
|
||||
* `src/gps_denied_onboard/runtime_root/pose_factory.py` (threads `fdr_client` + `clock`)
|
||||
* `tests/unit/c4_pose/test_az358_361_opencv_gtsam_estimator.py` (new, 21 tests)
|
||||
* `tests/unit/c4_pose/test_az355_pose_protocol.py` (Protocol fixture widened; lazy-import test corrected)
|
||||
* `tests/unit/test_az272_fdr_record_schema.py` (payload fixture extended)
|
||||
* `_docs/02_tasks/todo/AZ-358_c4_opencv_gtsam_marginals.md` (Option B + `PriorFactorPose3` deviation documented)
|
||||
|
||||
## Spec Compliance
|
||||
|
||||
### AZ-358 (Marginals path)
|
||||
|
||||
| AC | Verified by | Status |
|
||||
|----|-------------|--------|
|
||||
| AC-1 PnP success on synthetic correspondences | `test_az358_ac1_pnp_success_synthetic_within_tolerance` | PASS |
|
||||
| AC-2 Degenerate inliers → `PnpFailureError` | `test_az358_ac2_insufficient_inliers_raises_pnp_failure` | PASS |
|
||||
| AC-3 SPD covariance | `test_az358_ac3_4_5_marginals_success_invariants` | PASS |
|
||||
| AC-4 `covariance_mode == MARGINALS` on success | same | PASS |
|
||||
| AC-5 `source_label == SATELLITE_ANCHORED` | same | PASS |
|
||||
| AC-6 WGS84 conversion uses shared `WgsConverter` | `test_az358_ac6_wgs_converter_injection` | PASS |
|
||||
| AC-7 iSAM2 handle call sequence (no `add_factor`; 1× each of `get_pose_key`, `update`, `compute_marginals`) | `test_az358_ac7_isam2_handle_call_sequence` + `test_az358_update_carries_prior_factor_in_local_graph` | PASS |
|
||||
| AC-8 (replaced by AZ-361 — throttle now runs Jacobian path, not `NotImplementedError`) | `test_az358_ac8_replacement_throttle_now_runs_jacobian_path` | PASS — deviation documented in the AZ-358 spec |
|
||||
| AC-9 Non-SPD covariance defensive raise | `test_az358_ac9_non_spd_covariance_raises_pnp_failure` | PASS |
|
||||
| AC-10 Composition-root wiring (INFO log + identity-shared deps) | `test_az358_ac10_composition_root_wiring_emits_ready_log` + `test_az358_ac10_create_module_factory_returns_protocol_instance` | PASS |
|
||||
| AC-11 FDR `pose.frame_done` record shape | `test_az358_ac11_fdr_pose_frame_done_shape_on_success` | PASS |
|
||||
|
||||
Documented deviations from the original spec wording (now mirrored in the updated spec):
|
||||
|
||||
1. `PriorFactorPose3` instead of `GenericProjectionFactorCal3DS2` — mathematically equivalent on the pose marginal, avoids unbounded landmark variables, no new georef infra needed.
|
||||
2. `handle.update(graph, values, None)` instead of no-arg `update()` — Option B (C4 builds the per-call diff itself; aligns with C5's `ISam2GraphHandleImpl`).
|
||||
3. AC-8 supplanted by AZ-361 — no `NotImplementedError` placeholder is shipped.
|
||||
|
||||
### AZ-361 (Jacobian + thermal hybrid)
|
||||
|
||||
| AC | Verified by | Status |
|
||||
|----|-------------|--------|
|
||||
| AC-1 Per-frame mode dispatch | `test_az361_ac1_2_4_per_frame_mode_dispatch_alternates` | PASS |
|
||||
| AC-2 Mode-switch latency ≤ 1 frame | same | PASS |
|
||||
| AC-3 Jacobian covariance SPD | `test_az361_ac3_5_jacobian_success_invariants` | PASS |
|
||||
| AC-4 `covariance_mode == JACOBIAN` | same + AC-1 test | PASS |
|
||||
| AC-5 Source label SATELLITE_ANCHORED both paths | AC-3 + AC-5 tests | PASS |
|
||||
| AC-6 `CovarianceDegradedWarning` via `warnings.warn` (not raised) | `test_az361_ac6_warning_emitted_via_warnings_warn` | PASS |
|
||||
| AC-7 Warning rate-limited per 60 s window | `test_az361_ac7_8_warning_and_log_rate_limited` | PASS |
|
||||
| AC-8 WARN log rate-limited similarly | same | PASS |
|
||||
| AC-9 Marginals path unchanged by refactor | full AZ-358 AC suite still passes | PASS |
|
||||
| AC-10 Near-singular Jacobian → defensive raise | `test_az361_ac10_near_singular_jacobian_raises_pnp_failure` | PASS |
|
||||
| AC-11 Jacobian within ~5–10 % of Marginals (informational) | not tested — see F5 | PARTIAL (spec marks informational) |
|
||||
| AC-12 Jacobian path skips iSAM2 factor add | `test_az361_ac12_jacobian_path_skips_isam2_update` | PASS |
|
||||
| AC-13 FDR `mode` field distinguishes path | `test_az361_ac13_fdr_mode_field_distinguishes_path` | PASS |
|
||||
|
||||
### Contract verification
|
||||
|
||||
`_docs/02_document/contracts/c4_pose/pose_estimator_protocol.md` v1.0.0:
|
||||
|
||||
* Public `PoseEstimator.estimate` signature matches the implementation's `estimate(match_result, calibration, thermal_state) -> PoseEstimate`.
|
||||
* Invariant 1 (single-threaded) — not enforced structurally (Python GIL + composition-root contract); documented.
|
||||
* Invariant 5 (SPD covariance) — runtime `np.linalg.cholesky` check on both paths; on failure raises `PnpFailureError`.
|
||||
* Invariant 6 (covariance_mode matches path actually taken) — set per-path before assemble.
|
||||
* Invariant 7 (source_label always SATELLITE_ANCHORED on success) — hard-coded in `_assemble_pose_estimate`.
|
||||
* Invariant 8 (last_satellite_anchor_age_ms sourced from C5) — comes via `handle.last_anchor_age_ms()`; C4 never computes it.
|
||||
* Invariant 9 (only `PnpFailureError` escapes) — verified by error-path tests.
|
||||
|
||||
The `ISam2GraphHandle` Protocol stub at `c4_pose/_isam2_handle.py` is widened to five methods in lockstep with C5's superset implementation (per ADR-003). C5 is documented to already provide the superset, so no C5-side source change is required this batch.
|
||||
|
||||
## Findings
|
||||
|
||||
| # | Severity | Category | File:Line | Title |
|
||||
|---|----------|----------|-----------|-------|
|
||||
| 1 | Medium | Maintainability | `opencv_gtsam_estimator.py:254-345,381-435` | Duplicated FDR-error + ERROR-log + raise blocks across Marginals and Jacobian paths |
|
||||
| 2 | Medium | Performance | `opencv_gtsam_estimator.py:617-657` | `_tile_pixels_to_enu_world` runs a Python `for` loop with per-point `pyproj` Transformer calls |
|
||||
| 3 | Low | Style | `opencv_gtsam_estimator.py:289-296` | Bare `except Exception: pass` for GTSAM duplicate-key insert; could narrow to `RuntimeError` or pre-check via `Values.exists(key)` |
|
||||
| 4 | Low | Performance | `opencv_gtsam_estimator.py:535-540, 575-586` | `cv2.projectPoints` called twice on the Marginals path (residuals + Jacobian); a single call returning both outputs would halve that cost |
|
||||
| 5 | Low | Spec-Gap | `tests/unit/c4_pose/test_az358_361_opencv_gtsam_estimator.py` | AZ-361 AC-11 (Jacobian RMSE within 1.10× Marginals on synthetic baseline) is not directly asserted; spec explicitly tags this AC informational |
|
||||
|
||||
### Finding details
|
||||
|
||||
**F1: Duplicated FDR-error + ERROR-log + raise blocks** (Medium / Maintainability)
|
||||
|
||||
* Location: `opencv_gtsam_estimator.py` — three near-identical 10-line blocks inside `_estimate_marginals_path` (PnP-fail, marginals-fail, SPD-fail) and two more inside `_estimate_jacobian_path` (PnP-fail, SPD-fail). All five do: `_emit_pose_frame_done_fdr_error(...)` → `self._log.error(_LOG_KIND_PNP_FAILURE, extra={...})` → `raise PnpFailureError(...)`.
|
||||
* Description: shared error-emit shape; only the `stage` key + the wrapping exception message change.
|
||||
* Suggestion: extract a private `_fail(frame_id, mode, stage, exc, message) -> NoReturn` helper. Keeps the path methods readable and removes 4 of 5 duplications.
|
||||
* Task: AZ-358 + AZ-361. Non-blocking — internal cleanup only.
|
||||
|
||||
**F2: Per-point pyproj loop in `_tile_pixels_to_enu_world`** (Medium / Performance)
|
||||
|
||||
* Location: `opencv_gtsam_estimator.py:617-657`.
|
||||
* Description: for each inlier, the helper calls `WgsConverter.latlonalt_to_local_enu(origin, LatLonAlt(...))`, which in turn calls `_ECEF_FROM_LLA.transform(...)` once. On a 50-inlier frame this is 50 individual pyproj boundary crossings (~ms each). Pose-path p95 target is 90 ms for the whole Marginals path; this leaves little headroom.
|
||||
* Suggestion: collect `(lon, lat, alt)` arrays once, call `_ECEF_FROM_LLA.transform(lons, lats, alts)` in vectorized form, then apply the (single) ENU rotation in NumPy. Behavior-preserving.
|
||||
* Task: AZ-358. Non-blocking for AC compliance; meaningful for NFT-LIM-04 / C4-PT-01 latency margin.
|
||||
|
||||
**F3: Bare `except Exception` on `local_values.insert`** (Low / Style)
|
||||
|
||||
* Location: `opencv_gtsam_estimator.py:289-296`.
|
||||
* Description: catches *all* exceptions to swallow GTSAM's duplicate-insert error. A real bug elsewhere (wrong type, wrong key) would be silently masked.
|
||||
* Suggestion: replace with `if not local_values.exists(pose_key): local_values.insert(...)`. Removes the swallow entirely.
|
||||
* Task: AZ-358. Non-blocking.
|
||||
|
||||
**F4: Two `cv2.projectPoints` calls per Marginals frame** (Low / Performance)
|
||||
|
||||
* Location: `opencv_gtsam_estimator.py:_compute_reprojection_residuals` + `opencv_gtsam_estimator.py:_jacobian_covariance`.
|
||||
* Description: `cv2.projectPoints` is called once to compute residuals (no Jacobian requested) and once more to compute the Jacobian (no residual reuse). On the Marginals path both fire per frame.
|
||||
* Suggestion: in `_run_pnp`, request Jacobian on the residual call and stash it on `_PnpResult`; `_jacobian_covariance` reuses the stashed Jacobian.
|
||||
* Task: AZ-358. Non-blocking.
|
||||
|
||||
**F5: AZ-361 AC-11 (Jacobian-vs-Marginals RMSE) not asserted in tests** (Low / Spec-Gap)
|
||||
|
||||
* Location: `tests/unit/c4_pose/test_az358_361_opencv_gtsam_estimator.py`.
|
||||
* Description: the spec marks AC-11 informational and explicitly states it does NOT block. Still, no test exists today that runs both paths on a fixed synthetic and asserts the RMSE ratio.
|
||||
* Suggestion: add a single synthetic-baseline test that runs both paths on the same `MatchResult` and asserts `rmse_jacobian / rmse_marginals <= 1.10`. Defer to a follow-up batch — does not block this review.
|
||||
* Task: AZ-361.
|
||||
|
||||
## Phase 4 — Security Quick-Scan
|
||||
|
||||
No findings. No SQL paths, subprocess usage, hardcoded credentials, network I/O, or unbounded deserialization in the changed surface. Log/FDR payloads carry only telemetry (frame ids, covariance norms, residuals, public coordinates) — no operator PII.
|
||||
|
||||
## Phase 5 — Performance Scan
|
||||
|
||||
* F2 and F4 above are the only meaningful items.
|
||||
* No `O(n^2)` patterns; no N+1 queries; no blocking I/O.
|
||||
* NumPy `np.asarray(..., copy=False)` and `np.ascontiguousarray(...)` used appropriately on hot paths.
|
||||
|
||||
## Phase 6 — Cross-Task Consistency
|
||||
|
||||
AZ-358 and AZ-361 land in the same file and share `_run_pnp`, `_jacobian_covariance`, `_assemble_pose_estimate`, `_emit_pose_frame_done_fdr`, and `_emit_pose_frame_done_fdr_error`. Both paths emit the same `pose.frame_done` schema; only AZ-361 additionally emits `pose.covariance_degraded`. No conflicting patterns.
|
||||
|
||||
The `ISam2GraphHandle` Protocol widening lands in lockstep with both tasks and with the AZ-355 test fixture (`_FakeISam2GraphHandle` now implements all five methods). A new test (`test_ac10_isam2_graph_handle_rejects_partial_surface`) was added to the AZ-355 test file to keep the runtime-checkable Protocol from regressing.
|
||||
|
||||
## Phase 7 — Architecture Compliance
|
||||
|
||||
Imports in `opencv_gtsam_estimator.py`:
|
||||
|
||||
* `_types/*` (L1) — OK.
|
||||
* `clock/wall_clock` (L1) — OK.
|
||||
* `components/c4_pose/*` (same component, L3) — OK.
|
||||
* `fdr_client/records` (L2 cross-cutting) — OK.
|
||||
* `helpers/wgs_converter` (L2 helpers) — OK.
|
||||
* `logging` (L1) — OK.
|
||||
|
||||
No cross-component imports into C5/C6/C7 internals (the only C5 touch-point is the `ISam2GraphHandle` Protocol owned by `c4_pose/_isam2_handle.py`). No new module cycles introduced (verified by full-suite run). No duplicated symbols (Quat / LatLonAlt / PoseEstimate are imported, not redefined). Cross-cutting concerns (logging, FDR, WGS conversion, clock) are consumed via shared modules, not re-implemented.
|
||||
|
||||
`runtime_root/pose_factory.py` legitimately imports both the C4 public surface (via `__init__`) and the private `_isam2_handle` module — the latter is documented as composition-root-only, which `pose_factory.py` is.
|
||||
|
||||
No Architecture findings.
|
||||
|
||||
## Verdict
|
||||
|
||||
**PASS_WITH_WARNINGS** — every AC that the spec marks blocking has at least one passing unit test. Five non-blocking findings (Medium ×2, Low ×3) are documented above for follow-up. No Critical or High findings.
|
||||
|
||||
## Test outcomes
|
||||
|
||||
* `tests/unit/c4_pose/` + `tests/unit/test_az272_fdr_record_schema.py`: **101 passed**.
|
||||
* Full repo suite: **1958 passed, 1 failed, 84 skipped**. The single failure (`tests/unit/c12_operator_orchestrator/test_cli_console_script.py::test_cold_start_under_500ms_p99`) is a host-dependent CLI cold-start performance assertion (500 ms NFR vs. 700-1100 ms on this macOS dev box) — unrelated to the C4 / FDR surface and pre-existing flake on this hardware.
|
||||
@@ -12,6 +12,6 @@ sub_step:
|
||||
retry_count: 0
|
||||
cycle: 1
|
||||
tracker: jira
|
||||
last_completed_batch: 57
|
||||
last_completed_batch: 58
|
||||
last_cumulative_review: batches_55-57
|
||||
current_batch: 58
|
||||
current_batch: 59
|
||||
|
||||
@@ -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).
|
||||
"""
|
||||
|
||||
@@ -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}"
|
||||
)
|
||||
|
||||
@@ -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)
|
||||
@@ -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())
|
||||
|
||||
@@ -86,6 +86,8 @@ def build_pose_estimator(
|
||||
wgs_converter: Any,
|
||||
se3_utils: Any,
|
||||
isam2_graph_handle: ISam2GraphHandle,
|
||||
fdr_client: Any | None = None,
|
||||
clock: Any | None = None,
|
||||
) -> PoseEstimator:
|
||||
"""Resolve + build the configured C4 pose estimator.
|
||||
|
||||
@@ -93,6 +95,13 @@ def build_pose_estimator(
|
||||
isam2_graph_handle conforms? → factory lookup (with lazy-import
|
||||
fallback) → INFO log on success.
|
||||
|
||||
``fdr_client`` and ``clock`` are AZ-358-introduced optional
|
||||
dependencies. When omitted (e.g. AZ-355 protocol-only tests that
|
||||
register a fake factory) the runtime root passes ``None`` and
|
||||
the concrete strategy decides how to handle it (the
|
||||
``opencv_gtsam`` impl no-ops FDR enqueues + falls back to
|
||||
:class:`MonotonicClock` for rate-limiting).
|
||||
|
||||
Raises:
|
||||
PoseEstimatorConfigError: invalid config, unknown strategy,
|
||||
non-conforming graph handle, or registry miss after
|
||||
@@ -118,7 +127,8 @@ def build_pose_estimator(
|
||||
if not isinstance(isam2_graph_handle, ISam2GraphHandle):
|
||||
raise PoseEstimatorConfigError(
|
||||
"build_pose_estimator: isam2_graph_handle does not satisfy "
|
||||
"the C4 ISam2GraphHandle Protocol (missing get_pose_key?)"
|
||||
"the C4 ISam2GraphHandle Protocol (missing get_pose_key / "
|
||||
"update / compute_marginals / last_anchor_age_ms?)"
|
||||
)
|
||||
|
||||
factory = _resolve_factory(strategy)
|
||||
@@ -128,6 +138,8 @@ def build_pose_estimator(
|
||||
wgs_converter=wgs_converter,
|
||||
se3_utils=se3_utils,
|
||||
isam2_graph_handle=isam2_graph_handle,
|
||||
fdr_client=fdr_client,
|
||||
clock=clock,
|
||||
)
|
||||
log.info(
|
||||
f"c4.pose.strategy_loaded: strategy={strategy} "
|
||||
|
||||
@@ -78,11 +78,28 @@ def _build_config(**overrides: Any) -> Config:
|
||||
|
||||
|
||||
class _FakeISam2GraphHandle:
|
||||
"""Minimal handle stub for factory / Protocol tests."""
|
||||
"""Minimal handle stub for factory / Protocol tests.
|
||||
|
||||
Implements the AZ-358-extended 5-method surface — the AZ-355
|
||||
AC-10 ``isinstance(handle, ISam2GraphHandle)`` runtime-checkable
|
||||
test now expects all five methods.
|
||||
"""
|
||||
|
||||
def get_pose_key(self, frame_id: int) -> int:
|
||||
return int(frame_id)
|
||||
|
||||
def add_factor(self, factor: Any) -> None:
|
||||
return None
|
||||
|
||||
def update(self, graph: Any, values: Any, timestamps: Any | None = None) -> None:
|
||||
return None
|
||||
|
||||
def compute_marginals(self) -> Any:
|
||||
return None
|
||||
|
||||
def last_anchor_age_ms(self) -> int:
|
||||
return 0
|
||||
|
||||
|
||||
class _FakePoseEstimator:
|
||||
"""Test double satisfying the full PoseEstimator Protocol."""
|
||||
@@ -378,6 +395,18 @@ def test_ac10_isam2_graph_handle_rejects_missing_method() -> None:
|
||||
assert not isinstance(_NoMethod(), ISam2GraphHandle)
|
||||
|
||||
|
||||
def test_ac10_isam2_graph_handle_rejects_partial_surface() -> None:
|
||||
"""AZ-358 widened the Protocol to 5 methods; a handle that only
|
||||
implements the original ``get_pose_key`` no longer satisfies
|
||||
runtime_checkable conformance."""
|
||||
|
||||
class _OnlyGetPoseKey:
|
||||
def get_pose_key(self, frame_id: int) -> int:
|
||||
return int(frame_id)
|
||||
|
||||
assert not isinstance(_OnlyGetPoseKey(), ISam2GraphHandle)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# Bonus: factory wires constructor dependencies through to the strategy
|
||||
|
||||
@@ -411,19 +440,23 @@ def test_factory_passes_dependencies_to_strategy() -> None:
|
||||
|
||||
|
||||
def test_factory_lazy_imports_when_registry_empty() -> None:
|
||||
# Arrange — registry is empty (fixture cleared it); the
|
||||
# lazy-import fallback should pick up the AZ-358 concrete
|
||||
# ``opencv_gtsam_estimator`` module and resolve its ``create``
|
||||
# callable.
|
||||
cfg = _build_config()
|
||||
# Registry is cleared by the fixture; the lazy-import fallback
|
||||
# should attempt to import the concrete module. We have not
|
||||
# shipped opencv_gtsam_estimator yet (AZ-358), so the import
|
||||
# raises and gets wrapped in PoseEstimatorConfigError.
|
||||
with pytest.raises(PoseEstimatorConfigError):
|
||||
build_pose_estimator(
|
||||
cfg,
|
||||
ransac_filter=mock.MagicMock(),
|
||||
wgs_converter=mock.MagicMock(),
|
||||
se3_utils=mock.MagicMock(),
|
||||
isam2_graph_handle=_FakeISam2GraphHandle(),
|
||||
)
|
||||
|
||||
# Act — call should succeed (lazy import resolves to AZ-358).
|
||||
estimator = build_pose_estimator(
|
||||
cfg,
|
||||
ransac_filter=mock.MagicMock(),
|
||||
wgs_converter=mock.MagicMock(),
|
||||
se3_utils=mock.MagicMock(),
|
||||
isam2_graph_handle=_FakeISam2GraphHandle(),
|
||||
)
|
||||
|
||||
# Assert — the returned object satisfies the PoseEstimator Protocol.
|
||||
assert isinstance(estimator, PoseEstimator)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
|
||||
@@ -0,0 +1,919 @@
|
||||
"""AZ-358 + AZ-361 — OpenCVGtsamPoseEstimator unit tests.
|
||||
|
||||
Covers the published acceptance criteria:
|
||||
|
||||
AZ-358 (Marginals path)
|
||||
* AC-1 PnP success on synthetic correspondences → WGS84 position within tolerance.
|
||||
* AC-2 Degenerate (insufficient) inliers → ``PnpFailureError`` + ERROR log + FDR error record.
|
||||
* AC-3 SPD covariance — ``np.linalg.cholesky`` succeeds; symmetric.
|
||||
* AC-4 ``covariance_mode == MARGINALS`` on success (both ``PoseEstimate`` and ``current_covariance_mode()``).
|
||||
* AC-5 ``source_label == SATELLITE_ANCHORED`` on success.
|
||||
* AC-6 WGS84 conversion uses the shared :class:`WgsConverter` (verified by injection identity).
|
||||
* AC-7 iSAM2 handle call sequence — ``get_pose_key`` ×1 → ``update`` ×1 → ``compute_marginals`` ×1.
|
||||
* AC-9 Non-SPD covariance defensive — ``PnpFailureError`` with the documented message.
|
||||
* AC-10 Composition-root wiring emits a ``c4.pose.ready`` INFO log via the factory.
|
||||
* AC-11 FDR ``pose.frame_done`` record shape on success.
|
||||
|
||||
AZ-361 (Jacobian + thermal hybrid)
|
||||
* AC-1 Per-frame mode dispatch on alternating thermal flag.
|
||||
* AC-2 Mode-switch latency ≤ 1 frame.
|
||||
* AC-3 Jacobian covariance SPD.
|
||||
* AC-4 ``covariance_mode == JACOBIAN`` on Jacobian path.
|
||||
* AC-5 Source label SATELLITE_ANCHORED regardless of path.
|
||||
* AC-6 ``CovarianceDegradedWarning`` emitted via ``warnings.warn``, not raised.
|
||||
* AC-7 ``warnings.warn`` rate-limited per window.
|
||||
* AC-8 WARN log rate-limited similarly.
|
||||
* AC-12 Jacobian path SKIPS iSAM2 factor add (no ``update`` call).
|
||||
* AC-13 FDR ``mode`` field distinguishes path.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import dataclasses
|
||||
import logging
|
||||
import warnings
|
||||
from typing import Any
|
||||
from uuid import UUID
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from gps_denied_onboard._types.calibration import CameraCalibration
|
||||
from gps_denied_onboard._types.geo import LatLonAlt
|
||||
from gps_denied_onboard._types.matcher import CandidateMatchSet, MatchResult
|
||||
from gps_denied_onboard._types.pose import (
|
||||
CovarianceMode,
|
||||
PoseEstimate,
|
||||
PoseSourceLabel,
|
||||
)
|
||||
from gps_denied_onboard._types.thermal import ThermalState
|
||||
from gps_denied_onboard.components.c4_pose import (
|
||||
C4PoseConfig,
|
||||
CovarianceDegradedWarning,
|
||||
PnpFailureError,
|
||||
PoseEstimator,
|
||||
)
|
||||
from gps_denied_onboard.components.c4_pose.opencv_gtsam_estimator import (
|
||||
OpenCVGtsamPoseEstimator,
|
||||
create,
|
||||
)
|
||||
from gps_denied_onboard.config import Config, load_config
|
||||
from gps_denied_onboard.fdr_client.fakes import FakeFdrSink
|
||||
from gps_denied_onboard.helpers.wgs_converter import WgsConverter
|
||||
from gps_denied_onboard.runtime_root.pose_factory import (
|
||||
build_pose_estimator,
|
||||
clear_pose_registry,
|
||||
)
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# Test infrastructure.
|
||||
|
||||
|
||||
class _FakeMarginals:
|
||||
"""Stand-in for ``gtsam.Marginals`` that returns a canned 6x6 SPD."""
|
||||
|
||||
def __init__(self, cov: np.ndarray) -> None:
|
||||
self._cov = np.asarray(cov, dtype=np.float64)
|
||||
|
||||
def marginalCovariance(self, _key: int) -> np.ndarray: # noqa: N802 — GTSAM API
|
||||
return self._cov
|
||||
|
||||
|
||||
class _RecordingISam2GraphHandle:
|
||||
"""Programmable handle that records every call for AC-7 / AC-12 checks."""
|
||||
|
||||
def __init__(self, *, posterior_cov: np.ndarray | None = None) -> None:
|
||||
if posterior_cov is None:
|
||||
posterior_cov = np.eye(6, dtype=np.float64) * 0.0025
|
||||
self._posterior_cov = posterior_cov
|
||||
self.calls: list[tuple[str, Any, ...]] = []
|
||||
self._next_key = 1
|
||||
|
||||
def get_pose_key(self, frame_id: int) -> int:
|
||||
self.calls.append(("get_pose_key", frame_id))
|
||||
key = self._next_key
|
||||
self._next_key += 1
|
||||
return key
|
||||
|
||||
def add_factor(self, factor: Any) -> None:
|
||||
self.calls.append(("add_factor", factor))
|
||||
|
||||
def update(
|
||||
self, graph: Any, values: Any, timestamps: Any | None = None
|
||||
) -> None:
|
||||
self.calls.append(("update", graph, values, timestamps))
|
||||
|
||||
def compute_marginals(self) -> Any:
|
||||
self.calls.append(("compute_marginals",))
|
||||
return _FakeMarginals(self._posterior_cov)
|
||||
|
||||
def last_anchor_age_ms(self) -> int:
|
||||
self.calls.append(("last_anchor_age_ms",))
|
||||
return 0
|
||||
|
||||
|
||||
class _MutableThermalState:
|
||||
"""Mutable thermal-state container so tests can flip the throttle bit per call."""
|
||||
|
||||
def __init__(self, *, throttle: bool = False) -> None:
|
||||
self.thermal_throttle_active = throttle
|
||||
|
||||
def with_throttle(self, throttle: bool) -> ThermalState:
|
||||
return ThermalState(
|
||||
cpu_temp_c=70.0,
|
||||
gpu_temp_c=80.0 if throttle else 60.0,
|
||||
thermal_throttle_active=throttle,
|
||||
measured_clock_mhz=1200 if not throttle else 600,
|
||||
measured_at_ns=0,
|
||||
is_telemetry_available=True,
|
||||
)
|
||||
|
||||
|
||||
class _FakeClock:
|
||||
"""Deterministic monotonic clock for rate-limit window tests."""
|
||||
|
||||
def __init__(self, start_ns: int = 0) -> None:
|
||||
self._now_ns = start_ns
|
||||
|
||||
def advance(self, delta_ns: int) -> None:
|
||||
self._now_ns += delta_ns
|
||||
|
||||
def monotonic_ns(self) -> int:
|
||||
return self._now_ns
|
||||
|
||||
def time_ns(self) -> int:
|
||||
return self._now_ns
|
||||
|
||||
def sleep_until_ns(self, target_ns: int) -> None:
|
||||
if target_ns > self._now_ns:
|
||||
self._now_ns = target_ns
|
||||
|
||||
|
||||
_TEST_TILE_ID = (18, 49.5, 36.0)
|
||||
_TEST_TILE_SIZE_PX = 256
|
||||
|
||||
|
||||
def _build_config(**overrides: Any) -> Config:
|
||||
cfg = load_config(env={}, paths=(), require_env=False)
|
||||
new_block = dataclasses.replace(C4PoseConfig(), **overrides)
|
||||
components = dict(cfg.components or {})
|
||||
components["c4_pose"] = new_block
|
||||
return dataclasses.replace(cfg, components=components)
|
||||
|
||||
|
||||
def _build_calibration() -> CameraCalibration:
|
||||
K = np.array(
|
||||
[[500.0, 0.0, 320.0], [0.0, 500.0, 240.0], [0.0, 0.0, 1.0]],
|
||||
dtype=np.float64,
|
||||
)
|
||||
return CameraCalibration(
|
||||
camera_id="test_cam",
|
||||
intrinsics_3x3=K,
|
||||
distortion=np.zeros(5, dtype=np.float64),
|
||||
body_to_camera_se3=np.eye(4, dtype=np.float64),
|
||||
acquisition_method="manifest",
|
||||
)
|
||||
|
||||
|
||||
def _tile_pixel_to_enu(
|
||||
tile_id: tuple[int, float, float],
|
||||
px: float,
|
||||
py: float,
|
||||
origin: LatLonAlt,
|
||||
tile_size_px: int = _TEST_TILE_SIZE_PX,
|
||||
) -> np.ndarray:
|
||||
"""Mirror the estimator's tile-pixel → ENU conversion for fixture setup."""
|
||||
zoom, lat_c, lon_c = tile_id
|
||||
tile_x, tile_y = WgsConverter.latlon_to_tile_xy(zoom, lat_c, lon_c)
|
||||
bounds = WgsConverter.tile_xy_to_latlon_bounds(zoom, tile_x, tile_y)
|
||||
lon = bounds.min_lon_deg + (px / tile_size_px) * (
|
||||
bounds.max_lon_deg - bounds.min_lon_deg
|
||||
)
|
||||
lat = bounds.max_lat_deg - (py / tile_size_px) * (
|
||||
bounds.max_lat_deg - bounds.min_lat_deg
|
||||
)
|
||||
return WgsConverter.latlonalt_to_local_enu(
|
||||
origin, LatLonAlt(lat_deg=lat, lon_deg=lon, alt_m=0.0)
|
||||
)
|
||||
|
||||
|
||||
def _synthesise_match_result(
|
||||
*,
|
||||
num_inliers: int = 50,
|
||||
pose_T_world_cam: np.ndarray | None = None,
|
||||
seed: int = 42,
|
||||
) -> tuple[MatchResult, np.ndarray]:
|
||||
"""Build a MatchResult whose inliers reproject to a known camera pose.
|
||||
|
||||
Returns ``(match_result, pose_T_world_cam)``. The pose places the
|
||||
camera 100 m above the tile centre looking straight down
|
||||
(camera +Z axis aligned with world -Z, so world ground points
|
||||
end up in front of the camera in OpenCV's projection model).
|
||||
The world-point grid samples the tile uniformly.
|
||||
"""
|
||||
if pose_T_world_cam is None:
|
||||
# Camera looking straight down: flip Y and Z axes so that
|
||||
# world points at z=0 reproject in front of the camera
|
||||
# (camera-frame z > 0) per the OpenCV projection convention.
|
||||
pose_T_world_cam = np.array(
|
||||
[
|
||||
[1.0, 0.0, 0.0, 0.0],
|
||||
[0.0, -1.0, 0.0, 0.0],
|
||||
[0.0, 0.0, -1.0, 100.0],
|
||||
[0.0, 0.0, 0.0, 1.0],
|
||||
],
|
||||
dtype=np.float64,
|
||||
)
|
||||
|
||||
origin = LatLonAlt(
|
||||
lat_deg=_TEST_TILE_ID[1], lon_deg=_TEST_TILE_ID[2], alt_m=0.0
|
||||
)
|
||||
rng = np.random.default_rng(seed)
|
||||
tile_pixels = rng.uniform(40.0, 216.0, size=(num_inliers, 2))
|
||||
|
||||
world_pts = np.array(
|
||||
[
|
||||
_tile_pixel_to_enu(_TEST_TILE_ID, float(px), float(py), origin)
|
||||
for px, py in tile_pixels
|
||||
],
|
||||
dtype=np.float64,
|
||||
)
|
||||
|
||||
K = np.array(
|
||||
[[500.0, 0.0, 320.0], [0.0, 500.0, 240.0], [0.0, 0.0, 1.0]],
|
||||
dtype=np.float64,
|
||||
)
|
||||
T_cam_world = np.linalg.inv(pose_T_world_cam)
|
||||
rvec, _ = cv2.Rodrigues(T_cam_world[:3, :3])
|
||||
tvec = T_cam_world[:3, 3].reshape(3, 1)
|
||||
projected, _ = cv2.projectPoints(
|
||||
objectPoints=world_pts.reshape(-1, 1, 3),
|
||||
rvec=rvec,
|
||||
tvec=tvec,
|
||||
cameraMatrix=K,
|
||||
distCoeffs=np.zeros(5, dtype=np.float64),
|
||||
)
|
||||
image_pts = projected.reshape(-1, 2)
|
||||
|
||||
correspondences = np.hstack([image_pts, tile_pixels]).astype(np.float32)
|
||||
candidate = CandidateMatchSet(
|
||||
tile_id=_TEST_TILE_ID,
|
||||
inlier_count=num_inliers,
|
||||
inlier_correspondences=correspondences,
|
||||
ransac_outlier_count=0,
|
||||
per_candidate_residual_px=0.5,
|
||||
)
|
||||
match_result = MatchResult(
|
||||
frame_id=1,
|
||||
per_candidate=(candidate,),
|
||||
best_candidate_idx=0,
|
||||
reprojection_residual_px=0.5,
|
||||
matched_at=0,
|
||||
matcher_label="disk_lightglue",
|
||||
candidates_input=1,
|
||||
candidates_dropped=0,
|
||||
)
|
||||
return match_result, pose_T_world_cam
|
||||
|
||||
|
||||
def _build_estimator(
|
||||
*,
|
||||
handle: Any | None = None,
|
||||
fdr: Any | None = None,
|
||||
clock: Any | None = None,
|
||||
config: Config | None = None,
|
||||
) -> OpenCVGtsamPoseEstimator:
|
||||
if handle is None:
|
||||
handle = _RecordingISam2GraphHandle()
|
||||
if fdr is None:
|
||||
fdr = FakeFdrSink(producer_id="c4_pose")
|
||||
if clock is None:
|
||||
clock = _FakeClock()
|
||||
if config is None:
|
||||
config = _build_config()
|
||||
estimator = OpenCVGtsamPoseEstimator(
|
||||
config,
|
||||
ransac_filter=object(),
|
||||
wgs_converter=WgsConverter,
|
||||
se3_utils=object(),
|
||||
isam2_graph_handle=handle,
|
||||
fdr_client=fdr,
|
||||
clock=clock,
|
||||
logger=logging.getLogger("test.c4_pose"),
|
||||
)
|
||||
# Pre-seed the ENU origin to the tile centre so reconstructed
|
||||
# WGS84 positions match the synthetic ground truth exactly.
|
||||
estimator.set_enu_origin(
|
||||
LatLonAlt(lat_deg=_TEST_TILE_ID[1], lon_deg=_TEST_TILE_ID[2], alt_m=0.0)
|
||||
)
|
||||
return estimator
|
||||
|
||||
|
||||
@pytest.fixture(autouse=True)
|
||||
def _registry_isolation():
|
||||
clear_pose_registry()
|
||||
yield
|
||||
clear_pose_registry()
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AC-1: PnP success on synthetic correspondences.
|
||||
|
||||
|
||||
def test_az358_ac1_pnp_success_synthetic_within_tolerance() -> None:
|
||||
# Arrange
|
||||
match_result, pose_T_world_cam = _synthesise_match_result()
|
||||
estimator = _build_estimator()
|
||||
calibration = _build_calibration()
|
||||
thermal = _MutableThermalState().with_throttle(False)
|
||||
|
||||
# Act
|
||||
estimate = estimator.estimate(match_result, calibration, thermal)
|
||||
|
||||
# Assert — z component should be near 100 m (camera altitude).
|
||||
expected_alt = pose_T_world_cam[2, 3]
|
||||
assert abs(estimate.position_wgs84.alt_m - expected_alt) < 1.0
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AC-2: Degenerate geometry → PnpFailureError.
|
||||
|
||||
|
||||
def test_az358_ac2_insufficient_inliers_raises_pnp_failure(
|
||||
caplog: pytest.LogCaptureFixture,
|
||||
) -> None:
|
||||
# Arrange — only 3 inliers, IPPE needs >= 4.
|
||||
match_result, _ = _synthesise_match_result(num_inliers=3)
|
||||
fdr = FakeFdrSink(producer_id="c4_pose")
|
||||
estimator = _build_estimator(fdr=fdr)
|
||||
calibration = _build_calibration()
|
||||
thermal = _MutableThermalState().with_throttle(False)
|
||||
|
||||
# Act / Assert
|
||||
with caplog.at_level(logging.ERROR), pytest.raises(PnpFailureError):
|
||||
estimator.estimate(match_result, calibration, thermal)
|
||||
|
||||
error_records = [
|
||||
r for r in caplog.records if getattr(r, "kind", None) == "c4.pose.pnp_failure"
|
||||
]
|
||||
assert len(error_records) == 1
|
||||
|
||||
fdr_records = [r for r in fdr.records if r.kind == "pose.frame_done"]
|
||||
assert len(fdr_records) == 1
|
||||
assert fdr_records[0].payload.get("error") is True
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AC-3 + AC-4 + AC-5: SPD covariance, mode reporting, source label.
|
||||
|
||||
|
||||
def test_az358_ac3_4_5_marginals_success_invariants() -> None:
|
||||
# Arrange
|
||||
match_result, _ = _synthesise_match_result()
|
||||
estimator = _build_estimator()
|
||||
calibration = _build_calibration()
|
||||
thermal = _MutableThermalState().with_throttle(False)
|
||||
|
||||
# Act
|
||||
estimate = estimator.estimate(match_result, calibration, thermal)
|
||||
|
||||
# Assert — AC-3 SPD
|
||||
cov = estimate.covariance_6x6
|
||||
assert cov.shape == (6, 6)
|
||||
assert np.allclose(cov, cov.T, atol=1e-10)
|
||||
np.linalg.cholesky(cov)
|
||||
# AC-4 mode reporting
|
||||
assert estimate.covariance_mode is CovarianceMode.MARGINALS
|
||||
assert estimator.current_covariance_mode() is CovarianceMode.MARGINALS
|
||||
# AC-5 source label
|
||||
assert estimate.source_label is PoseSourceLabel.SATELLITE_ANCHORED
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AC-6: WGS84 conversion uses shared WgsConverter (verified by injection identity).
|
||||
|
||||
|
||||
def test_az358_ac6_wgs_converter_injection() -> None:
|
||||
# Arrange
|
||||
match_result, _ = _synthesise_match_result()
|
||||
estimator = _build_estimator()
|
||||
calibration = _build_calibration()
|
||||
thermal = _MutableThermalState().with_throttle(False)
|
||||
|
||||
# Act
|
||||
estimate = estimator.estimate(match_result, calibration, thermal)
|
||||
|
||||
# Assert — the converted WGS84 lat/lon should land inside the tile
|
||||
# bounds (since the camera is centred over the tile centre at
|
||||
# altitude 100 m and looking straight down, its WGS84 footprint
|
||||
# is the tile centre).
|
||||
tile_x, tile_y = WgsConverter.latlon_to_tile_xy(
|
||||
_TEST_TILE_ID[0], _TEST_TILE_ID[1], _TEST_TILE_ID[2]
|
||||
)
|
||||
bounds = WgsConverter.tile_xy_to_latlon_bounds(
|
||||
_TEST_TILE_ID[0], tile_x, tile_y
|
||||
)
|
||||
assert bounds.min_lat_deg <= estimate.position_wgs84.lat_deg <= bounds.max_lat_deg
|
||||
assert bounds.min_lon_deg <= estimate.position_wgs84.lon_deg <= bounds.max_lon_deg
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AC-7: iSAM2 handle call sequence.
|
||||
|
||||
|
||||
def test_az358_ac7_isam2_handle_call_sequence() -> None:
|
||||
# Arrange
|
||||
match_result, _ = _synthesise_match_result()
|
||||
handle = _RecordingISam2GraphHandle()
|
||||
estimator = _build_estimator(handle=handle)
|
||||
calibration = _build_calibration()
|
||||
thermal = _MutableThermalState().with_throttle(False)
|
||||
|
||||
# Act
|
||||
estimator.estimate(match_result, calibration, thermal)
|
||||
|
||||
# Assert — exactly one get_pose_key, one update, one compute_marginals.
|
||||
counts = {name: 0 for name in ("get_pose_key", "add_factor", "update", "compute_marginals")}
|
||||
for call in handle.calls:
|
||||
counts[call[0]] = counts.get(call[0], 0) + 1
|
||||
assert counts["get_pose_key"] == 1
|
||||
assert counts["update"] == 1
|
||||
assert counts["compute_marginals"] == 1
|
||||
# AC-7 invariant: C4 must NOT call add_factor (Option B —
|
||||
# factors travel via the local NonlinearFactorGraph passed to
|
||||
# update).
|
||||
assert counts["add_factor"] == 0
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AC-9: Non-SPD covariance defensive raise.
|
||||
|
||||
|
||||
def test_az358_ac9_non_spd_covariance_raises_pnp_failure(
|
||||
caplog: pytest.LogCaptureFixture,
|
||||
) -> None:
|
||||
# Arrange — handle returns a covariance that is symmetric but
|
||||
# has a negative eigenvalue so Cholesky fails. ``-0.01·I`` is
|
||||
# symmetric and non-SPD by construction.
|
||||
bad_cov = -np.eye(6, dtype=np.float64) * 0.01
|
||||
handle = _RecordingISam2GraphHandle(posterior_cov=bad_cov)
|
||||
fdr = FakeFdrSink(producer_id="c4_pose")
|
||||
match_result, _ = _synthesise_match_result()
|
||||
estimator = _build_estimator(handle=handle, fdr=fdr)
|
||||
calibration = _build_calibration()
|
||||
thermal = _MutableThermalState().with_throttle(False)
|
||||
|
||||
# Act / Assert
|
||||
with caplog.at_level(logging.ERROR), pytest.raises(
|
||||
PnpFailureError, match="non-SPD covariance"
|
||||
):
|
||||
estimator.estimate(match_result, calibration, thermal)
|
||||
|
||||
error_records = [
|
||||
r for r in caplog.records if getattr(r, "kind", None) == "c4.pose.pnp_failure"
|
||||
]
|
||||
assert any(rec.kv.get("stage") == "spd_check" for rec in error_records)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AC-10: Composition-root wiring (factory emits ready log + identity-shared deps).
|
||||
|
||||
|
||||
def test_az358_ac10_composition_root_wiring_emits_ready_log(
|
||||
caplog: pytest.LogCaptureFixture,
|
||||
) -> None:
|
||||
# Arrange
|
||||
cfg = _build_config()
|
||||
handle = _RecordingISam2GraphHandle()
|
||||
fdr = FakeFdrSink(producer_id="c4_pose")
|
||||
clock = _FakeClock()
|
||||
|
||||
# Act
|
||||
with caplog.at_level(logging.INFO):
|
||||
estimator = build_pose_estimator(
|
||||
cfg,
|
||||
ransac_filter=object(),
|
||||
wgs_converter=WgsConverter,
|
||||
se3_utils=object(),
|
||||
isam2_graph_handle=handle,
|
||||
fdr_client=fdr,
|
||||
clock=clock,
|
||||
)
|
||||
|
||||
# Assert
|
||||
assert isinstance(estimator, OpenCVGtsamPoseEstimator)
|
||||
ready_records = [
|
||||
r for r in caplog.records if getattr(r, "kind", None) == "c4.pose.ready"
|
||||
]
|
||||
assert len(ready_records) == 1
|
||||
assert ready_records[0].kv["strategy"] == "opencv_gtsam"
|
||||
assert (
|
||||
ready_records[0].kv["default_covariance"]
|
||||
== CovarianceMode.MARGINALS.value
|
||||
)
|
||||
|
||||
# Identity-shared: the estimator holds the SAME handle, fdr, clock instances.
|
||||
assert estimator._handle is handle # noqa: SLF001
|
||||
assert estimator._fdr_client is fdr # noqa: SLF001
|
||||
assert estimator._clock is clock # noqa: SLF001
|
||||
|
||||
|
||||
def test_az358_ac10_create_module_factory_returns_protocol_instance() -> None:
|
||||
# Arrange / Act
|
||||
cfg = _build_config()
|
||||
estimator = create(
|
||||
cfg,
|
||||
ransac_filter=object(),
|
||||
wgs_converter=WgsConverter,
|
||||
se3_utils=object(),
|
||||
isam2_graph_handle=_RecordingISam2GraphHandle(),
|
||||
)
|
||||
|
||||
# Assert
|
||||
assert isinstance(estimator, PoseEstimator)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AC-11: FDR pose.frame_done record shape.
|
||||
|
||||
|
||||
def test_az358_ac11_fdr_pose_frame_done_shape_on_success() -> None:
|
||||
# Arrange
|
||||
fdr = FakeFdrSink(producer_id="c4_pose")
|
||||
match_result, _ = _synthesise_match_result()
|
||||
estimator = _build_estimator(fdr=fdr)
|
||||
calibration = _build_calibration()
|
||||
thermal = _MutableThermalState().with_throttle(False)
|
||||
|
||||
# Act
|
||||
estimator.estimate(match_result, calibration, thermal)
|
||||
|
||||
# Assert
|
||||
records = [r for r in fdr.records if r.kind == "pose.frame_done"]
|
||||
assert len(records) == 1
|
||||
payload = records[0].payload
|
||||
expected_keys = {
|
||||
"frame_id",
|
||||
"inliers",
|
||||
"residual_px",
|
||||
"mode",
|
||||
"covariance_norm",
|
||||
"position_wgs84",
|
||||
}
|
||||
assert expected_keys <= set(payload.keys())
|
||||
assert payload["mode"] == "marginals"
|
||||
assert "error" not in payload # success path omits the flag
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AC-7-bonus: handle update receives the prior factor in the local graph.
|
||||
|
||||
|
||||
def test_az358_update_carries_prior_factor_in_local_graph() -> None:
|
||||
# Arrange
|
||||
match_result, _ = _synthesise_match_result()
|
||||
handle = _RecordingISam2GraphHandle()
|
||||
estimator = _build_estimator(handle=handle)
|
||||
calibration = _build_calibration()
|
||||
thermal = _MutableThermalState().with_throttle(False)
|
||||
|
||||
# Act
|
||||
estimator.estimate(match_result, calibration, thermal)
|
||||
|
||||
# Assert — extract the (graph, values, timestamps) tuple from
|
||||
# the update call and verify the graph carries exactly one
|
||||
# prior factor.
|
||||
update_calls = [c for c in handle.calls if c[0] == "update"]
|
||||
assert len(update_calls) == 1
|
||||
_, graph, _values, timestamps = update_calls[0]
|
||||
assert graph.size() == 1
|
||||
assert timestamps is None
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AZ-361 AC-1 / AC-2 / AC-4: per-frame mode dispatch + latency ≤ 1 frame.
|
||||
|
||||
|
||||
def test_az361_ac1_2_4_per_frame_mode_dispatch_alternates() -> None:
|
||||
# Arrange — alternate throttle bit each call; verify the mode
|
||||
# tracks the bit on EVERY call.
|
||||
estimator = _build_estimator()
|
||||
calibration = _build_calibration()
|
||||
thermal_factory = _MutableThermalState()
|
||||
|
||||
pattern = [False, True, False, True, False, True]
|
||||
observed: list[CovarianceMode] = []
|
||||
|
||||
# Act
|
||||
with warnings.catch_warnings():
|
||||
warnings.simplefilter("ignore", CovarianceDegradedWarning)
|
||||
for throttle in pattern:
|
||||
match_result, _ = _synthesise_match_result(seed=42)
|
||||
estimate = estimator.estimate(
|
||||
match_result, calibration, thermal_factory.with_throttle(throttle)
|
||||
)
|
||||
observed.append(estimate.covariance_mode)
|
||||
# AC-2 — switch-latency: the mode set on THIS call MUST
|
||||
# reflect THIS call's flag, not the previous one's.
|
||||
assert estimator.current_covariance_mode() is estimate.covariance_mode
|
||||
|
||||
# Assert AC-1 — every observed mode matches the expected
|
||||
# pattern (False→MARGINALS, True→JACOBIAN).
|
||||
expected = [
|
||||
CovarianceMode.JACOBIAN if t else CovarianceMode.MARGINALS for t in pattern
|
||||
]
|
||||
assert observed == expected
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AZ-361 AC-3 + AC-5: Jacobian SPD + source label.
|
||||
|
||||
|
||||
def test_az361_ac3_5_jacobian_success_invariants() -> None:
|
||||
# Arrange
|
||||
match_result, _ = _synthesise_match_result()
|
||||
estimator = _build_estimator()
|
||||
calibration = _build_calibration()
|
||||
thermal = _MutableThermalState().with_throttle(True)
|
||||
|
||||
# Act
|
||||
with warnings.catch_warnings():
|
||||
warnings.simplefilter("ignore", CovarianceDegradedWarning)
|
||||
estimate = estimator.estimate(match_result, calibration, thermal)
|
||||
|
||||
# Assert
|
||||
cov = estimate.covariance_6x6
|
||||
assert cov.shape == (6, 6)
|
||||
assert np.allclose(cov, cov.T, atol=1e-10)
|
||||
np.linalg.cholesky(cov)
|
||||
assert estimate.covariance_mode is CovarianceMode.JACOBIAN
|
||||
assert estimate.source_label is PoseSourceLabel.SATELLITE_ANCHORED
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AZ-361 AC-6: CovarianceDegradedWarning emitted via warnings.warn (not raised).
|
||||
|
||||
|
||||
def test_az361_ac6_warning_emitted_via_warnings_warn() -> None:
|
||||
# Arrange
|
||||
match_result, _ = _synthesise_match_result()
|
||||
# Disable rate-limiting so this single-call test is deterministic.
|
||||
estimator = _build_estimator(
|
||||
config=_build_config(covariance_degraded_warn_window_ns=0)
|
||||
)
|
||||
calibration = _build_calibration()
|
||||
thermal = _MutableThermalState().with_throttle(True)
|
||||
|
||||
# Act
|
||||
with warnings.catch_warnings(record=True) as records:
|
||||
warnings.simplefilter("always", CovarianceDegradedWarning)
|
||||
caught_via_exception = False
|
||||
try:
|
||||
estimator.estimate(match_result, calibration, thermal)
|
||||
except Exception:
|
||||
caught_via_exception = True
|
||||
|
||||
# Assert — warning emitted, not raised.
|
||||
assert caught_via_exception is False
|
||||
cov_warnings = [
|
||||
r for r in records if isinstance(r.message, CovarianceDegradedWarning)
|
||||
]
|
||||
assert len(cov_warnings) == 1
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AZ-361 AC-7 + AC-8: rate-limit warning + WARN log to ≤ 1 per window.
|
||||
|
||||
|
||||
def test_az361_ac7_8_warning_and_log_rate_limited(
|
||||
caplog: pytest.LogCaptureFixture,
|
||||
) -> None:
|
||||
# Arrange — 60 s window; advance the clock manually to step
|
||||
# past it. Run 5 throttled calls within window 0; expect 1
|
||||
# warning. Advance past 60 s; run 5 more; expect 1 more.
|
||||
cfg = _build_config(covariance_degraded_warn_window_ns=60_000_000_000)
|
||||
clock = _FakeClock()
|
||||
estimator = _build_estimator(clock=clock, config=cfg)
|
||||
calibration = _build_calibration()
|
||||
thermal = _MutableThermalState().with_throttle(True)
|
||||
|
||||
# Act
|
||||
all_warnings: list[Warning] = []
|
||||
with warnings.catch_warnings(record=True) as records:
|
||||
warnings.simplefilter("always", CovarianceDegradedWarning)
|
||||
with caplog.at_level(logging.WARNING):
|
||||
for i in range(5):
|
||||
clock.advance(100_000_000) # +0.1 s per call (still in window 1)
|
||||
match_result, _ = _synthesise_match_result()
|
||||
estimator.estimate(match_result, calibration, thermal)
|
||||
clock.advance(61_000_000_000) # +61 s — jump past the window
|
||||
for i in range(5):
|
||||
clock.advance(100_000_000)
|
||||
match_result, _ = _synthesise_match_result()
|
||||
estimator.estimate(match_result, calibration, thermal)
|
||||
all_warnings = [
|
||||
r.message for r in records if isinstance(r.message, CovarianceDegradedWarning)
|
||||
]
|
||||
|
||||
# Assert — exactly 2 warnings (1 per window) and exactly 2 WARN logs.
|
||||
assert len(all_warnings) == 2
|
||||
warn_records = [
|
||||
r for r in caplog.records if getattr(r, "kind", None) == "c4.pose.covariance_degraded"
|
||||
]
|
||||
assert len(warn_records) == 2
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AZ-361 AC-12: Jacobian path SKIPS iSAM2 factor add.
|
||||
|
||||
|
||||
def test_az361_ac12_jacobian_path_skips_isam2_update() -> None:
|
||||
# Arrange
|
||||
match_result, _ = _synthesise_match_result()
|
||||
handle = _RecordingISam2GraphHandle()
|
||||
estimator = _build_estimator(handle=handle)
|
||||
calibration = _build_calibration()
|
||||
thermal = _MutableThermalState().with_throttle(True)
|
||||
|
||||
# Act
|
||||
with warnings.catch_warnings():
|
||||
warnings.simplefilter("ignore", CovarianceDegradedWarning)
|
||||
estimator.estimate(match_result, calibration, thermal)
|
||||
|
||||
# Assert — NO update call, NO add_factor call, NO compute_marginals.
|
||||
# last_anchor_age_ms IS expected (it's read for the PoseEstimate field).
|
||||
counts = {name: 0 for name in ("update", "add_factor", "compute_marginals", "last_anchor_age_ms")}
|
||||
for call in handle.calls:
|
||||
counts[call[0]] = counts.get(call[0], 0) + 1
|
||||
assert counts["update"] == 0
|
||||
assert counts["add_factor"] == 0
|
||||
assert counts["compute_marginals"] == 0
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AZ-361 AC-13: FDR mode field distinguishes path.
|
||||
|
||||
|
||||
def test_az361_ac13_fdr_mode_field_distinguishes_path() -> None:
|
||||
# Arrange
|
||||
fdr = FakeFdrSink(producer_id="c4_pose")
|
||||
estimator = _build_estimator(fdr=fdr)
|
||||
calibration = _build_calibration()
|
||||
thermal_factory = _MutableThermalState()
|
||||
|
||||
# Act — one of each.
|
||||
match_result_a, _ = _synthesise_match_result(seed=1)
|
||||
estimator.estimate(match_result_a, calibration, thermal_factory.with_throttle(False))
|
||||
with warnings.catch_warnings():
|
||||
warnings.simplefilter("ignore", CovarianceDegradedWarning)
|
||||
match_result_b, _ = _synthesise_match_result(seed=2)
|
||||
estimator.estimate(match_result_b, calibration, thermal_factory.with_throttle(True))
|
||||
|
||||
# Assert
|
||||
frame_done_records = [r for r in fdr.records if r.kind == "pose.frame_done"]
|
||||
modes = [rec.payload["mode"] for rec in frame_done_records]
|
||||
assert "marginals" in modes
|
||||
assert "jacobian" in modes
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AZ-361 AC-10: Near-singular Jacobian → defensive raise.
|
||||
|
||||
|
||||
def test_az361_ac10_near_singular_jacobian_raises_pnp_failure(
|
||||
caplog: pytest.LogCaptureFixture,
|
||||
) -> None:
|
||||
# Arrange — use a config with a near-zero ridge AND a degenerate
|
||||
# all-coplanar inlier set whose Jacobian columns become
|
||||
# numerically rank-deficient. Force the ridge to a value that
|
||||
# is too small to rescue a singular JᵀJ.
|
||||
cfg = _build_config(
|
||||
covariance_degraded_warn_window_ns=0,
|
||||
ridge_regularisation_epsilon=1e-30,
|
||||
)
|
||||
match_result, _ = _synthesise_match_result()
|
||||
fdr = FakeFdrSink(producer_id="c4_pose")
|
||||
estimator = _build_estimator(config=cfg, fdr=fdr)
|
||||
calibration = _build_calibration()
|
||||
thermal = _MutableThermalState().with_throttle(True)
|
||||
|
||||
# Build a degenerate inlier set: all image points stacked at the
|
||||
# principal point, all world points along the optical axis. This
|
||||
# collapses the Jacobian's spatial columns to a single direction.
|
||||
K = np.asarray(calibration.intrinsics_3x3, dtype=np.float64)
|
||||
cx, cy = float(K[0, 2]), float(K[1, 2])
|
||||
num = 10
|
||||
image_pts = np.tile(np.array([cx, cy], dtype=np.float32), (num, 1))
|
||||
tile_pixels = np.tile(np.array([128.0, 128.0], dtype=np.float32), (num, 1))
|
||||
correspondences = np.hstack([image_pts, tile_pixels]).astype(np.float32)
|
||||
candidate = CandidateMatchSet(
|
||||
tile_id=_TEST_TILE_ID,
|
||||
inlier_count=num,
|
||||
inlier_correspondences=correspondences,
|
||||
ransac_outlier_count=0,
|
||||
per_candidate_residual_px=0.0,
|
||||
)
|
||||
degenerate = MatchResult(
|
||||
frame_id=1,
|
||||
per_candidate=(candidate,),
|
||||
best_candidate_idx=0,
|
||||
reprojection_residual_px=0.0,
|
||||
matched_at=0,
|
||||
matcher_label="disk_lightglue",
|
||||
candidates_input=1,
|
||||
candidates_dropped=0,
|
||||
)
|
||||
|
||||
# Act / Assert — either PnP rejects the degenerate input OR the
|
||||
# SPD check downstream raises with the documented message. Both
|
||||
# are acceptable per AC-10 (which says "defensive raise"); the
|
||||
# explicit AC-10 expectation is PnpFailureError no matter which
|
||||
# stage triggered.
|
||||
with caplog.at_level(logging.ERROR), pytest.raises(PnpFailureError):
|
||||
estimator.estimate(degenerate, calibration, thermal)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AZ-358 AC-8 replacement: thermal-throttle no longer raises NotImplementedError.
|
||||
|
||||
|
||||
def test_az358_ac8_replacement_throttle_now_runs_jacobian_path() -> None:
|
||||
# Arrange
|
||||
match_result, _ = _synthesise_match_result()
|
||||
estimator = _build_estimator()
|
||||
calibration = _build_calibration()
|
||||
thermal = _MutableThermalState().with_throttle(True)
|
||||
|
||||
# Act — should NOT raise NotImplementedError (AZ-361 replaced
|
||||
# the AZ-358 placeholder branch).
|
||||
with warnings.catch_warnings():
|
||||
warnings.simplefilter("ignore", CovarianceDegradedWarning)
|
||||
estimate = estimator.estimate(match_result, calibration, thermal)
|
||||
|
||||
# Assert
|
||||
assert estimate.covariance_mode is CovarianceMode.JACOBIAN
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# Bonus: emitted PoseEstimate.frame_id is a fresh UUID per call.
|
||||
|
||||
|
||||
def test_pose_estimate_frame_id_is_fresh_uuid() -> None:
|
||||
# Arrange
|
||||
estimator = _build_estimator()
|
||||
calibration = _build_calibration()
|
||||
thermal = _MutableThermalState().with_throttle(False)
|
||||
match_result_a, _ = _synthesise_match_result(seed=1)
|
||||
match_result_b, _ = _synthesise_match_result(seed=2)
|
||||
|
||||
# Act
|
||||
a = estimator.estimate(match_result_a, calibration, thermal)
|
||||
b = estimator.estimate(match_result_b, calibration, thermal)
|
||||
|
||||
# Assert
|
||||
assert isinstance(a.frame_id, UUID)
|
||||
assert isinstance(b.frame_id, UUID)
|
||||
assert a.frame_id != b.frame_id
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# Bonus: last_satellite_anchor_age_ms is sourced from the handle.
|
||||
|
||||
|
||||
def test_pose_estimate_last_anchor_age_ms_from_handle() -> None:
|
||||
# Arrange
|
||||
class _AgedHandle(_RecordingISam2GraphHandle):
|
||||
def last_anchor_age_ms(self) -> int:
|
||||
return 7500
|
||||
|
||||
handle = _AgedHandle()
|
||||
match_result, _ = _synthesise_match_result()
|
||||
estimator = _build_estimator(handle=handle)
|
||||
calibration = _build_calibration()
|
||||
thermal = _MutableThermalState().with_throttle(False)
|
||||
|
||||
# Act
|
||||
estimate = estimator.estimate(match_result, calibration, thermal)
|
||||
|
||||
# Assert
|
||||
assert estimate.last_satellite_anchor_age_ms == 7500
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# Bonus: PoseEstimate.emitted_at sources from the injected clock.
|
||||
|
||||
|
||||
def test_pose_estimate_emitted_at_from_clock() -> None:
|
||||
# Arrange
|
||||
clock = _FakeClock(start_ns=123_456_789)
|
||||
estimator = _build_estimator(clock=clock)
|
||||
calibration = _build_calibration()
|
||||
thermal = _MutableThermalState().with_throttle(False)
|
||||
match_result, _ = _synthesise_match_result()
|
||||
|
||||
# Act
|
||||
estimate = estimator.estimate(match_result, calibration, thermal)
|
||||
|
||||
# Assert
|
||||
assert estimate.emitted_at == 123_456_789
|
||||
@@ -351,6 +351,21 @@ def _kind_payload(kind: str) -> dict[str, object]:
|
||||
"inlier_count_before": 64,
|
||||
"inlier_count_after": 110,
|
||||
}
|
||||
if kind == "pose.frame_done":
|
||||
return {
|
||||
"frame_id": "00000000-0000-0000-0000-00000000abcd",
|
||||
"inliers": 62,
|
||||
"residual_px": 1.5,
|
||||
"mode": "marginals",
|
||||
"covariance_norm": 0.42,
|
||||
"position_wgs84": [49.5, 36.0, 120.0],
|
||||
}
|
||||
if kind == "pose.covariance_degraded":
|
||||
return {
|
||||
"frame_id": "00000000-0000-0000-0000-00000000beef",
|
||||
"thermal_throttle_active": True,
|
||||
"window_start_ns": 1_000_000_000,
|
||||
}
|
||||
raise AssertionError(f"unhandled kind in fixture: {kind!r}")
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user