mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 22:11:13 +00:00
[AZ-776] Open-loop ESKF composition profile via c4_pose.enabled
ADR-012: add c4_pose.enabled (default True) and enforce the (c4_pose.enabled, c5_state.strategy) 2x2 pairing matrix at compose time. When enabled=false, compose_root removes c4_pose from the selection map and build_pre_constructed omits c5_isam2_graph_handle. Replay protocol Invariant 13 owns the gate. Tier-2 conftest YAML writes the open-loop profile; un-xfails AC-1/2/5 and both AC-6 variants in Derkachi (AC-3 stays xfailed for AZ-777). 319/319 runtime_root + c4_pose + c5_state tests green. Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
@@ -27,6 +27,16 @@ class C4PoseConfig:
|
||||
|
||||
Fields per the C4 contract §"Config-load-time validation":
|
||||
|
||||
* ``enabled`` — AZ-776 composition-graph flag. When ``False`` the
|
||||
composition root strips ``c4_pose`` from the airborne component
|
||||
graph entirely (open-loop ESKF profile per ADR-012). Default
|
||||
``True`` preserves the full GTSAM pipeline that ADR-003 mandates
|
||||
as the steady-state airborne path. Forbidden pairings (rejected
|
||||
by ``compose_root`` with :class:`CompositionError`):
|
||||
``enabled=False`` + ``c5_state.strategy="gtsam_isam2"`` (iSAM2
|
||||
requires a C4 anchor); ``enabled=True`` +
|
||||
``c5_state.strategy="eskf"`` (ESKF has no iSAM2 graph for C4 to
|
||||
anchor against).
|
||||
* ``strategy`` — selects the concrete estimator. Currently only
|
||||
``"opencv_gtsam"`` is defined.
|
||||
* ``ransac_iterations`` — OpenCV ``solvePnPRansac`` iteration
|
||||
@@ -56,6 +66,7 @@ class C4PoseConfig:
|
||||
handling would land in a future config extension).
|
||||
"""
|
||||
|
||||
enabled: bool = True
|
||||
strategy: str = "opencv_gtsam"
|
||||
ransac_iterations: int = 200
|
||||
ransac_reprojection_threshold_px: float = 4.0
|
||||
|
||||
@@ -336,6 +336,7 @@ def _compose(
|
||||
allowed_tiers: frozenset[StrategyTier],
|
||||
extra_required_env: Iterable[str],
|
||||
pre_constructed: Mapping[str, Any] | None = None,
|
||||
skip_slugs: frozenset[str] = frozenset(),
|
||||
) -> tuple[dict[str, Any], tuple[str, ...]]:
|
||||
"""Shared composition path used by ``compose_root`` / ``compose_operator``.
|
||||
|
||||
@@ -345,9 +346,17 @@ def _compose(
|
||||
strategies (``frame_source``, ``fc_adapter``, ``clock``,
|
||||
``mavlink_transport``, ``replay_sink``) so any C1-C7 factory that
|
||||
declares a dependency on one finds it already populated.
|
||||
|
||||
``skip_slugs`` removes component slugs from the composition graph
|
||||
even when ``config.components`` contains a block for them. AZ-776
|
||||
uses this to drop ``c4_pose`` from the airborne open-loop ESKF
|
||||
profile; the slug is filtered out of the topological walk and the
|
||||
corresponding wrapper factory is never invoked.
|
||||
"""
|
||||
_check_required_env(extra_required=extra_required_env)
|
||||
selections = _resolve_component_strategies(config, allowed_tiers)
|
||||
for skipped in skip_slugs:
|
||||
selections.pop(skipped, None)
|
||||
resolved: dict[str, _Registration] = {
|
||||
slug: _resolve_strategy(slug, strategy, allowed_tiers)
|
||||
for slug, strategy in selections.items()
|
||||
@@ -423,6 +432,92 @@ def _read_strategy_attr(block: Any) -> Any:
|
||||
return None
|
||||
|
||||
|
||||
def _read_c4_pose_enabled(config: Config) -> bool:
|
||||
"""True iff the airborne ``c4_pose`` slot is enabled in the composition graph.
|
||||
|
||||
AZ-776: ``c4_pose.enabled = False`` selects the open-loop ESKF
|
||||
composition profile (ADR-012). Defaults to ``True`` when the
|
||||
block is absent or carries no ``enabled`` attribute, preserving
|
||||
the legacy GTSAM pipeline (ADR-003 steady-state path).
|
||||
"""
|
||||
components = getattr(config, "components", None) or {}
|
||||
if not isinstance(components, Mapping):
|
||||
return True
|
||||
block = components.get("c4_pose")
|
||||
if block is None:
|
||||
return True
|
||||
enabled = getattr(block, "enabled", True)
|
||||
return bool(enabled)
|
||||
|
||||
|
||||
def _read_c5_state_strategy(config: Config) -> str:
|
||||
"""Return ``config.components['c5_state'].strategy`` or the default.
|
||||
|
||||
Defaults to ``"gtsam_isam2"`` when the block is absent — the same
|
||||
fallback :func:`airborne_bootstrap._resolve_c5_state_strategy`
|
||||
uses for the eager :func:`build_state_estimator` invocation.
|
||||
"""
|
||||
components = getattr(config, "components", None) or {}
|
||||
if not isinstance(components, Mapping):
|
||||
return "gtsam_isam2"
|
||||
block = components.get("c5_state")
|
||||
if block is None:
|
||||
return "gtsam_isam2"
|
||||
strategy = getattr(block, "strategy", "gtsam_isam2")
|
||||
return str(strategy) if strategy else "gtsam_isam2"
|
||||
|
||||
|
||||
def _validate_c4_c5_composition_profile(config: Config) -> None:
|
||||
"""Reject incompatible ``c4_pose.enabled`` / ``c5_state.strategy`` pairings.
|
||||
|
||||
AZ-776 / ADR-012 defines two valid composition profiles for the
|
||||
airborne binary:
|
||||
|
||||
* **Full GTSAM** (steady-state, ADR-003): ``c4_pose.enabled=True``
|
||||
+ ``c5_state.strategy="gtsam_isam2"``. C4 anchors land into C5's
|
||||
iSAM2 graph; C5 produces smoothed estimates.
|
||||
* **Open-loop ESKF** (mandatory simple baseline, ADR-012):
|
||||
``c4_pose.enabled=False`` + ``c5_state.strategy="eskf"``. C4 is
|
||||
stripped from the graph; C5 ESKF integrates VIO + IMU
|
||||
open-loop. No iSAM2 graph handle is produced or consumed.
|
||||
|
||||
The two off-diagonal pairings are physically impossible:
|
||||
|
||||
* ``c4_pose.enabled=False`` + ``c5_state.strategy="gtsam_isam2"``:
|
||||
iSAM2 requires a C4 pose anchor to bound drift; without C4 the
|
||||
graph never receives a pose factor and the smoother is unbounded.
|
||||
* ``c4_pose.enabled=True`` + ``c5_state.strategy="eskf"``: C4
|
||||
needs the iSAM2 graph handle to inject pose factors, but ESKF
|
||||
returns ``handle=None`` by design (no graph). The C4 wrapper
|
||||
would crash on the ``None`` handle.
|
||||
|
||||
Either off-diagonal raises :class:`CompositionError` with a clear
|
||||
next-step naming both fields.
|
||||
"""
|
||||
c4_enabled = _read_c4_pose_enabled(config)
|
||||
c5_strategy = _read_c5_state_strategy(config)
|
||||
if not c4_enabled and c5_strategy == "gtsam_isam2":
|
||||
raise CompositionError(
|
||||
"compose_root: c4_pose.enabled=False is incompatible with "
|
||||
"c5_state.strategy='gtsam_isam2' — iSAM2 requires C4 pose "
|
||||
"anchors to bound smoother drift. Use either the full "
|
||||
"GTSAM profile (c4_pose.enabled=true, c5_state.strategy="
|
||||
"'gtsam_isam2') or the open-loop ESKF profile "
|
||||
"(c4_pose.enabled=false, c5_state.strategy='eskf'). See "
|
||||
"ADR-012 (open-loop ESKF composition profile)."
|
||||
)
|
||||
if c4_enabled and c5_strategy == "eskf":
|
||||
raise CompositionError(
|
||||
"compose_root: c4_pose.enabled=True is incompatible with "
|
||||
"c5_state.strategy='eskf' — ESKF has no iSAM2 graph for "
|
||||
"C4 to anchor against (handle=None by design), so the C4 "
|
||||
"wrapper cannot run. Set c4_pose.enabled=false to select "
|
||||
"the open-loop ESKF profile, or change c5_state.strategy "
|
||||
"to 'gtsam_isam2' for the full GTSAM pipeline. See "
|
||||
"ADR-012 (open-loop ESKF composition profile)."
|
||||
)
|
||||
|
||||
|
||||
def compose_root(
|
||||
config: Config,
|
||||
*,
|
||||
@@ -445,6 +540,14 @@ def compose_root(
|
||||
finds it already populated. C1-C7+C13 strategies are wired
|
||||
identically to live mode (replay protocol Invariant 1).
|
||||
|
||||
AZ-776 composition profile (ADR-012): when
|
||||
``config.components['c4_pose'].enabled`` is ``False`` the function
|
||||
strips ``c4_pose`` from the airborne component graph (open-loop
|
||||
ESKF profile). The forbidden off-diagonal pairings
|
||||
(``enabled=False`` + ``c5_state.strategy='gtsam_isam2'``;
|
||||
``enabled=True`` + ``c5_state.strategy='eskf'``) raise
|
||||
:class:`CompositionError` before any factory runs.
|
||||
|
||||
The ``pre_constructed`` kwarg (AZ-591) lets the caller seed
|
||||
``constructed`` with infrastructure objects (e.g. fdr_client,
|
||||
descriptor_index, inference_runtime) before any registered factory
|
||||
@@ -461,6 +564,8 @@ def compose_root(
|
||||
have to satisfy the full OpenCV / pymavlink / FDR side-effects of
|
||||
the real strategies.
|
||||
"""
|
||||
_validate_c4_c5_composition_profile(config)
|
||||
skip_slugs = frozenset() if _read_c4_pose_enabled(config) else frozenset({"c4_pose"})
|
||||
extra_env = ("MAVLINK_SIGNING_KEY",) if config.mode == "live" else ()
|
||||
if config.mode == "replay":
|
||||
replay_factory = replay_components_factory or build_replay_components
|
||||
@@ -478,6 +583,7 @@ def compose_root(
|
||||
allowed_tiers=frozenset({"airborne", "shared"}),
|
||||
extra_required_env=extra_env,
|
||||
pre_constructed=seeded,
|
||||
skip_slugs=skip_slugs,
|
||||
)
|
||||
merged: dict[str, Any] = dict(replay_components)
|
||||
merged.update(components)
|
||||
|
||||
@@ -1135,6 +1135,33 @@ def _build_c3_feature_extractor(config: Config) -> FeatureExtractor:
|
||||
return OpenCvOrbExtractor()
|
||||
|
||||
|
||||
def _c4_pose_disabled(config: Config) -> bool:
|
||||
"""True iff the airborne open-loop ESKF composition profile is active.
|
||||
|
||||
AZ-776 / ADR-012: when ``config.components["c4_pose"].enabled`` is
|
||||
explicitly ``False`` the composition root strips ``c4_pose`` from
|
||||
the component graph and the C4 wrapper never runs. The eager
|
||||
``(estimator, handle)`` build inside :func:`build_pre_constructed`
|
||||
therefore has no consumer for the iSAM2 graph handle slot
|
||||
(``c5_isam2_graph_handle``); leaving the slot absent makes the
|
||||
"no C4" property visible at the pre-bootstrap layer too.
|
||||
|
||||
Returns ``False`` when the block is absent or when ``enabled`` is
|
||||
missing / true — the full GTSAM profile (ADR-003 steady state)
|
||||
remains the default airborne pipeline. Replay-mode component-block
|
||||
omission is handled separately by
|
||||
:func:`_replay_omits_component_block`.
|
||||
"""
|
||||
components = getattr(config, "components", None) or {}
|
||||
if not isinstance(components, Mapping):
|
||||
return False
|
||||
block = components.get("c4_pose")
|
||||
if block is None:
|
||||
return False
|
||||
enabled = getattr(block, "enabled", True)
|
||||
return not bool(enabled)
|
||||
|
||||
|
||||
def _replay_omits_component_block(config: Config, block_name: str) -> bool:
|
||||
"""True iff replay-mode :class:`Config` has no ``components[block_name]`` entry.
|
||||
|
||||
@@ -1301,7 +1328,14 @@ def build_pre_constructed(config: Config) -> dict[str, Any]:
|
||||
fdr_client=constructed["c13_fdr"],
|
||||
tile_store=constructed.get("c6_tile_store"),
|
||||
)
|
||||
constructed["c5_isam2_graph_handle"] = handle
|
||||
# AZ-776 / ADR-012 open-loop ESKF profile: c4_pose.enabled=False
|
||||
# strips c4_pose from the composition graph, so the
|
||||
# c5_isam2_graph_handle slot has no consumer. ESKF returns
|
||||
# handle=None by design; omitting the slot makes the open-loop
|
||||
# property visible at the pre_constructed layer and refuses the
|
||||
# invariant "handle is None means C4 will crash on read".
|
||||
if not _c4_pose_disabled(config):
|
||||
constructed["c5_isam2_graph_handle"] = handle
|
||||
constructed[_C5_PREBUILT_ESTIMATOR_KEY] = estimator
|
||||
return constructed
|
||||
|
||||
|
||||
Reference in New Issue
Block a user