[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:
Oleksandr Bezdieniezhnykh
2026-05-21 13:40:01 +03:00
parent 6044a33197
commit 8de2716500
10 changed files with 687 additions and 83 deletions
@@ -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