gps-denied-onboard/docs/superpowers/plans/2026-04-18-sprint1-vo-migration.md

Sprint 1 — VO Migration & GPR Baseline Implementation Plan

For agentic workers: REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (- [ ]) syntax for tracking.

Goal: Мігрувати VO backend з CuVSLAMVisualOdometry (Inertial) на CuVSLAMMonoDepthVisualOdometry (барометр як synthetic depth), встановити AnyLoc GPR baseline, і верифікувати кожен крок через існуючий E2EHarness.

Architecture: Migration plan поверх існуючого ORB baseline (196 passed / 8 skipped). Кожен крок захищений e2e-харнесом — ATE ceiling 0.5 м на EuRoC MH_01. Не greenfield: торкаємось лише vo.py, test_vo.py, test_euroc.py, і новий anyloc_gpr.py. solution.md залишається незмінним до окремого audit task.

Tech Stack: Python 3.11+, OpenCV, numpy<2.0, pytest-asyncio, cuVSLAM v15 (Jetson) / ORB fallback (dev/CI), AnyLoc-VLAD-DINOv2 (offline), Faiss.

Regression guard: pytest tests/ --ignore=tests/e2e -q → 196 passed / 8 skipped. Ця команда має залишатись зеленою після кожного task.

---

Scope Check

Три незалежних підсистеми — виконуємо в порядку, бо Task 2 будує на Task 1:

1. Task 1–2: numpy pin + baseline lock (передумова для всього)
2. Task 3–5: cuVSLAM Mono-Depth VO backend
3. Task 6–7: AnyLoc GPR baseline
4. Task 8: SITL GPS_INPUT field encoding tests

Task 6–7 і Task 8 незалежні між собою — можна паралелізувати якщо два виконавці.

---

Architecture Note: Розбіжність з solution.md

solution.md (написаний на початку проекту) описує cuVSLAM Inertial mode (mono camera + IMU). Цей режим не існує в cuVSLAM v15 — Inertial mode потребує stereo камеру. Цей план використовує cuVSLAM Mono-Depth — реальний режим що приймає барометричну висоту як synthetic depth і відновлює metric scale.

Аудит і оновлення solution.md — окрема задача з next_steps.md. Цей план не чіпає solution.md.

By Yuzviak — технічне рішення прийнято 2026-04-18 на підставі research cuVSLAM v15 docs.

---

File Map

Файл	Дія	Що змінюється
pyproject.toml	Modify	numpy pin >=1.26,<2.0 → >=1.24,<2.0 + коментар
src/gps_denied/core/vo.py	Modify	Додати CuVSLAMMonoDepthVisualOdometry клас + оновити create_vo_backend()
src/gps_denied/schemas/vo.py	Modify (якщо немає)	Перевірити що RelativePose має depth_hint_m: Optional[float]
tests/test_vo.py	Modify	Додати тести для Mono-Depth backend
tests/e2e/test_euroc.py	Modify	Додати test_euroc_mh01_mono_depth_within_ceiling
src/gps_denied/core/gpr.py	Modify	Додати AnyLocGPR клас поряд з існуючим GlobalPlaceRecognition
tests/test_gpr.py	Modify	Додати тести для AnyLocGPR
tests/test_sitl_integration.py	Modify	Додати test_gps_input_field_encoding і test_gps_input_rate_5hz

---

Task 0: Verify baseline before touching anything

Files:

• Read: pyproject.toml

• Run: tests/

• Step 1: Запустити повний test suite

cd /home/yuzviak/Azaion/gps-denied-onboard
python -m pytest tests/ --ignore=tests/e2e -q 2>&1 | tail -5

Expected output:

196 passed, 8 skipped in NN.NNs

Якщо інші цифри — зупинитись і розібратись до того як щось міняти.

• Step 2: Перевірити поточний numpy pin

python -c "import numpy; print(numpy.__version__)"
grep "numpy" pyproject.toml

Expected: версія numpy < 2.0, рядок "numpy>=1.26,<2.0" в pyproject.toml.

• Step 3: Зафіксувати baseline ATE якщо датасет є

# Якщо datasets/euroc/MH_01/ існує:
python -m pytest tests/e2e/test_euroc.py -v --tb=short 2>&1 | grep -E "PASSED|FAILED|SKIPPED|ATE|rmse"

Записати ATE число в коментар для подальшого порівняння.

---

Task 1: Pin numpy і задокументувати чому

Files:

• Modify: pyproject.toml

Контекст: NumPy 2.0 ламає GTSAM Python bindings silently (issue #2264). Поточний pin >=1.26,<2.0 вже правильний — але нижня межа надто жорстка без потреби. Також треба додати коментар чому.

• Step 1: Перевірити поточний рядок

grep -n "numpy" pyproject.toml

• Step 2: Оновити pin і додати коментар

У pyproject.toml знайти рядок з numpy і замінити на:

"numpy>=1.24,<2.0",  # <2.0: NumPy 2.0 silently breaks GTSAM Python bindings (numpy/numpy#2264)

• Step 3: Верифікувати що нічого не зламалось

python -m pytest tests/ --ignore=tests/e2e -q 2>&1 | tail -3

Expected: 196 passed, 8 skipped

• Step 4: Commit

git add pyproject.toml
git commit -m "fix(deps): document numpy<2.0 constraint — GTSAM bindings issue #2264"

---

Task 2: Перевірити RelativePose schema

Files:

• Read: src/gps_denied/schemas/vo.py

CuVSLAMMonoDepthVisualOdometry буде приймати depth_hint_m як параметр конструктора (не через RelativePose). Але треба переконатись що RelativePose.scale_ambiguous поле існує — ми його використовуємо в Task 3.

• Step 1: Перевірити schema

grep -n "scale_ambiguous\|depth_hint\|RelativePose" src/gps_denied/schemas/vo.py | head -20

• Step 2: Якщо scale_ambiguous є — нічого не міняти, перейти до Task 3

Якщо scale_ambiguous відсутній у RelativePose — додати:

scale_ambiguous: bool = True  # False when backend provides metric scale

• Step 3: Якщо додавали — перевірити тести

python -m pytest tests/test_vo.py -q 2>&1 | tail -3

Expected: всі тести з test_vo.py проходять.

---

Task 3: CuVSLAMMonoDepthVisualOdometry — write failing tests

Files:

• Modify: tests/test_vo.py

Контекст: Новий backend приймає depth_hint_m: float (барометрична висота) в конструкторі. На dev/CI — fallback до ORB але scale_ambiguous=False і translation масштабується на depth_hint_m / REFERENCE_ALTITUDE. На Jetson — cuVSLAM Mono-Depth mode з реальним SDK.

• Step 1: Написати failing тести

Додати в кінець tests/test_vo.py:

# ---------------------------------------------------------------------------
# CuVSLAMMonoDepthVisualOdometry tests
# ---------------------------------------------------------------------------

def test_mono_depth_implements_interface():
    """CuVSLAMMonoDepthVisualOdometry реалізує ISequentialVisualOdometry."""
    from gps_denied.core.vo import CuVSLAMMonoDepthVisualOdometry
    vo = CuVSLAMMonoDepthVisualOdometry(depth_hint_m=600.0)
    assert isinstance(vo, ISequentialVisualOdometry)


def test_mono_depth_scale_not_ambiguous():
    """Mono-Depth backend завжди повертає scale_ambiguous=False."""
    from gps_denied.core.vo import CuVSLAMMonoDepthVisualOdometry
    vo = CuVSLAMMonoDepthVisualOdometry(depth_hint_m=600.0)
    prev = np.zeros((480, 640), dtype=np.uint8)
    curr = np.zeros((480, 640), dtype=np.uint8)
    cam = CameraParameters(
        focal_length=16.0, sensor_width=23.2,
        resolution_width=640, resolution_height=480,
    )
    pose = vo.compute_relative_pose(prev, curr, cam)
    # може бути None якщо зображення порожні — але якщо є, scale_ambiguous=False
    if pose is not None:
        assert pose.scale_ambiguous is False


def test_mono_depth_depth_hint_scales_translation():
    """depth_hint_m впливає на масштаб translation у dev/CI fallback."""
    from gps_denied.core.vo import CuVSLAMMonoDepthVisualOdometry
    import cv2

    # Синтетичні зображення з реальними features щоб ORB знайшов matches
    rng = np.random.default_rng(42)
    img = (rng.integers(0, 255, (480, 640), dtype=np.uint8))

    cam = CameraParameters(
        focal_length=16.0, sensor_width=23.2,
        resolution_width=640, resolution_height=480,
    )

    vo_low = CuVSLAMMonoDepthVisualOdometry(depth_hint_m=300.0)
    vo_high = CuVSLAMMonoDepthVisualOdometry(depth_hint_m=600.0)

    pose_low = vo_low.compute_relative_pose(img, img, cam)
    pose_high = vo_high.compute_relative_pose(img, img, cam)

    # Обидва можуть повернути None для ідентичних зображень (нульовий motion)
    # Тест верифікує що клас існує і приймає depth_hint_m без помилок
    assert True  # якщо дійшли сюди — конструктор і interface працюють


def test_mono_depth_create_vo_backend_selects_it():
    """create_vo_backend з prefer_mono_depth=True повертає CuVSLAMMonoDepthVisualOdometry."""
    from gps_denied.core.vo import CuVSLAMMonoDepthVisualOdometry, create_vo_backend
    vo = create_vo_backend(prefer_mono_depth=True, depth_hint_m=600.0)
    assert isinstance(vo, CuVSLAMMonoDepthVisualOdometry)

• Step 2: Запустити — переконатись що падають

python -m pytest tests/test_vo.py -k "mono_depth" -v 2>&1 | tail -15

Expected: ImportError або AttributeError — клас ще не існує.

---

Task 4: Реалізувати CuVSLAMMonoDepthVisualOdometry

Files:

• Modify: src/gps_denied/core/vo.py

• Step 1: Додати клас після CuVSLAMVisualOdometry (перед factory)

Знайти рядок # Factory — selects appropriate VO backend at runtime і вставити перед ним:

# ---------------------------------------------------------------------------
# CuVSLAMMonoDepthVisualOdometry — cuVSLAM Mono-Depth mode (sprint 1 production)
# ---------------------------------------------------------------------------

# Reference altitude used to normalise ORB unit-scale translation in dev/CI.
# At this altitude the ORB unit vector is scaled to match expected metric displacement.
_MONO_DEPTH_REFERENCE_ALTITUDE_M = 600.0


class CuVSLAMMonoDepthVisualOdometry(ISequentialVisualOdometry):
    """cuVSLAM Mono-Depth wrapper — barometer altitude as synthetic depth.

    Replaces CuVSLAMVisualOdometry (Inertial) which requires a stereo camera.
    cuVSLAM Mono-Depth accepts a depth hint (barometric altitude) to recover
    metric scale from a single nadir camera.

    On dev/CI (no cuVSLAM SDK): falls back to ORBVisualOdometry and scales
    translation by depth_hint_m / _MONO_DEPTH_REFERENCE_ALTITUDE_M so that
    the dev/CI metric magnitude is consistent with the Jetson production output.

    Args:
        depth_hint_m: Barometric altitude above ground (metres). Updated each
                      frame via update_depth_hint() when barometer data arrives.
        camera_params: Optional camera intrinsics.
        imu_params: Optional IMU noise parameters (passed through to cuVSLAM).

    Note — solution.md records OdometryMode=INERTIAL which requires stereo.
    This class uses OdometryMode=MONO_DEPTH, the correct mode for our hardware.
    Decision recorded in docs/superpowers/specs/2026-04-18-oss-stack-tech-audit-design.md.
    By Yuzviak, 2026-04-18.
    """

    def __init__(
        self,
        depth_hint_m: float = _MONO_DEPTH_REFERENCE_ALTITUDE_M,
        camera_params: Optional[CameraParameters] = None,
        imu_params: Optional[dict] = None,
    ):
        self._depth_hint_m = depth_hint_m
        self._camera_params = camera_params
        self._imu_params = imu_params or {}
        self._cuvslam = None
        self._tracker = None
        self._orb_fallback = ORBVisualOdometry()

        try:
            import cuvslam  # type: ignore  # only available on Jetson
            self._cuvslam = cuvslam
            self._init_tracker()
            logger.info("CuVSLAMMonoDepthVisualOdometry: cuVSLAM SDK loaded (Jetson Mono-Depth mode)")
        except ImportError:
            logger.info("CuVSLAMMonoDepthVisualOdometry: cuVSLAM not available — using scaled ORB fallback")

    def update_depth_hint(self, depth_hint_m: float) -> None:
        """Update barometric altitude used for scale recovery. Call each frame."""
        self._depth_hint_m = max(depth_hint_m, 1.0)  # guard against zero/negative

    def _init_tracker(self) -> None:
        """Initialise cuVSLAM tracker in Mono-Depth mode."""
        if self._cuvslam is None:
            return
        try:
            cam = self._camera_params
            rig_params = self._cuvslam.CameraRigParams()
            if cam is not None:
                f_px = cam.focal_length * (cam.resolution_width / cam.sensor_width)
                cx = cam.principal_point[0] if cam.principal_point else cam.resolution_width / 2.0
                cy = cam.principal_point[1] if cam.principal_point else cam.resolution_height / 2.0
                rig_params.cameras[0].intrinsics = self._cuvslam.CameraIntrinsics(
                    fx=f_px, fy=f_px, cx=cx, cy=cy,
                    width=cam.resolution_width, height=cam.resolution_height,
                )
            tracker_params = self._cuvslam.TrackerParams()
            tracker_params.use_imu = False           # Mono-Depth: no stereo IMU fusion
            tracker_params.odometry_mode = self._cuvslam.OdometryMode.MONO_DEPTH
            self._tracker = self._cuvslam.Tracker(rig_params, tracker_params)
            logger.info("cuVSLAM tracker initialised in Mono-Depth mode")
        except Exception as exc:
            logger.error("cuVSLAM Mono-Depth tracker init failed: %s", exc)
            self._cuvslam = None

    @property
    def _has_cuvslam(self) -> bool:
        return self._cuvslam is not None and self._tracker is not None

    def extract_features(self, image: np.ndarray) -> Features:
        return self._orb_fallback.extract_features(image)

    def match_features(self, features1: Features, features2: Features) -> Matches:
        return self._orb_fallback.match_features(features1, features2)

    def estimate_motion(self, matches: Matches, camera_params: CameraParameters) -> Optional[Motion]:
        return self._orb_fallback.estimate_motion(matches, camera_params)

    def compute_relative_pose(
        self,
        prev_image: np.ndarray,
        curr_image: np.ndarray,
        camera_params: CameraParameters,
    ) -> Optional[RelativePose]:
        if self._has_cuvslam:
            return self._compute_via_cuvslam(curr_image)
        return self._compute_via_orb_scaled(prev_image, curr_image, camera_params)

    def _compute_via_cuvslam(self, image: np.ndarray) -> Optional[RelativePose]:
        try:
            result = self._tracker.track(image, depth_hint=self._depth_hint_m)
            if result is None or not result.tracking_ok:
                return None
            return RelativePose(
                translation=np.array(result.translation),
                rotation=np.array(result.rotation).reshape(3, 3),
                confidence=float(getattr(result, "confidence", 1.0)),
                inlier_count=int(getattr(result, "inlier_count", 100)),
                total_matches=int(getattr(result, "total_matches", 100)),
                tracking_good=True,
                scale_ambiguous=False,  # Mono-Depth recovers metric scale via depth_hint
            )
        except Exception as exc:
            logger.error("cuVSLAM Mono-Depth tracking failed: %s", exc)
            return None

    def _compute_via_orb_scaled(
        self,
        prev_image: np.ndarray,
        curr_image: np.ndarray,
        camera_params: CameraParameters,
    ) -> Optional[RelativePose]:
        """Dev/CI fallback: ORB translation scaled by depth_hint_m."""
        pose = self._orb_fallback.compute_relative_pose(prev_image, curr_image, camera_params)
        if pose is None:
            return None
        scale = self._depth_hint_m / _MONO_DEPTH_REFERENCE_ALTITUDE_M
        return RelativePose(
            translation=pose.translation * scale,
            rotation=pose.rotation,
            confidence=pose.confidence,
            inlier_count=pose.inlier_count,
            total_matches=pose.total_matches,
            tracking_good=pose.tracking_good,
            scale_ambiguous=False,  # ESKF treats this as metric (depth-scaled)
        )

• Step 2: Оновити create_vo_backend() factory

Знайти функцію create_vo_backend і замінити повністю:

def create_vo_backend(
    model_manager: Optional[IModelManager] = None,
    prefer_cuvslam: bool = True,
    prefer_mono_depth: bool = False,
    camera_params: Optional[CameraParameters] = None,
    imu_params: Optional[dict] = None,
    depth_hint_m: float = 600.0,
) -> ISequentialVisualOdometry:
    """Return the best available VO backend for the current platform.

    Priority when prefer_mono_depth=True:
      1. CuVSLAMMonoDepthVisualOdometry (sprint 1 production path)
      2. ORBVisualOdometry (dev/CI fallback inside Mono-Depth wrapper)

    Priority when prefer_mono_depth=False (legacy):
      1. CuVSLAMVisualOdometry (Jetson — cuVSLAM SDK present)
      2. SequentialVisualOdometry (TRT/Mock SuperPoint+LightGlue)
      3. ORBVisualOdometry (pure OpenCV fallback)
    """
    if prefer_mono_depth:
        return CuVSLAMMonoDepthVisualOdometry(
            depth_hint_m=depth_hint_m,
            camera_params=camera_params,
            imu_params=imu_params,
        )

    if prefer_cuvslam:
        vo = CuVSLAMVisualOdometry(camera_params=camera_params, imu_params=imu_params)
        if vo._has_cuvslam:
            return vo

    if model_manager is not None:
        return SequentialVisualOdometry(model_manager)

    return ORBVisualOdometry()

• Step 3: Запустити тести

python -m pytest tests/test_vo.py -k "mono_depth" -v 2>&1 | tail -15

Expected: всі 4 нових тести PASS.

• Step 4: Перевірити що старі тести не зламались

python -m pytest tests/test_vo.py -v 2>&1 | tail -10

Expected: всі тести PASS (не менше ніж до Task 3).

• Step 5: Commit

git add src/gps_denied/core/vo.py tests/test_vo.py
git commit -m "feat(vo): add CuVSLAMMonoDepthVisualOdometry — barometer as synthetic depth

Replaces Inertial mode (requires stereo) with Mono-Depth mode.
Dev/CI fallback: ORB translation scaled by depth_hint_m.
factory: add prefer_mono_depth=True param.
Ref: docs/superpowers/specs/2026-04-18-oss-stack-tech-audit-design.md"

---

Task 5: e2e guard для Mono-Depth на EuRoC

Files:

• Modify: tests/e2e/test_euroc.py

Контекст: EuRoC MH_01 — indoor MAV з forward camera. cuVSLAM Mono-Depth оптимізований для outdoor nadir. ATE > 0.205 м очікується і є прийнятним. Ceiling залишається 0.5 м. Тест документує baseline для майбутнього порівняння.

• Step 1: Додати константу і тест в tests/e2e/test_euroc.py

Після блоку констант (EUROC_MH01_GPS_RMSE_CEILING_M) додати:

# Mono-Depth baseline — EuRoC indoor є worst-case для outdoor-optimised backend.
# ATE може бути гіршим ніж ORB на EuRoC — це очікувано.
# Ceiling 0.5 м (те саме що для ORB) — якщо перевищує, дивись Risk Budget в
# docs/superpowers/specs/2026-04-18-oss-stack-tech-audit-design.md §4.
EUROC_MH01_MONO_DEPTH_HINT_M = 1.5  # EuRoC indoor висота ~1-2м над підлогою

Після test_euroc_mh01_gps_rmse_within_ceiling додати:

@pytest.mark.e2e
@pytest.mark.needs_dataset
@pytest.mark.asyncio
async def test_euroc_mh01_mono_depth_within_ceiling(euroc_mh01_root: Path):
    """Mono-Depth backend ATE на EuRoC — regression guard для VO migration.

    EuRoC indoor ≠ production outdoor nadir. Поганий ATE тут не є блокером
    для production. Тест фіксує baseline і запобігає неочікуваній регресії.
    """
    from gps_denied.core.vo import CuVSLAMMonoDepthVisualOdometry

    adapter = EuRoCAdapter(euroc_mh01_root)
    harness = E2EHarness(
        adapter,
        max_frames=EUROC_MH01_MAX_FRAMES,
        vo_scale_m=EUROC_MH01_MONO_DEPTH_HINT_M / 300.0,  # scale=depth/reference
    )
    result = await harness.run()

    eskf = result.eskf_positions_enu
    gt = result.ground_truth
    if eskf.shape[0] == 0:
        pytest.xfail("ESKF empty — pipeline not initialised with Mono-Depth backend.")

    n = min(eskf.shape[0], gt.shape[0])
    ate = absolute_trajectory_error(eskf[:n], gt[:n])

    # Записати число для документування — навіть якщо гірше ніж ORB
    print(f"\n[Mono-Depth] EuRoC ATE RMSE = {ate['rmse']:.4f} m (ORB baseline ~0.205 m)")

    assert ate["rmse"] < EUROC_MH01_ESKF_RMSE_CEILING_M, (
        f"Mono-Depth ATE RMSE={ate['rmse']:.4f}m > ceiling {EUROC_MH01_ESKF_RMSE_CEILING_M}m. "
        f"Дивись Risk Budget: docs/superpowers/specs/2026-04-18-oss-stack-tech-audit-design.md §4"
    )

• Step 2: Верифікувати що non-e2e тести не зламались

python -m pytest tests/ --ignore=tests/e2e -q 2>&1 | tail -3

Expected: 196 passed, 8 skipped (або більше якщо нові тести є).

• Step 3: Запустити e2e якщо датасет є

# Якщо datasets/euroc/MH_01/ існує:
python -m pytest tests/e2e/test_euroc.py::test_euroc_mh01_mono_depth_within_ceiling -v -s 2>&1 | tail -20

Записати ATE число в коментар до тесту.

• Step 4: Commit

git add tests/e2e/test_euroc.py
git commit -m "test(e2e): add EuRoC Mono-Depth ATE regression guard

Documents baseline for cuVSLAM Mono-Depth on EuRoC MH_01.
ATE ceiling 0.5m — same as ORB. EuRoC indoor != production outdoor nadir."

---

Task 6: AnyLoc GPR — write failing tests

Files:

• Modify: tests/test_gpr.py

Контекст: Новий AnyLocGPR клас реалізує IGlobalPlaceRecognition. На dev/CI використовує numpy L2 fallback (DINOv2 недоступний). Головне — перевірити interface і що клас повертає правильні типи.

• Step 1: Написати failing тести

Додати в кінець tests/test_gpr.py:

# ---------------------------------------------------------------------------
# AnyLocGPR tests
# ---------------------------------------------------------------------------

def test_anyloc_gpr_implements_interface():
    """AnyLocGPR реалізує IGlobalPlaceRecognition."""
    from gps_denied.core.gpr import AnyLocGPR, IGlobalPlaceRecognition
    gpr = AnyLocGPR()
    assert isinstance(gpr, IGlobalPlaceRecognition)


def test_anyloc_compute_descriptor_returns_array():
    """compute_location_descriptor повертає numpy array фіксованого розміру."""
    from gps_denied.core.gpr import AnyLocGPR
    gpr = AnyLocGPR(descriptor_dim=256)
    image = np.zeros((224, 224, 3), dtype=np.uint8)
    descriptor = gpr.compute_location_descriptor(image)
    assert isinstance(descriptor, np.ndarray)
    assert descriptor.shape == (256,)
    assert descriptor.dtype == np.float32


def test_anyloc_retrieve_candidates_returns_list():
    """retrieve_candidate_tiles повертає список TileCandidate."""
    from gps_denied.core.gpr import AnyLocGPR
    from gps_denied.schemas.gpr import TileCandidate
    gpr = AnyLocGPR(descriptor_dim=256)
    image = np.zeros((224, 224, 3), dtype=np.uint8)
    candidates = gpr.retrieve_candidate_tiles(image, top_k=3)
    assert isinstance(candidates, list)
    # Без index — може повертати порожній список, не падати
    assert len(candidates) <= 3
    for c in candidates:
        assert isinstance(c, TileCandidate)


def test_anyloc_descriptor_dim_configurable():
    """descriptor_dim передається через конструктор."""
    from gps_denied.core.gpr import AnyLocGPR
    gpr = AnyLocGPR(descriptor_dim=512)
    image = np.zeros((224, 224, 3), dtype=np.uint8)
    desc = gpr.compute_location_descriptor(image)
    assert desc.shape == (512,)

• Step 2: Запустити — переконатись що падають

python -m pytest tests/test_gpr.py -k "anyloc" -v 2>&1 | tail -10

Expected: ImportError або AttributeError.

---

Task 7: Реалізувати AnyLocGPR

Files:

• Modify: src/gps_denied/core/gpr.py

• Step 1: Додати клас в gpr.py після існуючого GlobalPlaceRecognition

Знайти кінець файлу і додати:

# ---------------------------------------------------------------------------
# AnyLocGPR — DINOv2-VLAD descriptor + Faiss index (sprint 1 GPR baseline)
# ---------------------------------------------------------------------------

class AnyLocGPR(IGlobalPlaceRecognition):
    """Place recognition using AnyLoc-VLAD-DINOv2 descriptors.

    Sprint 1 baseline. On Jetson: DINOv2 ViT-base TRT FP16 (~10-12ms per frame).
    On dev/CI: numpy random projection fallback (same interface, random descriptors).

    Do NOT use INT8 quantization — broken for ViT on Jetson (NVIDIA/TRT#4348).

    Stage 2 evaluation: SuperPoint+LightGlue+FAISS (see _docs/03_backlog/stage2_ideas/).
    Long-term target: EigenPlaces (ICCV 2023) — better ONNX export, viewpoint-robust.

    By Yuzviak — selected over NetVLAD (deprecated, -2.4% R@1) and SP+LG
    (unvalidated for cross-view UAV↔satellite gap at sprint 1).
    Ref: docs/superpowers/specs/2026-04-18-oss-stack-tech-audit-design.md §2.3
    """

    def __init__(
        self,
        descriptor_dim: int = 256,
        index_path: Optional[str] = None,
        model_manager: Optional[IModelManager] = None,
    ):
        self._descriptor_dim = descriptor_dim
        self._index_path = index_path
        self._model_manager = model_manager
        self._index = None
        self._index_metadata: List[Dict] = []
        self._rng = np.random.default_rng(seed=0)  # dev/CI deterministic fallback

        if index_path and os.path.exists(index_path):
            self._load_index(index_path)

        self._has_trt = self._check_trt_engine()

    def _check_trt_engine(self) -> bool:
        """Check if DINOv2 TRT engine is available via model_manager."""
        if self._model_manager is None:
            return False
        try:
            self._model_manager.get_inference_engine("DINOv2")
            return True
        except Exception:
            return False

    def _load_index(self, path: str) -> None:
        """Load Faiss index from disk if available."""
        if not _FAISS_AVAILABLE:
            logger.info("AnyLocGPR: Faiss not available — index search disabled")
            return
        try:
            self._index = _faiss.read_index(path)
            meta_path = path.replace(".index", "_meta.json")
            if os.path.exists(meta_path):
                with open(meta_path) as f:
                    self._index_metadata = json.load(f)
            logger.info("AnyLocGPR: loaded index with %d entries", self._index.ntotal)
        except Exception as exc:
            logger.error("AnyLocGPR: failed to load index: %s", exc)

    def compute_location_descriptor(self, image: np.ndarray) -> np.ndarray:
        """Compute DINOv2-VLAD descriptor. Returns float32 array of shape (descriptor_dim,).

        On Jetson with TRT engine: real DINOv2 ViT-base inference.
        On dev/CI: deterministic random projection (same shape, not meaningful).
        """
        if self._has_trt:
            try:
                engine = self._model_manager.get_inference_engine("DINOv2")
                # Resize to 224×224, normalise to [0,1], run inference
                resized = cv2.resize(image, (224, 224)).astype(np.float32) / 255.0
                result = engine.infer(resized)
                raw = np.array(result["embedding"], dtype=np.float32)
                # VLAD aggregation: project to descriptor_dim
                if raw.shape[0] != self._descriptor_dim:
                    proj = self._rng.standard_normal((raw.shape[0], self._descriptor_dim)).astype(np.float32)
                    raw = raw @ proj
                norm = np.linalg.norm(raw)
                return raw / (norm + 1e-8)
            except Exception as exc:
                logger.warning("AnyLocGPR TRT inference failed, using fallback: %s", exc)

        # Dev/CI fallback: deterministic descriptor from image statistics
        gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) if image.ndim == 3 else image
        resized = cv2.resize(gray, (224, 224)).astype(np.float32) / 255.0
        flat = resized.flatten()
        # Random projection to descriptor_dim — deterministic via fixed seed
        rng = np.random.default_rng(seed=int(flat.sum() * 1000) % (2**31))
        proj = rng.standard_normal((flat.shape[0], self._descriptor_dim)).astype(np.float32)
        desc = flat @ proj
        norm = np.linalg.norm(desc)
        return (desc / (norm + 1e-8)).astype(np.float32)

    def retrieve_candidate_tiles(self, image: np.ndarray, top_k: int = 5) -> List[TileCandidate]:
        """Retrieve top-k satellite tile candidates by descriptor similarity.

        Returns empty list if no index is loaded (dev/CI without tile database).
        """
        if self._index is None:
            return []

        descriptor = self.compute_location_descriptor(image)
        query = descriptor.reshape(1, -1).astype(np.float32)

        try:
            k = min(top_k, self._index.ntotal)
            distances, indices = self._index.search(query, k)
            candidates = []
            for dist, idx in zip(distances[0], indices[0]):
                if idx < 0 or idx >= len(self._index_metadata):
                    continue
                meta = self._index_metadata[idx]
                candidates.append(TileCandidate(
                    tile_id=meta.get("tile_id", str(idx)),
                    score=float(1.0 / (1.0 + dist)),
                    bounds=TileBounds(
                        north=meta.get("north", 0.0),
                        south=meta.get("south", 0.0),
                        east=meta.get("east", 0.0),
                        west=meta.get("west", 0.0),
                    ),
                ))
            return candidates
        except Exception as exc:
            logger.error("AnyLocGPR index search failed: %s", exc)
            return []

Також додати import cv2 в imports якщо відсутній.

• Step 2: Запустити нові тести

python -m pytest tests/test_gpr.py -k "anyloc" -v 2>&1 | tail -15

Expected: всі 4 тести PASS.

• Step 3: Перевірити що старі GPR тести не зламались

python -m pytest tests/test_gpr.py -v 2>&1 | tail -10

Expected: всі тести PASS.

• Step 4: Перевірити весь non-e2e suite

python -m pytest tests/ --ignore=tests/e2e -q 2>&1 | tail -3

Expected: ≥196 passed.

• Step 5: Commit

git add src/gps_denied/core/gpr.py tests/test_gpr.py
git commit -m "feat(gpr): add AnyLocGPR — DINOv2-VLAD baseline for satellite matching

Sprint 1 GPR baseline. TRT FP16 on Jetson, numpy fallback on dev/CI.
No INT8 (broken for ViT on Jetson NVIDIA/TRT#4348).
Stage 2: SP+LG evaluation per stage2_ideas backlog."

---

Task 8: SITL GPS_INPUT field encoding tests

Files:

• Modify: tests/test_sitl_integration.py

Контекст: Верифікувати що кодування полів GPS_INPUT відповідає MAVLink spec (#232) і ArduPilot requirements. Reference: MAVProxy/modules/mavproxy_GPSInput.py.

• Step 1: Перевірити що є в існуючому файлі

grep -n "def test_" tests/test_sitl_integration.py

• Step 2: Додати тести field encoding

Додати в tests/test_sitl_integration.py після існуючих тестів:

# ---------------------------------------------------------------------------
# GPS_INPUT field encoding tests (MAVLink #232 spec compliance)
# Ref: MAVProxy/modules/mavproxy_GPSInput.py
# ---------------------------------------------------------------------------

def test_gps_input_lat_lon_encoding():
    """lat/lon мають кодуватись як int × 1e7 (MAVLink #232 spec)."""
    from gps_denied.core.mavlink import build_gps_input_fields

    fields = build_gps_input_fields(
        lat=48.123456, lon=37.654321, alt_m=600.0,
        vn_ms=10.0, ve_ms=5.0, vd_ms=0.5,
        h_acc_m=15.0, v_acc_m=8.0, fix_type=3,
    )
    assert fields["lat"] == int(48.123456 * 1e7)
    assert fields["lon"] == int(37.654321 * 1e7)


def test_gps_input_alt_encoding():
    """alt має кодуватись в міліметрах (MAVLink #232 spec)."""
    from gps_denied.core.mavlink import build_gps_input_fields

    fields = build_gps_input_fields(
        lat=48.0, lon=37.0, alt_m=600.5,
        vn_ms=0.0, ve_ms=0.0, vd_ms=0.0,
        h_acc_m=10.0, v_acc_m=5.0, fix_type=3,
    )
    assert fields["alt"] == int(600.5 * 1000)  # мм


def test_gps_input_velocity_encoding():
    """vn/ve/vd мають кодуватись в см/с (MAVLink #232 spec)."""
    from gps_denied.core.mavlink import build_gps_input_fields

    fields = build_gps_input_fields(
        lat=48.0, lon=37.0, alt_m=600.0,
        vn_ms=10.5, ve_ms=-5.2, vd_ms=0.3,
        h_acc_m=10.0, v_acc_m=5.0, fix_type=3,
    )
    assert fields["vn"] == int(10.5 * 100)   # см/с
    assert fields["ve"] == int(-5.2 * 100)
    assert fields["vd"] == int(0.3 * 100)


def test_gps_input_satellites_synthetic():
    """satellites_visible має бути 10 (synthetic — запобігає satellite-count failsafe)."""
    from gps_denied.core.mavlink import build_gps_input_fields

    fields = build_gps_input_fields(
        lat=48.0, lon=37.0, alt_m=600.0,
        vn_ms=0.0, ve_ms=0.0, vd_ms=0.0,
        h_acc_m=10.0, v_acc_m=5.0, fix_type=3,
    )
    assert fields["satellites_visible"] == 10


def test_gps_input_fix_type_tiers():
    """fix_type відповідає confidence tier mapping."""
    from gps_denied.core.mavlink import build_gps_input_fields

    # HIGH tier (satellite-anchored) → fix_type=3
    f3 = build_gps_input_fields(lat=48.0, lon=37.0, alt_m=600.0,
                                 vn_ms=0.0, ve_ms=0.0, vd_ms=0.0,
                                 h_acc_m=15.0, v_acc_m=8.0, fix_type=3)
    assert f3["fix_type"] == 3

    # LOW tier (IMU-only) → fix_type=2
    f2 = build_gps_input_fields(lat=48.0, lon=37.0, alt_m=600.0,
                                 vn_ms=0.0, ve_ms=0.0, vd_ms=0.0,
                                 h_acc_m=200.0, v_acc_m=50.0, fix_type=2)
    assert f2["fix_type"] == 2

    # FAILED → fix_type=0
    f0 = build_gps_input_fields(lat=48.0, lon=37.0, alt_m=600.0,
                                 vn_ms=0.0, ve_ms=0.0, vd_ms=0.0,
                                 h_acc_m=999.0, v_acc_m=999.0, fix_type=0)
    assert f0["fix_type"] == 0

• Step 3: Перевірити що build_gps_input_fields існує в mavlink.py

grep -n "build_gps_input_fields" src/gps_denied/core/mavlink.py

Якщо не існує — додати в src/gps_denied/core/mavlink.py:

def build_gps_input_fields(
    lat: float, lon: float, alt_m: float,
    vn_ms: float, ve_ms: float, vd_ms: float,
    h_acc_m: float, v_acc_m: float, fix_type: int,
    gps_id: int = 0,
) -> dict:
    """Build GPS_INPUT message fields per MAVLink #232 spec.

    Encoding: lat/lon ×1e7 (int), alt mm (int), vel cm/s (int).
    Ref: MAVProxy/modules/mavproxy_GPSInput.py
    """
    return {
        "lat": int(lat * 1e7),
        "lon": int(lon * 1e7),
        "alt": int(alt_m * 1000),          # mm
        "vn": int(vn_ms * 100),            # cm/s
        "ve": int(ve_ms * 100),            # cm/s
        "vd": int(vd_ms * 100),            # cm/s
        "horiz_accuracy": h_acc_m,
        "vert_accuracy": v_acc_m,
        "fix_type": fix_type,
        "satellites_visible": 10,           # synthetic — prevent satellite-count failsafe
        "gps_id": gps_id,
        "ignore_flags": 0,                  # provide all fields
    }

• Step 4: Запустити нові тести

python -m pytest tests/test_sitl_integration.py -k "gps_input" -v 2>&1 | tail -15

Expected: всі 5 нових тестів PASS.

• Step 5: Перевірити весь non-e2e suite

python -m pytest tests/ --ignore=tests/e2e -q 2>&1 | tail -3

Expected: ≥196 passed (більше — бо додали нові тести).

• Step 6: Commit

git add tests/test_sitl_integration.py src/gps_denied/core/mavlink.py
git commit -m "test(sitl): add GPS_INPUT field encoding compliance tests

Verifies MAVLink #232 spec: lat/lon ×1e7, alt mm, vel cm/s.
satellites_visible=10 (synthetic, prevents ArduPilot failsafe).
Ref: MAVProxy/modules/mavproxy_GPSInput.py"

---

Self-Review

Spec coverage

Секція з design doc	Task
numpy pin	Task 1
cuVSLAM Mono-Depth backend	Task 3–4
e2e guard через харнес	Task 5
Risk budget документований	Task 5 (assert message)
AnyLoc GPR baseline	Task 6–7
SITL GPS_INPUT field encoding	Task 8
aero-vloc benchmark	⚠️ Не включений — це research task, не implementation
FC processor check	⚠️ Не включений — manual hardware check, не code

aero-vloc і FC processor check — не кодові задачі, не входять в план. Їх виконання ручне і незалежне від цього плану.

Placeholder scan

• Всі code blocks повні.
• Всі команди конкретні з expected output.
• Немає TBD або "implement later".

Type consistency

• CuVSLAMMonoDepthVisualOdometry — визначено в Task 4, використовується в Task 5 і тестах Task 3 ✅
• build_gps_input_fields — визначено в Task 8 step 3, тести в step 2 ✅
• AnyLocGPR — визначено в Task 7, тести в Task 6 ✅
• ISequentialVisualOdometry, RelativePose, CameraParameters — існуючі типи, не змінюємо ✅