mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-23 02:01:13 +00:00
[AZ-271] [AZ-276] [AZ-278] [AZ-282] Finish cross-cutting helpers + relax opencv pin
E-CC-HELPERS closes with the three remaining Layer-1 helpers and E-CC-CONF closes with the env > YAML > defaults precedence test gate. All four tickets ship with frozen public surfaces, hermetic unit tests, and no upward (components.*) imports. * AZ-271 — tests/unit/shared/config/test_precedence.py (5 ACs + smoke test + helper that names the layer in failure messages). * AZ-282 — helpers/ransac_filter.py: static RansacFilter + RansacResult; cv2.setRNGSeed(0) for byte-equal determinism; median residual semantics pinned by contract. * AZ-276 — helpers/imu_preintegrator.py + make_imu_preintegrator; GTSAM PreintegratedCombinedMeasurements; strict-monotonic ts_ns guard runs before any state mutation. Adjacent hygiene: _types/nav.py ImuSample/ImuWindow now use ts_ns:int and the spec-mandated ImuBias dataclass. * AZ-278 — helpers/lightglue_runtime.py: structural R14 fix. LightGlueRuntime + non-blocking concurrent-access guard that raises rather than serialising. EngineHandle Protocol in _types/manifests.py + KeypointSet/CorrespondenceSet in _types/matching.py (Protocol surface adds approved by spec). Dependency conflict (Finding 1, user-approved): gtsam 4.2 (PyPI) is numpy-1.x-ABI only; opencv-python>=4.12 needs numpy>=2 at runtime. Resolution: opencv-python pin relaxed to >=4.11.0.86,<4.12. The D-CROSS-CVE-1 ratchet at ci/opencv_pin_gate.py is held at 4.11.0 with the original 4.12.0 floor restored once a numpy-2-compatible gtsam wheel ships. Full replay procedure in _docs/_process_leftovers/2026-05-11_d_cross_cve_1_opencv_pin_deferred.md. Tests: 294 passed, 2 skipped (cmake/actionlint env-skips, pre-existing). 43 new tests added for batch 5. Ruff check + format clean. Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
@@ -0,0 +1,235 @@
|
||||
"""AZ-271 — Config precedence tests (env > YAML > defaults).
|
||||
|
||||
Verifies the precedence rule for ≥3 keys at each layer plus the
|
||||
multi-file YAML merge order (later wins) per epic AZ-246 AC-3. Tests
|
||||
are hermetic: env is passed in via the loader's ``env`` argument and
|
||||
YAML is materialised via ``tmp_path``.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
import pytest
|
||||
|
||||
from gps_denied_onboard.config import (
|
||||
Config,
|
||||
FdrConfig,
|
||||
LogConfig,
|
||||
RuntimeConfig,
|
||||
load_config,
|
||||
)
|
||||
|
||||
REQUIRED_ENV: dict[str, str] = {
|
||||
"GPS_DENIED_FC_PROFILE": "ardupilot_plane",
|
||||
"GPS_DENIED_TIER": "1",
|
||||
"DB_URL": "postgresql://localhost:5432/test",
|
||||
"CAMERA_CALIBRATION_PATH": "/tmp/cal.yaml",
|
||||
"LOG_LEVEL": "INFO",
|
||||
"LOG_SINK": "console",
|
||||
"INFERENCE_BACKEND": "pytorch_fp16",
|
||||
"FDR_PATH": "/tmp/fdr",
|
||||
"TILE_CACHE_PATH": "/tmp/tiles",
|
||||
}
|
||||
|
||||
|
||||
def _write_yaml(tmp_path: Path, name: str, content: str) -> Path:
|
||||
path = tmp_path / name
|
||||
path.write_text(content)
|
||||
return path
|
||||
|
||||
|
||||
def _layer_msg(layer: str, key: str, expected: object, actual: object) -> str:
|
||||
"""Standardised assertion message naming the precedence layer (AC-5)."""
|
||||
return (
|
||||
f"precedence layer {layer!r} for key {key!r}: "
|
||||
f"expected {expected!r} (from {layer}), got {actual!r}"
|
||||
)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# AC-1: env wins over YAML for ≥3 keys (LOG_LEVEL, FDR_QUEUE_SIZE, GPS_DENIED_TIER).
|
||||
|
||||
|
||||
def test_ac1_env_wins_over_yaml_for_three_keys(tmp_path: Path) -> None:
|
||||
# Arrange
|
||||
yaml_path = _write_yaml(
|
||||
tmp_path,
|
||||
"base.yaml",
|
||||
"""
|
||||
log:
|
||||
level: WARN
|
||||
fdr:
|
||||
queue_size: 8192
|
||||
runtime:
|
||||
tier: 2
|
||||
""",
|
||||
)
|
||||
env = dict(REQUIRED_ENV)
|
||||
env["LOG_LEVEL"] = "ERROR"
|
||||
env["FDR_QUEUE_SIZE"] = "16384"
|
||||
env["GPS_DENIED_TIER"] = "1"
|
||||
|
||||
# Act
|
||||
config = load_config(env=env, paths=(yaml_path,))
|
||||
|
||||
# Assert
|
||||
assert config.log.level == "ERROR", _layer_msg("env", "log.level", "ERROR", config.log.level)
|
||||
assert config.fdr.queue_size == 16384, _layer_msg(
|
||||
"env", "fdr.queue_size", 16384, config.fdr.queue_size
|
||||
)
|
||||
assert config.runtime.tier == 1, _layer_msg("env", "runtime.tier", 1, config.runtime.tier)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# AC-2: YAML wins over defaults for ≥3 keys.
|
||||
|
||||
|
||||
def test_ac2_yaml_wins_over_defaults_for_three_keys(tmp_path: Path) -> None:
|
||||
# Arrange
|
||||
yaml_path = _write_yaml(
|
||||
tmp_path,
|
||||
"base.yaml",
|
||||
"""
|
||||
log:
|
||||
level: DEBUG
|
||||
sink: journald
|
||||
fdr:
|
||||
queue_size: 2048
|
||||
""",
|
||||
)
|
||||
env = dict(REQUIRED_ENV)
|
||||
# Remove any env keys that map to the YAML overrides — we want YAML > defaults
|
||||
# without env shadowing.
|
||||
for env_key in ("LOG_LEVEL", "LOG_SINK", "FDR_QUEUE_SIZE"):
|
||||
env.pop(env_key, None)
|
||||
# LOG_LEVEL is in REQUIRED_ENV but the loader's required-env gate would
|
||||
# complain; bypass with ``require_env=False`` to keep the test hermetic.
|
||||
|
||||
# Act
|
||||
config = load_config(env=env, paths=(yaml_path,), require_env=False)
|
||||
|
||||
# Assert
|
||||
log_defaults = LogConfig()
|
||||
fdr_defaults = FdrConfig()
|
||||
assert config.log.level == "DEBUG", _layer_msg("yaml", "log.level", "DEBUG", config.log.level)
|
||||
assert config.log.level != log_defaults.level
|
||||
assert config.log.sink == "journald", _layer_msg(
|
||||
"yaml", "log.sink", "journald", config.log.sink
|
||||
)
|
||||
assert config.log.sink != log_defaults.sink
|
||||
assert config.fdr.queue_size == 2048, _layer_msg(
|
||||
"yaml", "fdr.queue_size", 2048, config.fdr.queue_size
|
||||
)
|
||||
assert config.fdr.queue_size != fdr_defaults.queue_size
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# AC-3: defaults apply for ≥3 keys when env + YAML omit them.
|
||||
|
||||
|
||||
def test_ac3_defaults_apply_when_layers_silent() -> None:
|
||||
# Arrange — empty YAML, env_default-only-required-vars.
|
||||
env = dict(REQUIRED_ENV)
|
||||
# Strip three env keys so loader falls to defaults for those three.
|
||||
env.pop("LOG_LEVEL")
|
||||
env.pop("LOG_SINK")
|
||||
env.pop("FDR_QUEUE_SIZE", None) # FDR_QUEUE_SIZE is not required
|
||||
|
||||
# Act
|
||||
config = load_config(env=env, paths=(), require_env=False)
|
||||
|
||||
# Assert
|
||||
log_defaults = LogConfig()
|
||||
fdr_defaults = FdrConfig()
|
||||
runtime_defaults = RuntimeConfig()
|
||||
assert config.log.level == log_defaults.level, _layer_msg(
|
||||
"default", "log.level", log_defaults.level, config.log.level
|
||||
)
|
||||
assert config.log.sink == log_defaults.sink, _layer_msg(
|
||||
"default", "log.sink", log_defaults.sink, config.log.sink
|
||||
)
|
||||
assert config.fdr.queue_size == fdr_defaults.queue_size, _layer_msg(
|
||||
"default", "fdr.queue_size", fdr_defaults.queue_size, config.fdr.queue_size
|
||||
)
|
||||
# Sanity: runtime defaults also intact for keys with NO env override.
|
||||
assert (
|
||||
config.runtime.inference_backend == runtime_defaults.inference_backend
|
||||
or env.get("INFERENCE_BACKEND") == config.runtime.inference_backend
|
||||
)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# AC-4: multi-file YAML — later wins.
|
||||
|
||||
|
||||
def test_ac4_multi_file_yaml_later_wins(tmp_path: Path) -> None:
|
||||
# Arrange
|
||||
first = _write_yaml(
|
||||
tmp_path,
|
||||
"first.yaml",
|
||||
"""
|
||||
log:
|
||||
level: WARN
|
||||
fdr:
|
||||
queue_size: 1024
|
||||
""",
|
||||
)
|
||||
second = _write_yaml(
|
||||
tmp_path,
|
||||
"second.yaml",
|
||||
"""
|
||||
log:
|
||||
level: ERROR
|
||||
fdr:
|
||||
queue_size: 8192
|
||||
""",
|
||||
)
|
||||
env = dict(REQUIRED_ENV)
|
||||
env.pop("LOG_LEVEL") # don't let env shadow the YAML precedence test
|
||||
|
||||
# Act
|
||||
config = load_config(env=env, paths=(first, second), require_env=False)
|
||||
|
||||
# Assert
|
||||
assert config.log.level == "ERROR", _layer_msg(
|
||||
"later-yaml", "log.level", "ERROR", config.log.level
|
||||
)
|
||||
assert config.fdr.queue_size == 8192, _layer_msg(
|
||||
"later-yaml", "fdr.queue_size", 8192, config.fdr.queue_size
|
||||
)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# AC-5: assertion message names the layer (verified by introspecting helper).
|
||||
|
||||
|
||||
def test_ac5_failure_messages_name_the_layer() -> None:
|
||||
# Arrange
|
||||
msg = _layer_msg("env", "log.level", "ERROR", "INFO")
|
||||
|
||||
# Assert — message contains the layer name, the key, and both values.
|
||||
assert "env" in msg
|
||||
assert "log.level" in msg
|
||||
assert "ERROR" in msg
|
||||
assert "INFO" in msg
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Smoke: load_config + compose_root integrate (regression guard).
|
||||
|
||||
|
||||
def test_load_config_returns_frozen_config_dataclass() -> None:
|
||||
# Arrange
|
||||
env = dict(REQUIRED_ENV)
|
||||
|
||||
# Act
|
||||
config = load_config(env=env, paths=())
|
||||
|
||||
# Assert
|
||||
import dataclasses
|
||||
|
||||
assert isinstance(config, Config)
|
||||
with pytest.raises(dataclasses.FrozenInstanceError):
|
||||
# frozen=True → cannot mutate
|
||||
config.log = LogConfig() # type: ignore[misc]
|
||||
@@ -99,7 +99,7 @@ def test_opencv_pin_gate_passes_on_412_minimum() -> None:
|
||||
|
||||
|
||||
def test_opencv_pin_gate_fails_on_lower_version(tmp_path: Path) -> None:
|
||||
# Arrange
|
||||
# Arrange — 4.10 is below the (relaxed) 4.11 floor; the gate still rejects.
|
||||
bad_pyproject = tmp_path / "pyproject.toml"
|
||||
bad_pyproject.write_text(
|
||||
'[project]\nname = "x"\nversion = "0.1"\ndependencies = ["opencv-python>=4.10,<5"]\n'
|
||||
@@ -120,5 +120,6 @@ def test_opencv_pin_gate_fails_on_lower_version(tmp_path: Path) -> None:
|
||||
|
||||
# Assert
|
||||
assert result.returncode != 0, (
|
||||
"opencv_pin_gate must reject `opencv-python>=4.10` (D-CROSS-CVE-1 ≥ 4.12.0)"
|
||||
"opencv_pin_gate must reject `opencv-python>=4.10` "
|
||||
"(D-CROSS-CVE-1 floor relaxed to 4.11.0; see _docs/_process_leftovers/)"
|
||||
)
|
||||
|
||||
@@ -0,0 +1,271 @@
|
||||
"""AZ-276 — `ImuPreintegrator` AC suite (E-CC-HELPERS).
|
||||
|
||||
Covers the 7 ACs from `_docs/02_tasks/todo/AZ-276_imu_preintegrator.md`.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import ast
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from gps_denied_onboard._types.calibration import CameraCalibration
|
||||
from gps_denied_onboard._types.nav import ImuBias, ImuSample, ImuWindow
|
||||
from gps_denied_onboard.helpers import (
|
||||
CombinedImuFactor,
|
||||
ImuPreintegrationError,
|
||||
ImuPreintegrator,
|
||||
make_imu_preintegrator,
|
||||
)
|
||||
|
||||
|
||||
def _calibration() -> CameraCalibration:
|
||||
return CameraCalibration(
|
||||
camera_id="test_cam",
|
||||
intrinsics_3x3=np.eye(3, dtype=np.float64),
|
||||
distortion=np.zeros(5, dtype=np.float64),
|
||||
body_to_camera_se3=np.eye(4, dtype=np.float64),
|
||||
acquisition_method="lab_calibration",
|
||||
metadata={},
|
||||
)
|
||||
|
||||
|
||||
def _make_samples(n: int, start_ts_ns: int = 0, dt_ns: int = 5_000_000) -> tuple[ImuSample, ...]:
|
||||
"""``n`` strictly-monotonic samples at ``dt_ns`` cadence (default 5 ms = 200 Hz)."""
|
||||
accel = (0.0, 0.0, 9.80665)
|
||||
gyro = (0.0, 0.0, 0.0)
|
||||
return tuple(
|
||||
ImuSample(ts_ns=start_ts_ns + i * dt_ns, accel_xyz=accel, gyro_xyz=gyro) for i in range(n)
|
||||
)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# AC-1: round-trip preintegration.
|
||||
|
||||
|
||||
def test_ac1_round_trip_preintegration() -> None:
|
||||
# Arrange
|
||||
pre = make_imu_preintegrator(_calibration())
|
||||
samples = _make_samples(100)
|
||||
|
||||
# Act
|
||||
for s in samples:
|
||||
pre.integrate_sample(s)
|
||||
pim = pre.current_preintegration()
|
||||
|
||||
# Assert — deltaTij matches the span between first and last sample.
|
||||
expected_dt_s = (samples[-1].ts_ns - samples[0].ts_ns) * 1e-9
|
||||
assert pim.deltaTij() == pytest.approx(expected_dt_s, abs=1e-9)
|
||||
# Z gravity is removed by the preintegrator; we expect non-zero
|
||||
# rotation-frame translation because the device sits stationary
|
||||
# under gravity and PIM accumulates the doubly-integrated specific
|
||||
# force — sufficient for the "non-zero delta_pose" gate.
|
||||
delta_p = np.asarray(pim.deltaPij())
|
||||
assert np.linalg.norm(delta_p) > 0.0
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# AC-2: strict monotonicity rejection leaves state unchanged.
|
||||
|
||||
|
||||
def test_ac2_non_monotonic_rejection_preserves_state() -> None:
|
||||
# Arrange
|
||||
pre = make_imu_preintegrator(_calibration())
|
||||
samples = _make_samples(10)
|
||||
for s in samples:
|
||||
pre.integrate_sample(s)
|
||||
snapshot_dt = pre.current_preintegration().deltaTij()
|
||||
|
||||
bad_sample = ImuSample(
|
||||
ts_ns=samples[-1].ts_ns - 1, # equal/less is rejected
|
||||
accel_xyz=(0.0, 0.0, 9.80665),
|
||||
gyro_xyz=(0.0, 0.0, 0.0),
|
||||
)
|
||||
|
||||
# Act / Assert
|
||||
with pytest.raises(ImuPreintegrationError, match="non-monotonic"):
|
||||
pre.integrate_sample(bad_sample)
|
||||
|
||||
# State unchanged — deltaTij is the same as before the bad sample.
|
||||
assert pre.current_preintegration().deltaTij() == pytest.approx(snapshot_dt)
|
||||
|
||||
# Subsequent valid sample integrates normally.
|
||||
next_good = ImuSample(
|
||||
ts_ns=samples[-1].ts_ns + 5_000_000,
|
||||
accel_xyz=(0.0, 0.0, 9.80665),
|
||||
gyro_xyz=(0.0, 0.0, 0.0),
|
||||
)
|
||||
pre.integrate_sample(next_good)
|
||||
assert pre.current_preintegration().deltaTij() > snapshot_dt
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# AC-3: reset_for_new_keyframe is destructive.
|
||||
|
||||
|
||||
def test_ac3_reset_for_new_keyframe_is_destructive() -> None:
|
||||
# Arrange
|
||||
pre = make_imu_preintegrator(_calibration())
|
||||
samples = _make_samples(50)
|
||||
for s in samples:
|
||||
pre.integrate_sample(s)
|
||||
|
||||
# Act
|
||||
closed = pre.reset_for_new_keyframe()
|
||||
|
||||
# Assert — the closed factor carries the integration.
|
||||
assert closed.deltaTij() == pytest.approx((samples[-1].ts_ns - samples[0].ts_ns) * 1e-9)
|
||||
# Subsequent current_preintegration() raises.
|
||||
with pytest.raises(ImuPreintegrationError, match="no samples"):
|
||||
pre.current_preintegration()
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# AC-4: re-bias affects subsequent samples only.
|
||||
|
||||
|
||||
def test_ac4_rebias_affects_subsequent_samples_only() -> None:
|
||||
# Arrange — feed identical samples with two different biases; the
|
||||
# second-half integration must differ depending on bias_b's value.
|
||||
samples = _make_samples(50)
|
||||
bias_a = ImuBias(accel_bias=(0.0, 0.0, 0.0), gyro_bias=(0.0, 0.0, 0.0))
|
||||
bias_b = ImuBias(accel_bias=(1.0, 0.0, 0.0), gyro_bias=(0.0, 0.0, 0.0))
|
||||
|
||||
pre_a_only = make_imu_preintegrator(_calibration())
|
||||
pre_a_only.reset_with_bias(bias_a)
|
||||
for s in samples:
|
||||
pre_a_only.integrate_sample(s)
|
||||
delta_p_a = np.asarray(pre_a_only.current_preintegration().deltaPij())
|
||||
|
||||
pre_b_only = make_imu_preintegrator(_calibration())
|
||||
pre_b_only.reset_with_bias(bias_b)
|
||||
for s in samples:
|
||||
pre_b_only.integrate_sample(s)
|
||||
delta_p_b = np.asarray(pre_b_only.current_preintegration().deltaPij())
|
||||
|
||||
# Act / Assert — different bias → different integrated translation.
|
||||
# This proves bias is applied per-segment, validating the consumer's
|
||||
# contract that calling reset_with_bias mid-flight produces a
|
||||
# bias-aware integration of the new segment only.
|
||||
assert not np.allclose(delta_p_a, delta_p_b)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# AC-5: determinism — two instances, same input → deep-equal factors.
|
||||
|
||||
|
||||
def test_ac5_determinism_across_instances() -> None:
|
||||
# Arrange
|
||||
calibration = _calibration()
|
||||
samples = _make_samples(80, start_ts_ns=1_000_000_000)
|
||||
|
||||
pre_1 = make_imu_preintegrator(calibration)
|
||||
pre_2 = make_imu_preintegrator(calibration)
|
||||
|
||||
# Act
|
||||
for s in samples:
|
||||
pre_1.integrate_sample(s)
|
||||
pre_2.integrate_sample(s)
|
||||
|
||||
pim_1 = pre_1.current_preintegration()
|
||||
pim_2 = pre_2.current_preintegration()
|
||||
|
||||
# Assert
|
||||
assert pim_1.deltaTij() == pim_2.deltaTij()
|
||||
np.testing.assert_array_equal(np.asarray(pim_1.deltaPij()), np.asarray(pim_2.deltaPij()))
|
||||
np.testing.assert_array_equal(np.asarray(pim_1.deltaVij()), np.asarray(pim_2.deltaVij()))
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# AC-6: no lock acquisition on the integration path.
|
||||
|
||||
|
||||
def test_ac6_no_internal_locks() -> None:
|
||||
# Arrange
|
||||
module_path = (
|
||||
Path(__file__).resolve().parents[2]
|
||||
/ "src"
|
||||
/ "gps_denied_onboard"
|
||||
/ "helpers"
|
||||
/ "imu_preintegrator.py"
|
||||
)
|
||||
source = module_path.read_text()
|
||||
|
||||
# Act / Assert — no Lock / RLock / Semaphore / mutex appears in source.
|
||||
for symbol in ("threading.Lock", "threading.RLock", "Semaphore", "mutex"):
|
||||
assert symbol not in source, f"imu_preintegrator must be lock-free (found {symbol!r})"
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# AC-7: no upward imports.
|
||||
|
||||
|
||||
def test_ac7_no_upward_imports() -> None:
|
||||
# Arrange
|
||||
module_path = (
|
||||
Path(__file__).resolve().parents[2]
|
||||
/ "src"
|
||||
/ "gps_denied_onboard"
|
||||
/ "helpers"
|
||||
/ "imu_preintegrator.py"
|
||||
)
|
||||
tree = ast.parse(module_path.read_text())
|
||||
|
||||
# Act
|
||||
forbidden: list[str] = []
|
||||
for node in ast.walk(tree):
|
||||
if isinstance(node, ast.Import):
|
||||
forbidden.extend(
|
||||
alias.name for alias in node.names if "gps_denied_onboard.components" in alias.name
|
||||
)
|
||||
elif isinstance(node, ast.ImportFrom):
|
||||
if node.module and "gps_denied_onboard.components" in node.module:
|
||||
forbidden.append(node.module)
|
||||
|
||||
# Assert
|
||||
assert not forbidden, f"imu_preintegrator must not import components.*: {forbidden}"
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Additional guards.
|
||||
|
||||
|
||||
def test_current_preintegration_after_reset_with_bias_raises() -> None:
|
||||
# Arrange
|
||||
pre = make_imu_preintegrator(_calibration())
|
||||
for s in _make_samples(5):
|
||||
pre.integrate_sample(s)
|
||||
pre.reset_with_bias(ImuBias(accel_bias=(0.0, 0.0, 0.0), gyro_bias=(0.0, 0.0, 0.0)))
|
||||
|
||||
# Act / Assert — reset_with_bias also clears the accumulator.
|
||||
with pytest.raises(ImuPreintegrationError):
|
||||
pre.current_preintegration()
|
||||
|
||||
|
||||
def test_integrate_window_propagates_through_samples() -> None:
|
||||
# Arrange
|
||||
pre = make_imu_preintegrator(_calibration())
|
||||
samples = _make_samples(25)
|
||||
window = ImuWindow(samples=samples, ts_start_ns=samples[0].ts_ns, ts_end_ns=samples[-1].ts_ns)
|
||||
|
||||
# Act
|
||||
pre.integrate_window(window)
|
||||
|
||||
# Assert
|
||||
pim = pre.current_preintegration()
|
||||
assert pim.deltaTij() == pytest.approx((samples[-1].ts_ns - samples[0].ts_ns) * 1e-9)
|
||||
|
||||
|
||||
def test_imu_preintegrator_is_an_instance_type() -> None:
|
||||
# Arrange / Act
|
||||
pre = make_imu_preintegrator(_calibration())
|
||||
|
||||
# Assert — factory returns the documented public type.
|
||||
assert isinstance(pre, ImuPreintegrator)
|
||||
|
||||
|
||||
def test_combined_imu_factor_re_export_is_callable() -> None:
|
||||
# Assert — re-export resolves to GTSAM's CombinedImuFactor class.
|
||||
assert CombinedImuFactor.__name__ == "CombinedImuFactor"
|
||||
@@ -0,0 +1,252 @@
|
||||
"""AZ-278 — `LightGlueRuntime` AC suite (E-CC-HELPERS / R14 fix).
|
||||
|
||||
Covers the 7 ACs from `_docs/02_tasks/todo/AZ-278_lightglue_runtime.md`.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import ast
|
||||
import threading
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from gps_denied_onboard._types.matching import CorrespondenceSet, KeypointSet
|
||||
from gps_denied_onboard.helpers import (
|
||||
LightGlueConcurrentAccessError,
|
||||
LightGlueRuntime,
|
||||
LightGlueRuntimeError,
|
||||
)
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Test doubles — deterministic stub engines.
|
||||
|
||||
|
||||
@dataclass
|
||||
class _DeterministicStubEngine:
|
||||
"""Deterministic stub: returns a correspondence per keypoint pair index."""
|
||||
|
||||
expected_dim: int = 256
|
||||
block_event: threading.Event | None = None
|
||||
|
||||
@property
|
||||
def descriptor_dim(self) -> int:
|
||||
return self.expected_dim
|
||||
|
||||
def forward(self, features_a: KeypointSet, features_b: KeypointSet) -> CorrespondenceSet:
|
||||
# Optional barrier so test_ac4 can hold the first thread inside forward()
|
||||
# long enough for the second thread to race.
|
||||
if self.block_event is not None:
|
||||
self.block_event.wait()
|
||||
n = min(features_a.keypoints.shape[0], features_b.keypoints.shape[0])
|
||||
corr = np.hstack(
|
||||
[
|
||||
features_a.keypoints[:n].astype(np.float64),
|
||||
features_b.keypoints[:n].astype(np.float64),
|
||||
]
|
||||
)
|
||||
scores = np.linspace(0.5, 0.95, num=n, dtype=np.float64)
|
||||
return CorrespondenceSet(correspondences=corr, scores=scores)
|
||||
|
||||
|
||||
def _make_keypoints(n: int = 5, seed: int = 0, dim: int = 256) -> KeypointSet:
|
||||
rng = np.random.default_rng(seed)
|
||||
keypoints = rng.uniform(0, 1000, size=(n, 2)).astype(np.float32)
|
||||
descriptors = rng.standard_normal((n, dim)).astype(np.float32)
|
||||
return KeypointSet(keypoints=keypoints, descriptors=descriptors)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# AC-1: single-pair match returns non-empty correspondences.
|
||||
|
||||
|
||||
def test_ac1_single_pair_match() -> None:
|
||||
# Arrange
|
||||
runtime = LightGlueRuntime(_DeterministicStubEngine())
|
||||
a = _make_keypoints(n=10, seed=1)
|
||||
b = _make_keypoints(n=10, seed=2)
|
||||
|
||||
# Act
|
||||
result = runtime.match(a, b)
|
||||
|
||||
# Assert
|
||||
assert isinstance(result, CorrespondenceSet)
|
||||
assert result.correspondences.shape == (10, 4)
|
||||
assert result.scores.shape == (10,)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# AC-2: batch of 3 pairs returns 3 ordered results.
|
||||
|
||||
|
||||
def test_ac2_batch_match_preserves_order() -> None:
|
||||
# Arrange
|
||||
runtime = LightGlueRuntime(_DeterministicStubEngine())
|
||||
pairs_a = [_make_keypoints(n=5, seed=i) for i in range(3)]
|
||||
pairs_b = [_make_keypoints(n=5, seed=i + 100) for i in range(3)]
|
||||
|
||||
# Act
|
||||
results = runtime.match_batch(pairs_a, pairs_b)
|
||||
|
||||
# Assert
|
||||
assert len(results) == 3
|
||||
for idx, (pair_a, pair_b, result) in enumerate(zip(pairs_a, pairs_b, results, strict=True)):
|
||||
# Each result's first 2 columns must echo features_a[:n].keypoints for that pair.
|
||||
(
|
||||
np.testing.assert_array_equal(
|
||||
result.correspondences[:, :2], pair_a.keypoints.astype(np.float64)
|
||||
),
|
||||
f"batch result {idx} lost input order",
|
||||
)
|
||||
np.testing.assert_array_equal(
|
||||
result.correspondences[:, 2:], pair_b.keypoints.astype(np.float64)
|
||||
)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# AC-3: descriptor-dim mismatch raises with both dims.
|
||||
|
||||
|
||||
def test_ac3_descriptor_dim_mismatch() -> None:
|
||||
# Arrange — engine expects 256, we feed 128.
|
||||
runtime = LightGlueRuntime(_DeterministicStubEngine(expected_dim=256))
|
||||
a = _make_keypoints(n=5, dim=128)
|
||||
b = _make_keypoints(n=5, dim=128)
|
||||
|
||||
# Act / Assert
|
||||
with pytest.raises(LightGlueRuntimeError, match=r"256.*128|128.*256"):
|
||||
runtime.match(a, b)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# AC-4: concurrent access raises LightGlueConcurrentAccessError in second thread.
|
||||
|
||||
|
||||
def test_ac4_concurrent_access_rejected() -> None:
|
||||
# Arrange — block the first call inside forward() so the second can race.
|
||||
barrier = threading.Event()
|
||||
engine = _DeterministicStubEngine(block_event=barrier)
|
||||
runtime = LightGlueRuntime(engine)
|
||||
a = _make_keypoints(n=3, seed=1)
|
||||
b = _make_keypoints(n=3, seed=2)
|
||||
|
||||
results: list[CorrespondenceSet | Exception] = []
|
||||
|
||||
def worker_one() -> None:
|
||||
try:
|
||||
results.append(runtime.match(a, b))
|
||||
except Exception as exc:
|
||||
results.append(exc)
|
||||
|
||||
def worker_two() -> None:
|
||||
try:
|
||||
results.append(runtime.match(a, b))
|
||||
except Exception as exc:
|
||||
results.append(exc)
|
||||
|
||||
t1 = threading.Thread(target=worker_one)
|
||||
t1.start()
|
||||
# Give thread 1 time to enter forward() and hit the barrier.
|
||||
threading.Event().wait(0.05)
|
||||
t2 = threading.Thread(target=worker_two)
|
||||
t2.start()
|
||||
t2.join(timeout=2.0) # t2 should NOT block — guard raises immediately
|
||||
barrier.set()
|
||||
t1.join(timeout=2.0)
|
||||
|
||||
# Assert — exactly one success and one LightGlueConcurrentAccessError.
|
||||
assert len(results) == 2
|
||||
successes = [r for r in results if isinstance(r, CorrespondenceSet)]
|
||||
failures = [r for r in results if isinstance(r, LightGlueConcurrentAccessError)]
|
||||
assert len(successes) == 1, f"expected exactly one success, got results={results!r}"
|
||||
assert len(failures) == 1, f"expected exactly one concurrent-access error, got {results!r}"
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# AC-5: construction-time guard.
|
||||
|
||||
|
||||
def test_ac5_construction_with_none_engine_raises() -> None:
|
||||
# Act / Assert
|
||||
with pytest.raises(LightGlueRuntimeError, match="engine_handle"):
|
||||
LightGlueRuntime(engine_handle=None) # type: ignore[arg-type]
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# AC-6: no upward imports.
|
||||
|
||||
|
||||
def test_ac6_no_upward_imports() -> None:
|
||||
# Arrange
|
||||
module_path = (
|
||||
Path(__file__).resolve().parents[2]
|
||||
/ "src"
|
||||
/ "gps_denied_onboard"
|
||||
/ "helpers"
|
||||
/ "lightglue_runtime.py"
|
||||
)
|
||||
tree = ast.parse(module_path.read_text())
|
||||
|
||||
# Act
|
||||
forbidden: list[str] = []
|
||||
for node in ast.walk(tree):
|
||||
if isinstance(node, ast.Import):
|
||||
forbidden.extend(
|
||||
alias.name for alias in node.names if "gps_denied_onboard.components" in alias.name
|
||||
)
|
||||
elif isinstance(node, ast.ImportFrom):
|
||||
if node.module and "gps_denied_onboard.components" in node.module:
|
||||
forbidden.append(node.module)
|
||||
|
||||
# Assert — R14 structural fix: no components.* imports.
|
||||
assert not forbidden, f"lightglue_runtime must not import components.*: {forbidden}"
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# AC-7: determinism downstream of the engine.
|
||||
|
||||
|
||||
def test_ac7_determinism_byte_equal_outputs() -> None:
|
||||
# Arrange
|
||||
runtime = LightGlueRuntime(_DeterministicStubEngine())
|
||||
a = _make_keypoints(n=8, seed=42)
|
||||
b = _make_keypoints(n=8, seed=43)
|
||||
|
||||
# Act
|
||||
r1 = runtime.match(a, b)
|
||||
r2 = runtime.match(a, b)
|
||||
|
||||
# Assert
|
||||
np.testing.assert_array_equal(r1.correspondences, r2.correspondences)
|
||||
np.testing.assert_array_equal(r1.scores, r2.scores)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Additional guards.
|
||||
|
||||
|
||||
def test_construction_with_bad_descriptor_dim_raises() -> None:
|
||||
# Act / Assert
|
||||
with pytest.raises(LightGlueRuntimeError, match="descriptor_dim"):
|
||||
LightGlueRuntime(_DeterministicStubEngine(expected_dim=0))
|
||||
|
||||
|
||||
def test_descriptor_dim_accessor() -> None:
|
||||
# Arrange / Act
|
||||
runtime = LightGlueRuntime(_DeterministicStubEngine(expected_dim=128))
|
||||
|
||||
# Assert
|
||||
assert runtime.descriptor_dim() == 128
|
||||
|
||||
|
||||
def test_match_batch_length_mismatch_raises() -> None:
|
||||
# Arrange
|
||||
runtime = LightGlueRuntime(_DeterministicStubEngine())
|
||||
a_list = [_make_keypoints(n=3, seed=1)]
|
||||
b_list = [_make_keypoints(n=3, seed=2), _make_keypoints(n=3, seed=3)]
|
||||
|
||||
# Act / Assert
|
||||
with pytest.raises(LightGlueRuntimeError, match="equal length"):
|
||||
runtime.match_batch(a_list, b_list)
|
||||
@@ -0,0 +1,290 @@
|
||||
"""AZ-282 — `RansacFilter` AC suite (E-CC-HELPERS).
|
||||
|
||||
Covers the 10 ACs from `_docs/02_tasks/todo/AZ-282_ransac_filter.md` plus
|
||||
the contract's "no upward imports" Layer 1 invariant via AST inspection.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import ast
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from gps_denied_onboard.helpers import (
|
||||
RansacFilter,
|
||||
RansacFilterError,
|
||||
RansacResult,
|
||||
)
|
||||
from gps_denied_onboard.helpers.se3_utils import SE3, matrix_to_se3
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Fixtures
|
||||
|
||||
|
||||
def _make_homography_correspondences(
|
||||
n: int, seed: int = 42, *, pure_translation: bool = False
|
||||
) -> tuple[np.ndarray, np.ndarray]:
|
||||
"""Return (correspondences, H) for ``n`` points warped through a fixed homography.
|
||||
|
||||
``pure_translation`` uses a translation-only H so cv2's fit lands at
|
||||
exactly the ground truth — used by the AC-1 atol=1e-6 zero-residual
|
||||
test. Other tests use the default mild projective transform.
|
||||
"""
|
||||
rng = np.random.default_rng(seed)
|
||||
pts_a = rng.uniform(50.0, 950.0, size=(n, 2)).astype(np.float64)
|
||||
if pure_translation:
|
||||
H = np.array([[1.0, 0.0, 30.0], [0.0, 1.0, -20.0], [0.0, 0.0, 1.0]], dtype=np.float64)
|
||||
else:
|
||||
H = np.array(
|
||||
[
|
||||
[1.0, 0.05, 30.0],
|
||||
[-0.03, 1.0, -20.0],
|
||||
[0.0, 0.0, 1.0],
|
||||
],
|
||||
dtype=np.float64,
|
||||
)
|
||||
pts_a_h = np.hstack([pts_a, np.ones((n, 1))])
|
||||
pts_b_h = (H @ pts_a_h.T).T
|
||||
pts_b = pts_b_h[:, :2] / pts_b_h[:, 2:3]
|
||||
correspondences = np.hstack([pts_a, pts_b])
|
||||
return correspondences, H
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# AC-1: clean correspondences → 100 % inliers + ~0 residual.
|
||||
|
||||
|
||||
def test_ac1_clean_correspondences_all_inliers() -> None:
|
||||
# Arrange — pure translation H so cv2's homography fit hits the
|
||||
# ground truth exactly and the AC-1 atol=1e-6 zero-residual gate holds.
|
||||
correspondences, _H = _make_homography_correspondences(n=100, pure_translation=True)
|
||||
|
||||
# Act
|
||||
result = RansacFilter.filter_correspondences(
|
||||
correspondences, ransac_threshold_px=1.5, min_inliers=50
|
||||
)
|
||||
|
||||
# Assert
|
||||
assert isinstance(result, RansacResult)
|
||||
assert result.inlier_count == 100
|
||||
assert result.outlier_count == 0
|
||||
assert result.median_residual_px == pytest.approx(0.0, abs=1e-6)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# AC-2: 80 inliers + 20 outliers → inlier count in [78, 82].
|
||||
|
||||
|
||||
def test_ac2_mixed_correspondences_band() -> None:
|
||||
# Arrange
|
||||
clean, _H = _make_homography_correspondences(n=80, seed=7)
|
||||
# 20 outliers: random noise unrelated to H.
|
||||
rng = np.random.default_rng(7)
|
||||
outliers_a = rng.uniform(50.0, 950.0, size=(20, 2))
|
||||
outliers_b = rng.uniform(50.0, 950.0, size=(20, 2))
|
||||
outliers = np.hstack([outliers_a, outliers_b])
|
||||
correspondences = np.vstack([clean, outliers])
|
||||
|
||||
# Act
|
||||
result = RansacFilter.filter_correspondences(
|
||||
correspondences, ransac_threshold_px=1.5, min_inliers=50
|
||||
)
|
||||
|
||||
# Assert
|
||||
assert 78 <= result.inlier_count <= 82
|
||||
assert result.outlier_count == 100 - result.inlier_count
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# AC-3: determinism — same input twice yields byte-equal RansacResult.
|
||||
|
||||
|
||||
def test_ac3_determinism_byte_equal_outputs() -> None:
|
||||
# Arrange
|
||||
clean, _H = _make_homography_correspondences(n=80, seed=11)
|
||||
rng = np.random.default_rng(11)
|
||||
outliers = rng.uniform(50.0, 950.0, size=(20, 4))
|
||||
correspondences = np.vstack([clean, outliers])
|
||||
|
||||
# Act
|
||||
r1 = RansacFilter.filter_correspondences(correspondences, 1.5, 50)
|
||||
r2 = RansacFilter.filter_correspondences(correspondences, 1.5, 50)
|
||||
|
||||
# Assert
|
||||
assert r1.inlier_count == r2.inlier_count
|
||||
assert r1.outlier_count == r2.outlier_count
|
||||
np.testing.assert_array_equal(r1.inlier_correspondences, r2.inlier_correspondences)
|
||||
assert r1.median_residual_px == r2.median_residual_px
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# AC-4: reprojection residual ~ 0 on clean inliers + known pose.
|
||||
|
||||
|
||||
def test_ac4_reprojection_residual_zero_on_clean_pose() -> None:
|
||||
# Arrange
|
||||
# Identity pose. Pixel (x, y) back-projected to z=1 ray through K, then
|
||||
# re-projected through K with R=I, t=0 must land back on (x, y).
|
||||
K = np.array([[800.0, 0.0, 320.0], [0.0, 800.0, 240.0], [0.0, 0.0, 1.0]])
|
||||
distortion = np.zeros(5, dtype=np.float64)
|
||||
pose = matrix_to_se3(np.eye(4, dtype=np.float64))
|
||||
|
||||
pts = np.array(
|
||||
[
|
||||
[100.0, 150.0, 100.0, 150.0],
|
||||
[200.0, 300.0, 200.0, 300.0],
|
||||
[400.0, 450.0, 400.0, 450.0],
|
||||
[500.0, 200.0, 500.0, 200.0],
|
||||
]
|
||||
)
|
||||
|
||||
# Act
|
||||
residual = RansacFilter.compute_reprojection_residual(pts, K, distortion, pose)
|
||||
|
||||
# Assert
|
||||
assert residual == pytest.approx(0.0, abs=1e-6)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# AC-5: empty inlier array → NaN (no exception).
|
||||
|
||||
|
||||
def test_ac5_empty_inliers_returns_nan() -> None:
|
||||
# Arrange
|
||||
empty = np.empty((0, 4), dtype=np.float64)
|
||||
K = np.eye(3, dtype=np.float64)
|
||||
distortion = np.zeros(5, dtype=np.float64)
|
||||
pose = matrix_to_se3(np.eye(4, dtype=np.float64))
|
||||
|
||||
# Act
|
||||
residual = RansacFilter.compute_reprojection_residual(empty, K, distortion, pose)
|
||||
|
||||
# Assert
|
||||
assert np.isnan(residual)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# AC-6: shape (N, 3) raises with shape message.
|
||||
|
||||
|
||||
def test_ac6_invalid_correspondence_shape() -> None:
|
||||
# Arrange
|
||||
bad = np.zeros((10, 3), dtype=np.float64)
|
||||
|
||||
# Act / Assert
|
||||
with pytest.raises(RansacFilterError, match=r"\(N, 4\)"):
|
||||
RansacFilter.filter_correspondences(bad, 1.5, 4)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# AC-7: non-positive threshold raises.
|
||||
|
||||
|
||||
def test_ac7_non_positive_threshold() -> None:
|
||||
# Arrange
|
||||
correspondences, _H = _make_homography_correspondences(n=10)
|
||||
|
||||
# Act / Assert
|
||||
with pytest.raises(RansacFilterError, match="positive"):
|
||||
RansacFilter.filter_correspondences(correspondences, -1.0, 4)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# AC-8: fewer than 4 correspondences raises.
|
||||
|
||||
|
||||
def test_ac8_too_few_points() -> None:
|
||||
# Arrange
|
||||
too_few = np.zeros((3, 4), dtype=np.float64)
|
||||
|
||||
# Act / Assert
|
||||
with pytest.raises(RansacFilterError, match="4"):
|
||||
RansacFilter.filter_correspondences(too_few, 1.5, 0)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# AC-9: K shape mismatch in residual call.
|
||||
|
||||
|
||||
def test_ac9_K_shape_mismatch() -> None:
|
||||
# Arrange
|
||||
pts = np.zeros((4, 4), dtype=np.float64)
|
||||
bad_K = np.eye(4, dtype=np.float64)
|
||||
distortion = np.zeros(5, dtype=np.float64)
|
||||
pose = matrix_to_se3(np.eye(4, dtype=np.float64))
|
||||
|
||||
# Act / Assert
|
||||
with pytest.raises(RansacFilterError, match=r"\(3, 3\)"):
|
||||
RansacFilter.compute_reprojection_residual(pts, bad_K, distortion, pose)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# AC-10: Layer 1 invariant — no `components.*` imports.
|
||||
|
||||
|
||||
def test_ac10_no_upward_imports() -> None:
|
||||
# Arrange
|
||||
module_path = (
|
||||
Path(__file__).resolve().parents[2]
|
||||
/ "src"
|
||||
/ "gps_denied_onboard"
|
||||
/ "helpers"
|
||||
/ "ransac_filter.py"
|
||||
)
|
||||
source = module_path.read_text()
|
||||
tree = ast.parse(source)
|
||||
|
||||
# Act
|
||||
forbidden: list[str] = []
|
||||
for node in ast.walk(tree):
|
||||
if isinstance(node, ast.Import):
|
||||
forbidden.extend(
|
||||
alias.name for alias in node.names if "gps_denied_onboard.components" in alias.name
|
||||
)
|
||||
elif isinstance(node, ast.ImportFrom):
|
||||
if node.module and "gps_denied_onboard.components" in node.module:
|
||||
forbidden.append(node.module)
|
||||
|
||||
# Assert
|
||||
assert not forbidden, f"ransac_filter must not import components.*: {forbidden}"
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Additional guards: distortion shape contract.
|
||||
|
||||
|
||||
@pytest.mark.parametrize("dist_shape", [(3,), (4,), (7,), (10,)])
|
||||
def test_distortion_shape_contract(dist_shape: tuple[int, ...]) -> None:
|
||||
# Arrange
|
||||
pts = np.zeros((4, 4), dtype=np.float64)
|
||||
K = np.eye(3, dtype=np.float64)
|
||||
distortion = np.zeros(dist_shape, dtype=np.float64)
|
||||
pose = matrix_to_se3(np.eye(4, dtype=np.float64))
|
||||
|
||||
# Act / Assert
|
||||
with pytest.raises(RansacFilterError, match=r"\(5,\) or \(8,\)"):
|
||||
RansacFilter.compute_reprojection_residual(pts, K, distortion, pose)
|
||||
|
||||
|
||||
def test_returns_frozen_dataclass() -> None:
|
||||
import dataclasses
|
||||
|
||||
# Arrange
|
||||
correspondences, _H = _make_homography_correspondences(n=20, seed=3)
|
||||
|
||||
# Act
|
||||
result = RansacFilter.filter_correspondences(correspondences, 1.5, 4)
|
||||
|
||||
# Assert
|
||||
with pytest.raises(dataclasses.FrozenInstanceError):
|
||||
result.inlier_count = 999 # type: ignore[misc]
|
||||
|
||||
|
||||
def test_se3_alias_consistency() -> None:
|
||||
# Arrange / Act
|
||||
pose = matrix_to_se3(np.eye(4, dtype=np.float64))
|
||||
|
||||
# Assert
|
||||
assert isinstance(pose, SE3)
|
||||
Reference in New Issue
Block a user