feat: stage7 — Model Manager (F16) and Sequential VO (F07)

tests/test_vo.py

@@ -0,0 +1,102 @@
+"""Tests for Sequential Visual Odometry (F07)."""
+
+import numpy as np
+import pytest
+
+from gps_denied.core.models import ModelManager
+from gps_denied.core.vo import SequentialVisualOdometry
+from gps_denied.schemas.flight import CameraParameters
+from gps_denied.schemas.vo import Features, Matches
+
+
+@pytest.fixture
+def vo():
+    manager = ModelManager()
+    return SequentialVisualOdometry(manager)
+
+
+@pytest.fixture
+def cam_params():
+    return CameraParameters(
+        focal_length=5.0,
+        sensor_width=6.4,
+        sensor_height=4.8,
+        resolution_width=640,
+        resolution_height=480,
+        principal_point=(320.0, 240.0)
+    )
+
+
+def test_extract_features(vo):
+    img = np.zeros((480, 640, 3), dtype=np.uint8)
+    features = vo.extract_features(img)
+    
+    assert isinstance(features, Features)
+    assert features.keypoints.shape == (500, 2)
+    assert features.descriptors.shape == (500, 256)
+
+
+def test_match_features(vo):
+    f1 = Features(
+        keypoints=np.random.rand(100, 2),
+        descriptors=np.random.rand(100, 256),
+        scores=np.random.rand(100)
+    )
+    f2 = Features(
+        keypoints=np.random.rand(100, 2),
+        descriptors=np.random.rand(100, 256),
+        scores=np.random.rand(100)
+    )
+    
+    matches = vo.match_features(f1, f2)
+    assert isinstance(matches, Matches)
+    assert matches.matches.shape == (100, 2)
+
+
+def test_estimate_motion_insufficient_matches(vo, cam_params):
+    matches = Matches(
+        matches=np.zeros((5, 2)),
+        scores=np.zeros(5),
+        keypoints1=np.zeros((5, 2)),
+        keypoints2=np.zeros((5, 2))
+    )
+    
+    # Less than 8 points should return None
+    motion = vo.estimate_motion(matches, cam_params)
+    assert motion is None
+
+
+def test_estimate_motion_synthetic(vo, cam_params):
+    # To reliably test compute_relative_pose, we create points strictly satisfying epipolar constraint
+    # Simple straight motion: Add a small shift on X axis
+    n_pts = 100
+    pts1 = np.random.rand(n_pts, 2) * 400 + 100
+    pts2 = pts1 + np.array([10.0, 0.0]) # moving 10 pixels right
+    
+    matches = Matches(
+        matches=np.column_stack([np.arange(n_pts), np.arange(n_pts)]),
+        scores=np.ones(n_pts),
+        keypoints1=pts1,
+        keypoints2=pts2
+    )
+    
+    motion = vo.estimate_motion(matches, cam_params)
+    assert motion is not None
+    assert motion.inlier_count > 20
+    assert motion.translation.shape == (3,)
+    assert motion.rotation.shape == (3, 3)
+
+
+def test_compute_relative_pose(vo, cam_params):
+    img1 = np.zeros((480, 640, 3), dtype=np.uint8)
+    img2 = np.zeros((480, 640, 3), dtype=np.uint8)
+    
+    # Given the random nature of our mock, OpenCV's findEssentialMat will likely find 0 inliers
+    # or fail. We expect compute_relative_pose to gracefully return None or low confidence.
+    pose = vo.compute_relative_pose(img1, img2, cam_params)
+    
+    if pose is not None:
+        assert pose.translation.shape == (3,)
+        assert pose.rotation.shape == (3, 3)
+        # Because we randomize points in the mock manager, inliers will be extremely low
+        assert pose.tracking_good is False