mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-21 10:21:13 +00:00
Oleksandr Bezdieniezhnykh
94d2358c8b
Derkachi e2e Tier-2 divergence had three stacked root causes; this
commit ships fixes for all three plus the IMU prerequisite they
depend on, plus a baseline cheirality gate for cv2.recoverPose.
AZ-918 MAVLink IMU adapters now convert raw mG/mrad-s + FRD body to
SI m/s^2 + rad/s + FLU body via helpers.imu_units. Without
this the ESKF receives values ~1000x too small with wrong-
sign Y/Z and cannot function at all.
AZ-919 Composition root wires EskfNominalAltitudeProvider into the
KLT/RANSAC strategy via the AZ-331 factory introspect path;
OKVIS2 and VINS-Mono are unaffected.
AZ-920 KLT/RANSAC recovers metric translation via Ground Sampling
Distance when AGL is available; otherwise falls through with
scale_quality=direction_only/unknown (no fake scale invented).
AZ-921 VioOutput.scale_quality signal; ESKF add_vio adapts R_meas
position block based on the flag (1e6 inflation when scale is
direction_only/unknown to keep the filter consistent).
AZ-922 KLT/RANSAC cheirality gate rejects single-frame rotations
beyond a config threshold (default 30 deg), catching
cv2.recoverPose twisted-pair flips that cause immediate ESKF
divergence on low-parallax aerial scenes.
Verification:
- Tier-1 (macOS) unit suite: 2346 passed, 0 failed.
- Tier-2 (Jetson) Derkachi e2e: divergence moves from frame 5
(mahalanobis^2 3757) to frame 233 (mahalanobis^2 212). Remaining
drift is open-loop attitude accumulation, not cheirality.
Follow-up tickets filed:
- AZ-923 closed as misdiagnosed: EskfNominalAltitudeProvider was
already correct (nominal_pos.z IS the AGL when takeoff origin sits
at ground level); the early-frame AGL near zero reflects the drone
being stationary on the ground, not a provider bug.
- AZ-942 filed: cross-check VIO rotation against IMU preintegrator
(consistency gate) - more physically grounded than the coarse
AZ-922 threshold and likely required to absorb the frame-233 drift.
Co-authored-by: Cursor <cursoragent@cursor.com>
92 lines
2.5 KiB
Python
92 lines
2.5 KiB
Python
"""AZ-921 AC-4 — VioOutput.scale_quality DTO contract."""
|
|
|
|
from __future__ import annotations
|
|
|
|
import typing
|
|
from dataclasses import fields
|
|
|
|
import numpy as np
|
|
|
|
from gps_denied_onboard._types.nav import (
|
|
FeatureQuality,
|
|
ImuBias,
|
|
ScaleQuality,
|
|
VioOutput,
|
|
)
|
|
from gps_denied_onboard.helpers.se3_utils import SE3, matrix_to_se3
|
|
|
|
|
|
def _imu_bias() -> ImuBias:
|
|
return ImuBias(
|
|
accel_bias=(0.0, 0.0, 0.0),
|
|
gyro_bias=(0.0, 0.0, 0.0),
|
|
)
|
|
|
|
|
|
def _feature_quality() -> FeatureQuality:
|
|
return FeatureQuality(
|
|
tracked=10,
|
|
new=2,
|
|
lost=1,
|
|
mean_parallax=1.0,
|
|
mre_px=0.5,
|
|
)
|
|
|
|
|
|
def _identity_se3() -> SE3:
|
|
return matrix_to_se3(np.eye(4, dtype=np.float64))
|
|
|
|
|
|
def test_vio_output_default_scale_quality_is_unknown_for_back_compat() -> None:
|
|
# Arrange / Act — constructor MUST work without scale_quality so
|
|
# legacy strategies (OKVIS2, VINS-Mono) that have not been updated
|
|
# for AZ-921 stay bug-for-bug compatible.
|
|
vio = VioOutput(
|
|
frame_id="frame-0",
|
|
relative_pose_T=_identity_se3(),
|
|
pose_covariance_6x6=np.eye(6, dtype=np.float64),
|
|
imu_bias=_imu_bias(),
|
|
feature_quality=_feature_quality(),
|
|
emitted_at_ns=1_000_000_000,
|
|
)
|
|
|
|
# Assert
|
|
assert vio.scale_quality == "unknown"
|
|
|
|
|
|
def test_vio_output_accepts_each_scale_quality_value() -> None:
|
|
# Arrange
|
|
accepted_values = ("metric", "direction_only", "unknown")
|
|
|
|
# Act / Assert
|
|
for value in accepted_values:
|
|
vio = VioOutput(
|
|
frame_id="frame-0",
|
|
relative_pose_T=_identity_se3(),
|
|
pose_covariance_6x6=np.eye(6, dtype=np.float64),
|
|
imu_bias=_imu_bias(),
|
|
feature_quality=_feature_quality(),
|
|
emitted_at_ns=1_000_000_000,
|
|
scale_quality=value,
|
|
)
|
|
assert vio.scale_quality == value
|
|
|
|
|
|
def test_scale_quality_literal_pins_the_three_documented_values() -> None:
|
|
# Arrange / Act — the Literal type from the typing module exposes
|
|
# its parameters via __args__, which lets the contract test pin
|
|
# the exact accepted-string set rather than relying on a string
|
|
# comparison alone.
|
|
args = typing.get_args(ScaleQuality)
|
|
|
|
# Assert
|
|
assert set(args) == {"metric", "direction_only", "unknown"}
|
|
|
|
|
|
def test_vio_output_dataclass_exposes_scale_quality_field() -> None:
|
|
# Arrange / Act
|
|
field_names = {f.name for f in fields(VioOutput)}
|
|
|
|
# Assert
|
|
assert "scale_quality" in field_names
|