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

[AZ-389] C5 orthorectifier emits mid-flight tiles to C6

Browse Source 
Adds an opt-in C5-internal orthorectifier (`_orthorectifier.py`) that
emits at most one tile-aligned JPEG candidate per nav frame to the
C6 `TileStore.write_tile` API.  Quality gates fire before any
OpenCV work: covariance Frobenius, inlier floor, source-label
(`SATELLITE_ANCHORED` only), and once-per-frame rate limit.

Cross-component import rule (AZ-507) is preserved: c5_state never
imports c6_tile_cache.  `runtime_root.state_factory` carries a new
`_C6MidFlightIngestAdapter` that builds the canonical
`TileMetadata` (`ONBOARD_INGEST` / `FRESH` / `PENDING`), hashes
the JPEG, and translates `FreshnessRejectionError` to a `None`
return so the orthorectifier silently swallows freshness
rejection per AC-NEW-3.

Wiring is opt-in via `C5StateConfig.orthorectifier.enabled`;
existing tests/binaries default to disabled and are unaffected.
Both `GtsamIsam2StateEstimator` and `EskfStateEstimator`
participate through new `attach_orthorectifier` /
`set_latest_nav_frame` extension methods (Protocol surface
unchanged).

Tests: 22 new unit tests cover AC-1..AC-9 plus inlier-floor
gate plus the composition-root adapter.  216/216 c5_state and
38/38 runtime-root + compose tests pass.

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Oleksandr Bezdieniezhnykh
2026-05-16 09:02:33 +03:00
parent 811ddc8aa7
commit c5ffc14fe9
9 changed files with 1952 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/gps_denied_onboard/components/c5_state/_orthorectifier.py
+598
View File
@@ -0,0 +1,598 @@
"""C5 internal orthorectifier sub-component (AZ-389).
Per epic spec § Scope: "orthorectifier (lives within C5 as an internal
subcomponent)". Receives a converged pose + nav-camera frame, gates on
quality thresholds (covariance, inlier count, source label), produces a
JPEG-encoded tile-aligned image patch via OpenCV ``warpPerspective``,
and persists it to C6 as an ``ONBOARD_INGEST`` tile via a
constructor-injected :class:`MidFlightTileWriter`.
Cross-component decoupling — c5_state cannot import from c6_tile_cache
(AZ-507; enforced by
``test_az270_compose_root.test_ac6_only_compose_root_imports_concrete_strategies``).
The :class:`MidFlightTileWriter` Protocol below is the consumer-side
cut; the runtime root supplies the concrete adapter via
``state_factory``. The writer translates a C6 ``FreshnessRejectionError``
into a ``None`` return — the orthorectifier swallows that silently
per AC-NEW-3 (opportunistic emission).
NOT exported from ``c5_state.__init__``; constructed inside the
estimator factory and observed only through the estimator's
``set_latest_nav_frame`` / ``current_estimate`` hook pair.
"""
from __future__ import annotations
import logging
from dataclasses import dataclass
from typing import TYPE_CHECKING, Any, Final, Protocol, runtime_checkable
import cv2
import numpy as np
from gps_denied_onboard._types.state import PoseSourceLabel
if TYPE_CHECKING:
from datetime import datetime
from gps_denied_onboard._types.calibration import CameraCalibration
from gps_denied_onboard._types.nav import NavCameraFrame
__all__ = [
"MidFlightTileWriter",
"Orthorectifier",
"OrthorectifierThresholds",
]
_DEFAULT_JPEG_QUALITY: Final[int] = 90
_DEFAULT_TILE_SIZE_PIXELS: Final[int] = 256
_DEFAULT_TILE_SIZE_METERS: Final[float] = 100.0
_DEFAULT_ZOOM_LEVEL: Final[int] = 18
_LOG = logging.getLogger("gps_denied_onboard.c5_state.orthorectifier")
@runtime_checkable
class MidFlightTileWriter(Protocol):
"""Consumer-side cut of the C6 mid-flight tile write surface.
The concrete adapter lives in
``gps_denied_onboard.runtime_root.state_factory`` and is the only
place that knows about C6's :class:`TileMetadata` / :class:`TileSource`
/ :class:`FreshnessLabel` / :class:`VotingStatus` /
:class:`FreshnessRejectionError`. C5's orthorectifier never imports
them (AZ-507 architectural rule).
Returning ``None`` indicates the C6 freshness gate rejected the
insert. Per AC-NEW-3 the orthorectifier swallows that silently —
mid-flight tile generation is opportunistic. Returning the
``(zoom_level, lat, lon)`` tuple confirms persistence and is the
tile's spatial identity.
"""
def write_mid_flight_tile(
self,
*,
jpeg_bytes: bytes,
zoom_level: int,
lat: float,
lon: float,
tile_size_meters: float,
tile_size_pixels: int,
capture_timestamp: "datetime",
flight_id: str,
companion_id: str,
estimator_label: str,
covariance_2x2_horizontal: tuple[
tuple[float, float], tuple[float, float]
],
last_anchor_age_ms: int,
mre_px: float,
imu_bias_norm: float,
) -> tuple[int, float, float] | None: ...
@dataclass(frozen=True, slots=True)
class OrthorectifierThresholds:
"""Quality + output-grid configuration for :class:`Orthorectifier`.
``cov_norm_threshold`` is the Frobenius-norm gate on the C5 6x6
pose covariance. ``inlier_floor`` is the strict-greater-than
threshold on the C4 reported inlier count. ``tile_size_meters``
and ``tile_size_pixels`` describe the orthorectified output patch
grid; ``zoom_level`` is the satellite-tile zoom the C6 row will be
indexed at. ``jpeg_quality`` is the OpenCV ``cv2.imencode``
quality knob (0-100; default 90 to balance fidelity and budget).
"""
cov_norm_threshold: float
inlier_floor: int
tile_size_meters: float = _DEFAULT_TILE_SIZE_METERS
tile_size_pixels: int = _DEFAULT_TILE_SIZE_PIXELS
zoom_level: int = _DEFAULT_ZOOM_LEVEL
jpeg_quality: int = _DEFAULT_JPEG_QUALITY
def __post_init__(self) -> None:
if not self.cov_norm_threshold > 0.0:
raise ValueError(
f"OrthorectifierThresholds.cov_norm_threshold must be > 0; "
f"got {self.cov_norm_threshold}"
)
if self.inlier_floor < 0:
raise ValueError(
f"OrthorectifierThresholds.inlier_floor must be >= 0; "
f"got {self.inlier_floor}"
)
if not self.tile_size_meters > 0.0:
raise ValueError(
f"OrthorectifierThresholds.tile_size_meters must be > 0; "
f"got {self.tile_size_meters}"
)
if self.tile_size_pixels <= 0:
raise ValueError(
f"OrthorectifierThresholds.tile_size_pixels must be > 0; "
f"got {self.tile_size_pixels}"
)
if not 0 <= self.zoom_level <= 21:
raise ValueError(
f"OrthorectifierThresholds.zoom_level must be in [0, 21]; "
f"got {self.zoom_level}"
)
if not 0 <= self.jpeg_quality <= 100:
raise ValueError(
f"OrthorectifierThresholds.jpeg_quality must be in [0, 100]; "
f"got {self.jpeg_quality}"
)
class Orthorectifier:
"""C5-internal sub-component — emits at most one mid-flight tile per frame.
All logic is single-threaded by construction (state-ingest thread per
Invariant 1). The orthorectifier is opaque to test wiring: tests
construct it with a fake :class:`MidFlightTileWriter` and a synthetic
:class:`CameraCalibration`, then call :meth:`try_emit_candidate`
directly with crafted ``(frame, pose, cov_6x6, ...)`` tuples.
Quality gates (AC-2 / AC-3 / AC-4 / AC-8) run before any heavy
OpenCV work. The orthorectification kernel (AC-1) computes the
ground→image plane homography from the pose + calibration and
delegates sampling to ``cv2.warpPerspective``.
Per AC-NEW-3 (replay protocol Invariant 12) the emission is
opportunistic — a freshness rejection from C6 is swallowed (AC-9)
and a DEBUG log emitted. Hard failures inside the OpenCV kernel
are also swallowed and logged at WARNING (the steady-state
estimate is the priority; mid-flight tile gen is best-effort).
"""
__slots__ = (
"_tile_writer",
"_thresholds",
"_calibration",
"_flight_id",
"_companion_id",
"_first_emission_done",
"_last_processed_frame_id",
)
def __init__(
self,
*,
tile_writer: MidFlightTileWriter,
thresholds: OrthorectifierThresholds,
camera_calibration: "CameraCalibration",
flight_id: str,
companion_id: str,
) -> None:
self._tile_writer = tile_writer
self._thresholds = thresholds
self._calibration = camera_calibration
self._flight_id = flight_id
self._companion_id = companion_id
self._first_emission_done: bool = False
self._last_processed_frame_id: int | None = None
def try_emit_candidate(
self,
*,
frame: "NavCameraFrame | None",
pose_estimate: Any,
cov_6x6: np.ndarray | None,
inlier_count: int,
mre_px: float,
source_label: PoseSourceLabel,
last_anchor_age_ms: int,
imu_bias_norm: float,
estimator_label: str,
) -> tuple[int, float, float] | None:
"""Run quality gates → orthorectify → write. Returns persisted ``(zoom, lat, lon)`` or ``None``.
Returns ``None`` (silently) when ANY of the following hold:
* AC-8 — ``frame`` or ``pose_estimate`` or ``cov_6x6`` is ``None``;
* AC-4 — ``frame.frame_id`` was already processed in a prior call;
* AC-3 — ``source_label != SATELLITE_ANCHORED``;
* AC-2 — Frobenius norm of ``cov_6x6`` >= ``cov_norm_threshold``;
* inlier-floor — ``inlier_count <= inlier_floor``;
* AC-9 — :class:`MidFlightTileWriter` returned ``None``
(freshness rejection — DEBUG log only);
* orthorectification kernel raised (WARNING log; not propagated).
"""
if frame is None or pose_estimate is None or cov_6x6 is None:
_LOG.debug(
"c5.state.orthorectifier_skip_missing_input",
extra={
"kind": "c5.state.orthorectifier_skip_missing_input",
"kv": {
"frame_present": frame is not None,
"pose_present": pose_estimate is not None,
"cov_present": cov_6x6 is not None,
},
},
)
return None
if (
self._last_processed_frame_id is not None
and self._last_processed_frame_id == int(frame.frame_id)
):
return None
self._last_processed_frame_id = int(frame.frame_id)
if source_label != PoseSourceLabel.SATELLITE_ANCHORED:
_LOG.debug(
"c5.state.orthorectifier_skip_source_label",
extra={
"kind": "c5.state.orthorectifier_skip_source_label",
"kv": {
"frame_id": int(frame.frame_id),
"source_label": source_label.value,
},
},
)
return None
cov_norm = float(np.linalg.norm(np.asarray(cov_6x6), ord="fro"))
if cov_norm >= self._thresholds.cov_norm_threshold:
_LOG.debug(
"c5.state.orthorectifier_skip_cov_gate",
extra={
"kind": "c5.state.orthorectifier_skip_cov_gate",
"kv": {
"frame_id": int(frame.frame_id),
"cov_norm": cov_norm,
"threshold": self._thresholds.cov_norm_threshold,
},
},
)
return None
if inlier_count <= self._thresholds.inlier_floor:
_LOG.debug(
"c5.state.orthorectifier_skip_inlier_gate",
extra={
"kind": "c5.state.orthorectifier_skip_inlier_gate",
"kv": {
"frame_id": int(frame.frame_id),
"inlier_count": int(inlier_count),
"floor": self._thresholds.inlier_floor,
},
},
)
return None
try:
jpeg_bytes = self._orthorectify_to_jpeg(frame, pose_estimate)
except Exception as exc:
# AC-NEW-3 opportunistic emission — log + return; do not
# propagate to the C5 estimator's caller.
_LOG.warning(
"c5.state.orthorectifier_kernel_failed",
extra={
"kind": "c5.state.orthorectifier_kernel_failed",
"kv": {
"frame_id": int(frame.frame_id),
"error": str(exc),
"error_type": type(exc).__name__,
},
},
)
return None
if jpeg_bytes is None or len(jpeg_bytes) == 0:
_LOG.warning(
"c5.state.orthorectifier_empty_jpeg",
extra={
"kind": "c5.state.orthorectifier_empty_jpeg",
"kv": {"frame_id": int(frame.frame_id)},
},
)
return None
cov_2x2_h = self._extract_horizontal_covariance(np.asarray(cov_6x6))
try:
result = self._tile_writer.write_mid_flight_tile(
jpeg_bytes=jpeg_bytes,
zoom_level=int(self._thresholds.zoom_level),
lat=float(pose_estimate.position_wgs84.lat_deg),
lon=float(pose_estimate.position_wgs84.lon_deg),
tile_size_meters=float(self._thresholds.tile_size_meters),
tile_size_pixels=int(self._thresholds.tile_size_pixels),
capture_timestamp=frame.timestamp,
flight_id=self._flight_id,
companion_id=self._companion_id,
estimator_label=estimator_label,
covariance_2x2_horizontal=cov_2x2_h,
last_anchor_age_ms=int(last_anchor_age_ms),
mre_px=float(mre_px),
imu_bias_norm=float(imu_bias_norm),
)
except Exception as exc:
# The writer is responsible for catching the C6
# FreshnessRejectionError family and translating it to None.
# Anything else escaping is an infrastructure failure — log
# at WARNING but do not propagate (mid-flight tile gen is
# best-effort and must never break the steady-state
# current_estimate path).
_LOG.warning(
"c5.state.orthorectifier_writer_failed",
extra={
"kind": "c5.state.orthorectifier_writer_failed",
"kv": {
"frame_id": int(frame.frame_id),
"error": str(exc),
"error_type": type(exc).__name__,
},
},
)
return None
if result is None:
_LOG.debug(
"c5.state.mid_flight_candidate_freshness_rejected",
extra={
"kind": "c5.state.mid_flight_candidate_freshness_rejected",
"kv": {
"frame_id": int(frame.frame_id),
"lat": float(pose_estimate.position_wgs84.lat_deg),
"lon": float(pose_estimate.position_wgs84.lon_deg),
},
},
)
return None
if not self._first_emission_done:
self._first_emission_done = True
_LOG.info(
"c5.state.first_mid_flight_candidate",
extra={
"kind": "c5.state.first_mid_flight_candidate",
"kv": {
"frame_id": int(frame.frame_id),
"tile_id": list(result),
"cov_norm": cov_norm,
},
},
)
else:
_LOG.debug(
"c5.state.mid_flight_candidate_emitted",
extra={
"kind": "c5.state.mid_flight_candidate_emitted",
"kv": {
"frame_id": int(frame.frame_id),
"tile_id": list(result),
"cov_norm": cov_norm,
"inlier_count": int(inlier_count),
},
},
)
return result
def _orthorectify_to_jpeg(
self, frame: "NavCameraFrame", pose: Any
) -> bytes:
image = self._coerce_image(frame.image)
K = self._coerce_intrinsics(self._calibration.intrinsics_3x3)
body_T_cam = self._coerce_se3(self._calibration.body_to_camera_se3)
world_T_body = self._world_T_body_from_pose(pose)
world_T_cam = world_T_body @ body_T_cam
cam_T_world = _invert_se3(world_T_cam)
H_ground_to_image = _ground_plane_homography(K=K, cam_T_world=cam_T_world)
H_tile_to_image = _compose_tile_to_image_homography(
H_ground_to_image=H_ground_to_image,
ground_x_center=float(world_T_cam[0, 3]),
ground_y_center=float(world_T_cam[1, 3]),
tile_size_meters=float(self._thresholds.tile_size_meters),
tile_size_pixels=int(self._thresholds.tile_size_pixels),
)
N = int(self._thresholds.tile_size_pixels)
warped = cv2.warpPerspective(
image,
H_tile_to_image,
(N, N),
flags=cv2.INTER_LINEAR | cv2.WARP_INVERSE_MAP,
borderMode=cv2.BORDER_CONSTANT,
borderValue=0,
)
encode_params = [
int(cv2.IMWRITE_JPEG_QUALITY),
int(self._thresholds.jpeg_quality),
]
success, buffer = cv2.imencode(".jpg", warped, encode_params)
if not success:
raise RuntimeError("cv2.imencode(.jpg) returned failure")
return bytes(buffer.tobytes())
@staticmethod
def _extract_horizontal_covariance(
cov_6x6: np.ndarray,
) -> tuple[tuple[float, float], tuple[float, float]]:
"""Pull the horizontal-translation 2x2 sub-block.
The C5 6x6 covariance is documented as rotation-then-translation
(rxx, ryy, rzz, txx, tyy, tzz) in the GTSAM tangent-space order
(matching ``VioOutput.pose_covariance_6x6``). Indices [3, 4] are
the East / North translation components — the horizontal sub-block
the C6 voting layer consumes (per AZ-389 § Constraints).
"""
if cov_6x6.shape != (6, 6):
raise ValueError(
f"cov_6x6 must be shape (6, 6); got {cov_6x6.shape}"
)
sub = cov_6x6[3:5, 3:5]
return (
(float(sub[0, 0]), float(sub[0, 1])),
(float(sub[1, 0]), float(sub[1, 1])),
)
@staticmethod
def _coerce_image(image: Any) -> np.ndarray:
arr = np.asarray(image)
if arr.ndim not in (2, 3):
raise ValueError(
f"NavCameraFrame.image must be 2-D (grayscale) or 3-D "
f"(H, W, C); got ndim={arr.ndim}"
)
return arr
@staticmethod
def _coerce_intrinsics(k: Any) -> np.ndarray:
arr = np.asarray(k, dtype=np.float64)
if arr.shape != (3, 3):
raise ValueError(
f"intrinsics_3x3 must be shape (3, 3); got {arr.shape}"
)
return arr
@staticmethod
def _coerce_se3(se3: Any) -> np.ndarray:
arr = np.asarray(se3, dtype=np.float64)
if arr.shape == (4, 4):
return arr
# Some helper SE3 wrappers expose ``.matrix()``; try that.
if hasattr(se3, "matrix"):
arr = np.asarray(se3.matrix(), dtype=np.float64)
if arr.shape == (4, 4):
return arr
raise ValueError(
f"body_to_camera_se3 must be a 4x4 array or expose .matrix(); "
f"got shape={arr.shape}"
)
@staticmethod
def _world_T_body_from_pose(pose: Any) -> np.ndarray:
"""Build a 4x4 homogeneous ``world_T_body`` from a :class:`PoseEstimate`.
The pose's ``position_wgs84`` is collapsed to the local-ENU
plane anchored at the camera's own lat/lon — i.e. the camera
is at world-origin in the orthorectifier's ENU frame. The
ground plane is at ``Z = -altitude`` so the homography sees a
non-degenerate plane separation. This keeps the tile-relative
math agnostic to the global ENU origin and avoids re-injecting
the ``WgsConverter`` at this layer; the persisted tile_id lat
/ lon comes directly from the pose's ``position_wgs84``.
"""
R = _quat_to_rotation_matrix(pose.orientation_world_T_body)
T = np.eye(4, dtype=np.float64)
T[:3, :3] = R
# Camera body translation: at world origin in the local-ENU
# working frame (East=0, North=0). Altitude-above-ground is
# the only dimension that matters for the homography.
T[0, 3] = 0.0
T[1, 3] = 0.0
T[2, 3] = float(pose.position_wgs84.alt_m)
return T
def _quat_to_rotation_matrix(q: Any) -> np.ndarray:
"""Convert a scalar-first :class:`Quat` ``(w, x, y, z)`` to a 3x3 rotation."""
w = float(q.w)
x = float(q.x)
y = float(q.y)
z = float(q.z)
norm = (w * w + x * x + y * y + z * z) ** 0.5
if norm <= 0.0:
raise ValueError("Quat has zero norm; cannot convert to rotation")
w /= norm
x /= norm
y /= norm
z /= norm
return np.array(
[
[1.0 - 2.0 * (y * y + z * z), 2.0 * (x * y - z * w), 2.0 * (x * z + y * w)],
[2.0 * (x * y + z * w), 1.0 - 2.0 * (x * x + z * z), 2.0 * (y * z - x * w)],
[2.0 * (x * z - y * w), 2.0 * (y * z + x * w), 1.0 - 2.0 * (x * x + y * y)],
],
dtype=np.float64,
)
def _invert_se3(T: np.ndarray) -> np.ndarray:
"""Invert a 4x4 rigid SE(3) transform analytically."""
if T.shape != (4, 4):
raise ValueError(f"_invert_se3: expected (4, 4); got {T.shape}")
R = T[:3, :3]
t = T[:3, 3]
inv = np.eye(4, dtype=np.float64)
inv[:3, :3] = R.T
inv[:3, 3] = -R.T @ t
return inv
def _ground_plane_homography(
*, K: np.ndarray, cam_T_world: np.ndarray
) -> np.ndarray:
"""Return the 3x3 homography mapping ground-plane points ``(X, Y, 1)`` (world ENU, ``Z = 0``) to image pixels ``(u, v, 1)``.
Construction:
.. code:: text
[u] [X]
[v] = K @ [r1 r2 t] @ [Y]
[w] [1]
where ``[r1 | r2 | r3 | t]`` is ``cam_T_world`` and ``r1``,
``r2`` are its first two columns. The third column ``r3`` is
dropped because the ground plane fixes ``Z = 0`` (so the
homography is full-rank in 2D).
"""
R = cam_T_world[:3, :3]
t = cam_T_world[:3, 3]
return K @ np.column_stack([R[:, 0], R[:, 1], t])
def _compose_tile_to_image_homography(
*,
H_ground_to_image: np.ndarray,
ground_x_center: float,
ground_y_center: float,
tile_size_meters: float,
tile_size_pixels: int,
) -> np.ndarray:
"""Build ``H_tile_to_image = H_ground_to_image @ M_tile_to_ground``.
``M_tile_to_ground`` maps tile pixel ``(j, i, 1)`` (image-y-down
convention) to ground ``(X, Y, 1)`` (world Y is North, image i is
rows-down). Centred at ``(ground_x_center, ground_y_center)`` so
the camera's nadir lands at the tile centre.
"""
s = float(tile_size_meters) / float(tile_size_pixels)
n = int(tile_size_pixels)
M = np.array(
[
[s, 0.0, ground_x_center - s * (n / 2.0)],
[0.0, -s, ground_y_center + s * (n / 2.0)],
[0.0, 0.0, 1.0],
],
dtype=np.float64,
)
return H_ground_to_image @ M
src/gps_denied_onboard/components/c5_state/config.py
+61 -2
View File
@@ -9,12 +9,16 @@ root reads ``config.components["c5_state"]`` and dispatches by
from __future__ import annotations
from dataclasses import dataclass
from dataclasses import dataclass, field
from typing import Final
from gps_denied_onboard.config.schema import ConfigError
__all__ = ["KNOWN_STATE_STRATEGIES", "C5StateConfig"]
__all__ = [
"KNOWN_STATE_STRATEGIES",
"C5StateConfig",
"OrthorectifierConfig",
]
KNOWN_STATE_STRATEGIES: Final[frozenset[str]] = frozenset({"gtsam_isam2", "eskf"})
@@ -23,6 +27,60 @@ _KEYFRAME_WINDOW_MIN: Final[int] = 10
_KEYFRAME_WINDOW_MAX: Final[int] = 20
@dataclass(frozen=True)
class OrthorectifierConfig:
"""AZ-389 orthorectifier knobs (sub-block of :class:`C5StateConfig`).
``enabled`` is the main feature flag. When ``False`` the runtime
root skips orthorectifier construction entirely and the estimator
behaves exactly as before AZ-389; this preserves the existing
smoke-test wiring that does not provide a TileStore. The four
quality / output knobs match
:class:`gps_denied_onboard.components.c5_state._orthorectifier.OrthorectifierThresholds`
one-for-one.
"""
enabled: bool = False
cov_norm_threshold: float = 1.0
inlier_floor: int = 30
tile_size_meters: float = 100.0
tile_size_pixels: int = 256
zoom_level: int = 18
jpeg_quality: int = 90
def __post_init__(self) -> None:
if not self.cov_norm_threshold > 0.0:
raise ConfigError(
"OrthorectifierConfig.cov_norm_threshold must be > 0; "
f"got {self.cov_norm_threshold}"
)
if self.inlier_floor < 0:
raise ConfigError(
"OrthorectifierConfig.inlier_floor must be >= 0; "
f"got {self.inlier_floor}"
)
if not self.tile_size_meters > 0.0:
raise ConfigError(
"OrthorectifierConfig.tile_size_meters must be > 0; "
f"got {self.tile_size_meters}"
)
if self.tile_size_pixels <= 0:
raise ConfigError(
"OrthorectifierConfig.tile_size_pixels must be > 0; "
f"got {self.tile_size_pixels}"
)
if not 0 <= self.zoom_level <= 21:
raise ConfigError(
"OrthorectifierConfig.zoom_level must be in [0, 21]; "
f"got {self.zoom_level}"
)
if not 0 <= self.jpeg_quality <= 100:
raise ConfigError(
"OrthorectifierConfig.jpeg_quality must be in [0, 100]; "
f"got {self.jpeg_quality}"
)
@dataclass(frozen=True)
class C5StateConfig:
"""C5 state-estimator config block.
@@ -58,6 +116,7 @@ class C5StateConfig:
default_takeoff_origin_sigma_horiz_m: float = 5.0
default_takeoff_origin_sigma_vert_m: float = 10.0
no_estimate_fallback_s: float = 3.0
orthorectifier: OrthorectifierConfig = field(default_factory=OrthorectifierConfig)
def __post_init__(self) -> None:
if self.strategy not in KNOWN_STATE_STRATEGIES:
src/gps_denied_onboard/components/c5_state/eskf_baseline.py
+130 -1
View File
@@ -75,6 +75,11 @@ from gps_denied_onboard.components.c5_state._source_label_sm import (
RejectionSubscription,
SourceLabelStateMachine,
)
from gps_denied_onboard.components.c5_state._orthorectifier import (
MidFlightTileWriter,
Orthorectifier,
OrthorectifierThresholds,
)
from gps_denied_onboard.components.c5_state.config import C5StateConfig
from gps_denied_onboard.components.c5_state.errors import (
EstimatorAlreadyStartedError,
@@ -88,8 +93,9 @@ 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.fc import GpsHealth, GpsSample
from gps_denied_onboard._types.nav import ImuWindow, VioOutput
from gps_denied_onboard._types.nav import ImuWindow, NavCameraFrame, VioOutput
from gps_denied_onboard._types.pose import PoseEstimate
from gps_denied_onboard.clock import Clock
from gps_denied_onboard.config import Config
@@ -229,6 +235,17 @@ class EskfStateEstimator(StateEstimator):
producer_id="c5_state",
)
# AZ-389: orthorectifier hook. Same semantics as the iSAM2
# estimator — wired by the composition root only when AZ-389
# is enabled in config + the runtime root has a TileStore.
# ``_latest_vio`` is the source of inlier_count / mre_px /
# imu_bias_norm metadata for ``try_emit_candidate``; ESKF
# historically did NOT cache the full DTO so we keep a
# dedicated field rather than abuse ``_prev_vio_*``.
self._orthorectifier: Orthorectifier | None = None
self._latest_nav_frame: "NavCameraFrame | None" = None
self._latest_vio: "VioOutput | None" = None
self._log.debug(
"c5.state.eskf_initialised",
extra={
@@ -269,6 +286,25 @@ class EskfStateEstimator(StateEstimator):
def attach_source_label_state_machine(self, machine: Any) -> None:
self._source_label_machine = machine
def attach_orthorectifier(self, orthorectifier: Orthorectifier) -> None:
"""AZ-389 — wire the mid-flight tile orthorectifier.
Optional; called by the composition root only when AZ-389 is
enabled in config. See
:meth:`GtsamIsam2StateEstimator.attach_orthorectifier` for the
cross-estimator contract.
"""
self._orthorectifier = orthorectifier
def set_latest_nav_frame(self, frame: "NavCameraFrame") -> None:
"""AZ-389 — buffer the most recent nav-camera frame.
Quiet no-op when no orthorectifier is attached. See
:meth:`GtsamIsam2StateEstimator.set_latest_nav_frame` for the
cross-estimator contract.
"""
self._latest_nav_frame = frame
def notify_gps_health(self, gps_health: GpsHealth, now_ns: int | None = None) -> None:
machine = self._source_label_machine
if isinstance(machine, SourceLabelStateMachine):
@@ -455,6 +491,8 @@ class EskfStateEstimator(StateEstimator):
self._close_cold_start_window()
ts_ns = vio.emitted_at_ns
self._guard_timestamp(ts_ns, source="vio")
# AZ-389: cache full DTO for the orthorectifier metadata path.
self._latest_vio = vio
curr_pose = vio.relative_pose_T.matrix()
if self._prev_vio_pose is None:
@@ -656,8 +694,50 @@ class EskfStateEstimator(StateEstimator):
)
self._history.append(out)
self._fallback.mark_successful_estimate(emitted_at)
# AZ-389: opportunistic mid-flight tile emission.
self._maybe_emit_mid_flight_tile(
output=out,
covariance=cov6,
last_anchor_age_ms=last_anchor_age_ms,
)
return out
def _maybe_emit_mid_flight_tile(
self,
*,
output: EstimatorOutput,
covariance: np.ndarray,
last_anchor_age_ms: int,
) -> None:
"""AZ-389 hook — invoke the orthorectifier when wired."""
orthorectifier = self._orthorectifier
frame = self._latest_nav_frame
if orthorectifier is None or frame is None:
return
latest_vio = self._latest_vio
if latest_vio is None:
inlier_count = 0
mre_px = float("inf")
imu_bias_norm = 0.0
else:
quality = latest_vio.feature_quality
inlier_count = int(quality.tracked)
mre_px = float(quality.mre_px)
imu_bias_norm = _imu_bias_norm(latest_vio.imu_bias)
orthorectifier.try_emit_candidate(
frame=frame,
pose_estimate=output,
cov_6x6=covariance,
inlier_count=inlier_count,
mre_px=mre_px,
source_label=output.source_label,
last_anchor_age_ms=int(last_anchor_age_ms),
imu_bias_norm=imu_bias_norm,
estimator_label=_STRATEGY,
)
def smoothed_history(self, n_keyframes: int) -> list[EstimatorOutput]:
"""Return up to ``n_keyframes`` recent forward-time entries.
@@ -910,11 +990,22 @@ def create(
se3_utils: Any,
wgs_converter: Any,
fdr_client: Any,
mid_flight_tile_writer: MidFlightTileWriter | None = None,
camera_calibration: "CameraCalibration | None" = None,
flight_id: str | None = None,
companion_id: str | None = None,
) -> tuple[StateEstimator, Any]:
"""Composition-root factory for the ESKF estimator.
Returns ``(estimator, handle)`` to match the iSAM2 factory shape;
the ESKF returns ``handle=None`` because it has no GTSAM graph.
AZ-389 wiring (optional): same contract as the iSAM2 factory —
when ``mid_flight_tile_writer`` is supplied AND
``config.components['c5_state'].orthorectifier.enabled`` is true,
the orthorectifier is constructed and attached. Missing
``camera_calibration`` / ``flight_id`` / ``companion_id`` while
enabled raises :class:`StateEstimatorConfigError`.
"""
estimator = EskfStateEstimator(
config,
@@ -923,6 +1014,31 @@ def create(
wgs_converter=wgs_converter,
fdr_client=fdr_client,
)
block: C5StateConfig = estimator._block
if mid_flight_tile_writer is not None and block.orthorectifier.enabled:
if camera_calibration is None or flight_id is None or companion_id is None:
raise StateEstimatorConfigError(
"AZ-389 orthorectifier requires camera_calibration + "
"flight_id + companion_id alongside mid_flight_tile_writer"
)
thresholds = OrthorectifierThresholds(
cov_norm_threshold=float(block.orthorectifier.cov_norm_threshold),
inlier_floor=int(block.orthorectifier.inlier_floor),
tile_size_meters=float(block.orthorectifier.tile_size_meters),
tile_size_pixels=int(block.orthorectifier.tile_size_pixels),
zoom_level=int(block.orthorectifier.zoom_level),
jpeg_quality=int(block.orthorectifier.jpeg_quality),
)
orthorectifier = Orthorectifier(
tile_writer=mid_flight_tile_writer,
thresholds=thresholds,
camera_calibration=camera_calibration,
flight_id=flight_id,
companion_id=companion_id,
)
estimator.attach_orthorectifier(orthorectifier)
return estimator, None
@@ -937,6 +1053,19 @@ def register() -> None:
# Module-level math + DTO helpers (kept local; small and self-contained).
def _imu_bias_norm(bias: Any) -> float:
"""Return ``sqrt(||accel_bias||^2 + ||gyro_bias||^2)`` for a :class:`ImuBias`.
Mirrors the iSAM2 helper; AZ-389 quality-metadata path. Robust to
absent / ``None`` components (legacy fixtures) by treating them
as zero.
"""
accel = getattr(bias, "accel_bias", (0.0, 0.0, 0.0)) or (0.0, 0.0, 0.0)
gyro = getattr(bias, "gyro_bias", (0.0, 0.0, 0.0)) or (0.0, 0.0, 0.0)
sq = sum(float(c) ** 2 for c in accel) + sum(float(c) ** 2 for c in gyro)
return sq**0.5
def _datetime_to_ns(dt: Any) -> int:
return int(dt.timestamp() * 1_000_000_000)
src/gps_denied_onboard/components/c5_state/gtsam_isam2_estimator.py
+141 -2
View File
@@ -65,6 +65,11 @@ from gps_denied_onboard.components.c5_state._source_label_sm import (
RejectionSubscription,
SourceLabelStateMachine,
)
from gps_denied_onboard.components.c5_state._orthorectifier import (
MidFlightTileWriter,
Orthorectifier,
OrthorectifierThresholds,
)
from gps_denied_onboard.components.c5_state.config import C5StateConfig
from gps_denied_onboard.components.c5_state.errors import (
EstimatorAlreadyStartedError,
@@ -78,8 +83,9 @@ 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.fc import GpsHealth, GpsSample
from gps_denied_onboard._types.nav import ImuWindow, VioOutput
from gps_denied_onboard._types.nav import ImuWindow, NavCameraFrame, VioOutput
from gps_denied_onboard._types.pose import PoseEstimate
from gps_denied_onboard.clock import Clock
from gps_denied_onboard.config import Config
@@ -281,6 +287,17 @@ class GtsamIsam2StateEstimator(StateEstimator):
# ``EstimatorAlreadyStartedError`` (AC-6).
self._cold_start_window_closed: bool = False
# AZ-389 state ----------------------------------------------------
# The orthorectifier is wired in the composition root via
# :meth:`attach_orthorectifier` whenever AZ-389 is enabled in
# config. ``None`` keeps the legacy steady-state path untouched
# (no mid-flight tile gen). ``_latest_nav_frame`` is the
# per-frame buffer the orthorectifier reads on a successful
# ``current_estimate`` (Invariant 1 — single ingest thread, so
# no locking needed).
self._orthorectifier: Orthorectifier | None = None
self._latest_nav_frame: "NavCameraFrame | None" = None
self._log.debug(
"c5.state.isam2_initialised",
extra={
@@ -315,6 +332,30 @@ class GtsamIsam2StateEstimator(StateEstimator):
"""
self._isam2_handle = handle
def attach_orthorectifier(self, orthorectifier: Orthorectifier) -> None:
"""Wire the AZ-389 mid-flight tile orthorectifier.
Optional; called by the composition root only when
``config.components['c5_state'].orthorectifier.enabled`` is
true. Once attached, every successful :meth:`current_estimate`
will invoke
:meth:`Orthorectifier.try_emit_candidate` with the most recent
nav-camera frame buffered via :meth:`set_latest_nav_frame`.
"""
self._orthorectifier = orthorectifier
def set_latest_nav_frame(self, frame: "NavCameraFrame") -> None:
"""Buffer the most recent nav-camera frame for AZ-389.
The composition root's per-frame loop calls this BEFORE
:meth:`current_estimate` so the orthorectifier sees a
consistent ``(frame, pose)`` pair. Single-threaded contract
(Invariant 1) — no locking. Quiet no-op when no orthorectifier
is attached so callers don't need to inspect the estimator
state before pushing frames.
"""
self._latest_nav_frame = frame
def set_enu_origin(self, origin: LatLonAlt) -> None:
"""Set the WGS84 origin used by :meth:`current_estimate` for ENU→WGS84.
@@ -1004,7 +1045,7 @@ class GtsamIsam2StateEstimator(StateEstimator):
# already been computed).
self._emit_smoothed_to_fdr_if_any(emitted_at, source_label, last_anchor_age_ms)
return EstimatorOutput(
output = EstimatorOutput(
frame_id=uuid4(),
position_wgs84=position_wgs84,
orientation_world_T_body=orientation,
@@ -1016,6 +1057,54 @@ class GtsamIsam2StateEstimator(StateEstimator):
emitted_at=emitted_at,
)
# AZ-389: opportunistic mid-flight tile emission. Best-effort —
# the orthorectifier internally swallows any kernel / writer
# failure; only the steady-state ``output`` is on the critical
# path. The hook fires on every successful ``current_estimate``
# call but the orthorectifier rate-limits itself to one tile per
# ``frame.frame_id`` (AC-4).
self._maybe_emit_mid_flight_tile(
output=output,
covariance=covariance,
last_anchor_age_ms=last_anchor_age_ms,
)
return output
def _maybe_emit_mid_flight_tile(
self,
*,
output: EstimatorOutput,
covariance: np.ndarray,
last_anchor_age_ms: int,
) -> None:
"""AZ-389 hook — invoke the orthorectifier when wired."""
orthorectifier = self._orthorectifier
frame = self._latest_nav_frame
if orthorectifier is None or frame is None:
return
prev_vio = self._prev_vio
if prev_vio is None:
inlier_count = 0
mre_px = float("inf")
imu_bias_norm = 0.0
else:
quality = prev_vio.feature_quality
inlier_count = int(quality.tracked)
mre_px = float(quality.mre_px)
imu_bias_norm = _imu_bias_norm(prev_vio.imu_bias)
orthorectifier.try_emit_candidate(
frame=frame,
pose_estimate=output,
cov_6x6=covariance,
inlier_count=inlier_count,
mre_px=mre_px,
source_label=output.source_label,
last_anchor_age_ms=int(last_anchor_age_ms),
imu_bias_norm=imu_bias_norm,
estimator_label=_STRATEGY,
)
def smoothed_history(self, n_keyframes: int) -> list[EstimatorOutput]:
"""Return up to ``min(n_keyframes, K)`` past smoothed estimates.
@@ -1475,6 +1564,10 @@ def create(
se3_utils: Any,
wgs_converter: Any,
fdr_client: Any,
mid_flight_tile_writer: MidFlightTileWriter | None = None,
camera_calibration: "CameraCalibration | None" = None,
flight_id: str | None = None,
companion_id: str | None = None,
) -> tuple[StateEstimator, ISam2GraphHandle]:
"""Composition-root factory — returns ``(estimator, handle)`` tuple.
@@ -1482,6 +1575,14 @@ def create(
handle is wired against it (the handle holds the estimator
reference for graph access), and the estimator is back-wired with
the handle so its ``add_*`` methods can call into it.
AZ-389 wiring (optional): if ``mid_flight_tile_writer`` is
provided AND ``config.components['c5_state'].orthorectifier.enabled``
is true, an :class:`Orthorectifier` is constructed and attached to
the estimator. ``camera_calibration``, ``flight_id``, and
``companion_id`` MUST be supplied alongside the writer; missing
any of them while orthorectifier is enabled raises
:class:`StateEstimatorConfigError`.
"""
estimator = GtsamIsam2StateEstimator(
config,
@@ -1492,6 +1593,31 @@ def create(
)
handle = ISam2GraphHandleImpl(estimator)
estimator.attach_handle(handle)
block: C5StateConfig = estimator._block
if mid_flight_tile_writer is not None and block.orthorectifier.enabled:
if camera_calibration is None or flight_id is None or companion_id is None:
raise StateEstimatorConfigError(
"AZ-389 orthorectifier requires camera_calibration + "
"flight_id + companion_id alongside mid_flight_tile_writer"
)
thresholds = OrthorectifierThresholds(
cov_norm_threshold=float(block.orthorectifier.cov_norm_threshold),
inlier_floor=int(block.orthorectifier.inlier_floor),
tile_size_meters=float(block.orthorectifier.tile_size_meters),
tile_size_pixels=int(block.orthorectifier.tile_size_pixels),
zoom_level=int(block.orthorectifier.zoom_level),
jpeg_quality=int(block.orthorectifier.jpeg_quality),
)
orthorectifier = Orthorectifier(
tile_writer=mid_flight_tile_writer,
thresholds=thresholds,
camera_calibration=camera_calibration,
flight_id=flight_id,
companion_id=companion_id,
)
estimator.attach_orthorectifier(orthorectifier)
return estimator, handle
@@ -1581,6 +1707,19 @@ def _make_timestamp_map(
# Module-level pure helpers (AZ-384).
def _imu_bias_norm(bias: Any) -> float:
"""Return ``sqrt(||accel_bias||^2 + ||gyro_bias||^2)`` for a :class:`ImuBias`.
Used by :meth:`GtsamIsam2StateEstimator._maybe_emit_mid_flight_tile`
to populate the AZ-389 quality metadata. Robust to ``None``
components (legacy fixtures) by treating them as zero.
"""
accel = getattr(bias, "accel_bias", (0.0, 0.0, 0.0)) or (0.0, 0.0, 0.0)
gyro = getattr(bias, "gyro_bias", (0.0, 0.0, 0.0)) or (0.0, 0.0, 0.0)
sq = sum(float(c) ** 2 for c in accel) + sum(float(c) ** 2 for c in gyro)
return sq**0.5
def _quat_from_pose3(pose: gtsam.Pose3) -> Quat:
"""Build a scalar-first :class:`Quat` from a ``gtsam.Pose3``.
src/gps_denied_onboard/runtime_root/state_factory.py
+156 -10
View File
@@ -21,6 +21,7 @@ binary may set ``OFF`` and only link the ESKF baseline.
from __future__ import annotations
import hashlib
import os
import threading
from collections.abc import Callable
@@ -37,7 +38,9 @@ from gps_denied_onboard.config import Config
from gps_denied_onboard.logging import get_logger
if TYPE_CHECKING:
pass
from datetime import datetime
from gps_denied_onboard._types.calibration import CameraCalibration
__all__ = [
"StateEstimatorFactory",
@@ -136,6 +139,10 @@ def build_state_estimator(
se3_utils: Any,
wgs_converter: Any,
fdr_client: Any,
tile_store: Any | None = None,
camera_calibration: "CameraCalibration | None" = None,
flight_id: str | None = None,
companion_id: str | None = None,
) -> tuple[StateEstimator, ISam2GraphHandle]:
"""Resolve + build the configured state estimator.
@@ -147,6 +154,16 @@ def build_state_estimator(
lookup. The first failure surfaces a :class:`StateEstimatorConfigError`
with the offending strategy + flag name so the operator gets a
clear next step.
AZ-389 wiring (optional): when ``tile_store`` is provided AND
``config.components['c5_state'].orthorectifier.enabled`` is true,
a :class:`_C6MidFlightIngestAdapter` wraps the C6 surface
(hiding ``TileMetadata`` / ``TileSource`` /
``FreshnessRejectionError`` so c5_state stays free of
cross-component imports per AZ-507) and is forwarded to the
estimator factory together with ``camera_calibration``,
``flight_id``, and ``companion_id``. The factory then constructs
the orthorectifier and attaches it to the estimator.
"""
block = _read_state_block(config)
strategy = block.strategy
@@ -169,16 +186,37 @@ def build_state_estimator(
f"state strategy {strategy!r} selected by config but not registered; "
f"registered strategies: {list_registered_state_strategies()}"
)
estimator, handle = factory(
config=config,
imu_preintegrator=imu_preintegrator,
se3_utils=se3_utils,
wgs_converter=wgs_converter,
fdr_client=fdr_client,
)
factory_kwargs: dict[str, Any] = {
"config": config,
"imu_preintegrator": imu_preintegrator,
"se3_utils": se3_utils,
"wgs_converter": wgs_converter,
"fdr_client": fdr_client,
}
if tile_store is not None and block.orthorectifier.enabled:
if camera_calibration is None or flight_id is None or companion_id is None:
raise StateEstimatorConfigError(
"AZ-389 orthorectifier enabled — tile_store must be paired with "
"camera_calibration + flight_id + companion_id "
"(missing: "
f"camera_calibration={camera_calibration is None}, "
f"flight_id={flight_id is None}, "
f"companion_id={companion_id is None})"
)
factory_kwargs["mid_flight_tile_writer"] = _C6MidFlightIngestAdapter(
tile_store=tile_store
)
factory_kwargs["camera_calibration"] = camera_calibration
factory_kwargs["flight_id"] = flight_id
factory_kwargs["companion_id"] = companion_id
estimator, handle = factory(**factory_kwargs)
_log_strategy_loaded(
strategy=strategy,
keyframe_window_size=block.keyframe_window_size,
orthorectifier_enabled=block.orthorectifier.enabled
and tile_store is not None,
)
return estimator, handle
@@ -199,16 +237,124 @@ def _read_state_block(config: Config) -> C5StateConfig:
)
def _log_strategy_loaded(*, strategy: str, keyframe_window_size: int) -> None:
def _log_strategy_loaded(
*,
strategy: str,
keyframe_window_size: int,
orthorectifier_enabled: bool,
) -> None:
log = get_logger("runtime_root.state_factory")
log.info(
f"c5.state.strategy_loaded: strategy={strategy} "
f"keyframe_window_size={keyframe_window_size}",
f"keyframe_window_size={keyframe_window_size} "
f"orthorectifier_enabled={orthorectifier_enabled}",
extra={
"kind": "c5.state.strategy_loaded",
"kv": {
"strategy": strategy,
"keyframe_window_size": keyframe_window_size,
"orthorectifier_enabled": orthorectifier_enabled,
},
},
)
# ----------------------------------------------------------------------
# AZ-389: composition-root bridge between c5_state and c6_tile_cache.
class _C6MidFlightIngestAdapter:
"""C5 ↔ C6 adapter for AZ-389 mid-flight tile candidates.
The c5_state component cannot import from c6_tile_cache (AZ-507;
``test_az270_compose_root.test_ac6_only_compose_root_imports_concrete_strategies``).
This adapter is the single allowed bridge: it lives in the
composition root, accepts the c6 :class:`TileStore` reference,
and exposes the
:class:`gps_denied_onboard.components.c5_state._orthorectifier.MidFlightTileWriter`
Protocol surface that c5 depends on.
Translation rules:
* ``write_mid_flight_tile`` builds the canonical
:class:`TileMetadata` (``TileSource.ONBOARD_INGEST`` +
``FreshnessLabel.FRESH`` + ``VotingStatus.PENDING``) and a
:class:`TileQualityMetadata` from the C5 inputs, hashes the
JPEG bytes, and calls :meth:`TileStore.write_tile` once
(atomic file + metadata insert).
* A :class:`FreshnessRejectionError` from C6 is translated to a
``None`` return — the orthorectifier swallows that silently
per AC-NEW-3.
* Any other C6 error propagates; the orthorectifier itself
wraps that in a WARNING log and returns ``None`` so the C5
``current_estimate`` path is never broken by tile-cache
infrastructure failures.
"""
__slots__ = ("_tile_store",)
def __init__(self, *, tile_store: Any) -> None:
self._tile_store = tile_store
def write_mid_flight_tile(
self,
*,
jpeg_bytes: bytes,
zoom_level: int,
lat: float,
lon: float,
tile_size_meters: float,
tile_size_pixels: int,
capture_timestamp: "datetime",
flight_id: str,
companion_id: str,
estimator_label: str,
covariance_2x2_horizontal: tuple[
tuple[float, float], tuple[float, float]
],
last_anchor_age_ms: int,
mre_px: float,
imu_bias_norm: float,
) -> tuple[int, float, float] | None:
from gps_denied_onboard.components.c6_tile_cache._types import (
FreshnessLabel,
TileId,
TileMetadata,
TileQualityMetadata,
TileSource,
VotingStatus,
)
from gps_denied_onboard.components.c6_tile_cache.errors import (
FreshnessRejectionError,
)
tile_id = TileId(
zoom_level=int(zoom_level),
lat=float(lat),
lon=float(lon),
)
quality = TileQualityMetadata(
estimator_label=str(estimator_label),
covariance_2x2=covariance_2x2_horizontal,
last_anchor_age_ms=int(last_anchor_age_ms),
mre_px=float(mre_px),
imu_bias_norm=float(imu_bias_norm),
)
metadata = TileMetadata(
tile_id=tile_id,
tile_size_meters=float(tile_size_meters),
tile_size_pixels=int(tile_size_pixels),
capture_timestamp=capture_timestamp,
source=TileSource.ONBOARD_INGEST,
content_sha256_hex=hashlib.sha256(jpeg_bytes).hexdigest(),
freshness_label=FreshnessLabel.FRESH,
flight_id=str(flight_id),
companion_id=str(companion_id),
quality_metadata=quality,
voting_status=VotingStatus.PENDING,
)
try:
self._tile_store.write_tile(jpeg_bytes, metadata)
except FreshnessRejectionError:
return None
return (tile_id.zoom_level, tile_id.lat, tile_id.lon)