mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-04-22 09:06:37 +00:00
refactor(vo): address code review for CuVSLAMMonoDepthVisualOdometry
- test_depth_hint_scales_translation: replace assert True with mock-based verification of scale factor - _init_tracker / _compute_via_cuvslam: logger.exception for stack traces - _init_tracker: loud warning when Jetson path disabled - drop personal attribution (git blame suffices) - translate Ukrainian test docstrings to English
This commit is contained in:
@@ -415,7 +415,6 @@ class CuVSLAMMonoDepthVisualOdometry(ISequentialVisualOdometry):
|
||||
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__(
|
||||
@@ -462,8 +461,11 @@ class CuVSLAMMonoDepthVisualOdometry(ISequentialVisualOdometry):
|
||||
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)
|
||||
except Exception:
|
||||
logger.exception(
|
||||
"cuVSLAM Mono-Depth tracker init FAILED — falling back to ORB. "
|
||||
"Production Jetson path is DISABLED until this is fixed."
|
||||
)
|
||||
self._cuvslam = None
|
||||
|
||||
@property
|
||||
@@ -503,8 +505,8 @@ class CuVSLAMMonoDepthVisualOdometry(ISequentialVisualOdometry):
|
||||
tracking_good=True,
|
||||
scale_ambiguous=False,
|
||||
)
|
||||
except Exception as exc:
|
||||
logger.error("cuVSLAM Mono-Depth tracking failed: %s", exc)
|
||||
except Exception:
|
||||
logger.exception("cuVSLAM Mono-Depth tracking step failed — frame dropped")
|
||||
return None
|
||||
|
||||
def _compute_via_orb_scaled(
|
||||
|
||||
+37
-18
@@ -230,14 +230,14 @@ def test_create_vo_backend_orb_fallback():
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def test_mono_depth_implements_interface():
|
||||
"""CuVSLAMMonoDepthVisualOdometry реалізує ISequentialVisualOdometry."""
|
||||
"""CuVSLAMMonoDepthVisualOdometry implements 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."""
|
||||
"""Mono-Depth backend always returns 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)
|
||||
@@ -247,38 +247,57 @@ def test_mono_depth_scale_not_ambiguous():
|
||||
resolution_width=640, resolution_height=480,
|
||||
)
|
||||
pose = vo.compute_relative_pose(prev, curr, cam)
|
||||
# може бути None якщо зображення порожні — але якщо є, scale_ambiguous=False
|
||||
# may be None when images are blank — but if present, 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))
|
||||
"""depth_hint_m масштабує translation у dev/CI ORB fallback by depth_hint_m / 600.0."""
|
||||
from unittest.mock import patch
|
||||
from gps_denied.core.vo import CuVSLAMMonoDepthVisualOdometry, ORBVisualOdometry
|
||||
from gps_denied.schemas.vo import RelativePose
|
||||
|
||||
cam = CameraParameters(
|
||||
focal_length=16.0, sensor_width=23.2, sensor_height=17.4,
|
||||
resolution_width=640, resolution_height=480,
|
||||
)
|
||||
img = np.zeros((480, 640), dtype=np.uint8)
|
||||
|
||||
vo_low = CuVSLAMMonoDepthVisualOdometry(depth_hint_m=300.0)
|
||||
vo_high = CuVSLAMMonoDepthVisualOdometry(depth_hint_m=600.0)
|
||||
# Mock ORB to always return a unit translation — lets us verify the scale factor cleanly
|
||||
unit_pose = RelativePose(
|
||||
translation=np.array([1.0, 0.0, 0.0]),
|
||||
rotation=np.eye(3),
|
||||
confidence=1.0,
|
||||
inlier_count=100,
|
||||
total_matches=100,
|
||||
tracking_good=True,
|
||||
scale_ambiguous=True,
|
||||
)
|
||||
|
||||
pose_low = vo_low.compute_relative_pose(img, img, cam)
|
||||
pose_high = vo_high.compute_relative_pose(img, img, cam)
|
||||
with patch.object(ORBVisualOdometry, "compute_relative_pose", return_value=unit_pose):
|
||||
vo_300 = CuVSLAMMonoDepthVisualOdometry(depth_hint_m=300.0)
|
||||
vo_600 = CuVSLAMMonoDepthVisualOdometry(depth_hint_m=600.0)
|
||||
vo_1200 = CuVSLAMMonoDepthVisualOdometry(depth_hint_m=1200.0)
|
||||
|
||||
# Обидва можуть повернути None для ідентичних зображень (нульовий motion)
|
||||
# Тест верифікує що клас існує і приймає depth_hint_m без помилок
|
||||
assert True # якщо дійшли сюди — конструктор і interface працюють
|
||||
pose_300 = vo_300.compute_relative_pose(img, img, cam)
|
||||
pose_600 = vo_600.compute_relative_pose(img, img, cam)
|
||||
pose_1200 = vo_1200.compute_relative_pose(img, img, cam)
|
||||
|
||||
# At 600m reference altitude, scale = 1.0 → unchanged
|
||||
assert np.allclose(pose_600.translation, [1.0, 0.0, 0.0])
|
||||
# At 300m, scale = 0.5 → halved
|
||||
assert np.allclose(pose_300.translation, [0.5, 0.0, 0.0])
|
||||
# At 1200m, scale = 2.0 → doubled
|
||||
assert np.allclose(pose_1200.translation, [2.0, 0.0, 0.0])
|
||||
# All must report metric scale
|
||||
assert pose_300.scale_ambiguous is False
|
||||
assert pose_600.scale_ambiguous is False
|
||||
assert pose_1200.scale_ambiguous is False
|
||||
|
||||
|
||||
def test_mono_depth_create_vo_backend_selects_it():
|
||||
"""create_vo_backend з prefer_mono_depth=True повертає CuVSLAMMonoDepthVisualOdometry."""
|
||||
"""create_vo_backend with prefer_mono_depth=True returns 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)
|
||||
|
||||
Reference in New Issue
Block a user