Initial commit

test_f07_sequential_visual_odometry.py

@@ -0,0 +1,288 @@
+import pytest
+import numpy as np
+from unittest.mock import Mock, patch
+
+from f07_sequential_visual_odometry import SequentialVisualOdometry, Features, Matches, Motion
+from f02_1_flight_lifecycle_manager import CameraParameters
+
+@pytest.fixture
+def svo():
+    return SequentialVisualOdometry()
+
+@pytest.fixture
+def mock_model_manager():
+    manager = Mock()
+    # Mock SuperPoint output: 500 keypoints
+    kpts = np.random.rand(500, 2) * [1920, 1080]
+    desc = np.random.rand(500, 256).astype(np.float32)
+    scores = np.random.rand(500).astype(np.float32)
+    manager.run_superpoint.return_value = (kpts, desc, scores)
+    
+    # Mock LightGlue output
+    num_matches = 300
+    matches0 = np.random.choice(500, num_matches, replace=False)
+    matches1 = np.random.choice(500, num_matches, replace=False)
+    match_indices = np.stack([matches0, matches1], axis=-1)
+    match_scores = np.random.rand(num_matches).astype(np.float32)
+    manager.run_lightglue.return_value = (match_indices, match_scores)
+    
+    return manager
+
+@pytest.fixture
+def camera_params():
+    return CameraParameters(focal_length_mm=25.0, sensor_width_mm=36.0, resolution={"width": 1920, "height": 1080})
+
+class TestSequentialVisualOdometry:
+
+    # --- Feature 07.01: Feature Extraction ---
+
+    def test_preprocess_image_rgb(self, svo):
+        rgb_img = np.random.randint(0, 255, (1080, 1920, 3), dtype=np.uint8)
+        preprocessed = svo._preprocess_image(rgb_img)
+        
+        assert len(preprocessed.shape) == 2  # Grayscale
+        assert preprocessed.shape == (1080, 1920)
+        assert preprocessed.dtype == np.float32
+        assert np.max(preprocessed) <= 1.0  # Normalized
+
+    def test_preprocess_image_grayscale_passthrough(self, svo):
+        gray_img = np.random.randint(0, 255, (1080, 1920), dtype=np.uint8)
+        preprocessed = svo._preprocess_image(gray_img)
+        
+        assert len(preprocessed.shape) == 2
+        assert preprocessed.shape == (1080, 1920)
+        assert preprocessed.dtype == np.float32
+
+    def test_extract_features_empty_image(self, svo):
+        empty_img = np.array([])
+        features = svo.extract_features(empty_img)
+        
+        assert features.keypoints.shape == (0, 2)
+        assert features.descriptors.shape == (0, 256)
+        assert len(features.scores) == 0
+
+    def test_extract_features_descriptor_dimensions(self, svo):
+        img = np.zeros((480, 640, 3), dtype=np.uint8)
+        features = svo.extract_features(img)
+        
+        # Using fallback mock, should return up to 2000 points
+        n_points = len(features.keypoints)
+        assert features.descriptors.shape == (n_points, 256)
+        assert features.scores.shape == (n_points,)
+
+    def test_extract_features_feature_count_range(self, svo):
+        img = np.random.randint(0, 255, (1080, 1920, 3), dtype=np.uint8)
+        features = svo.extract_features(img)
+        
+        # Should cap at 2000 points via NMS fallback
+        assert len(features.keypoints) <= 2000
+        assert len(features.keypoints) > 0
+
+    def test_model_manager_integration(self, mock_model_manager):
+        svo_with_model = SequentialVisualOdometry(model_manager=mock_model_manager)
+        img = np.random.randint(0, 255, (1080, 1920, 3), dtype=np.uint8)
+        
+        features = svo_with_model.extract_features(img)
+        
+        # Should use the mocked returns
+        mock_model_manager.run_superpoint.assert_called_once()
+        assert len(features.keypoints) == 500
+        assert features.descriptors.shape == (500, 256)
+        assert len(features.scores) == 500
+
+    # --- Feature 07.02: Feature Matching ---
+
+    def test_match_features_high_overlap(self, svo):
+        # Simulate high overlap by matching features with themselves
+        img = np.random.randint(0, 255, (480, 640, 3), dtype=np.uint8)
+        features = svo.extract_features(img)
+        
+        matches = svo.match_features(features, features)
+        
+        assert len(matches.matches) > 0
+        assert len(matches.matches) == len(matches.scores)
+        assert matches.keypoints1.shape == matches.keypoints2.shape
+
+    def test_match_features_no_overlap(self, svo):
+        # Simulate no overlap with two different random images
+        img1 = np.random.randint(0, 255, (480, 640, 3), dtype=np.uint8)
+        img2 = np.random.randint(1, 254, (480, 640, 3), dtype=np.uint8)
+        features1 = svo.extract_features(img1)
+        features2 = svo.extract_features(img2)
+        
+        matches = svo.match_features(features1, features2)
+        
+        # Mock should still produce some matches, but a real system would have very few
+        assert matches is not None
+
+    def test_match_features_empty_input(self, svo):
+        empty_features = Features(keypoints=np.empty((0, 2)), descriptors=np.empty((0, 256)), scores=np.empty((0,)))
+        img = np.random.randint(0, 255, (480, 640, 3), dtype=np.uint8)
+        features = svo.extract_features(img)
+        
+        matches = svo.match_features(empty_features, features)
+        assert len(matches.matches) == 0
+        
+        matches2 = svo.match_features(features, empty_features)
+        assert len(matches2.matches) == 0
+
+    def test_match_features_confidence_filtering(self, svo, mock_model_manager):
+        svo_with_model = SequentialVisualOdometry(model_manager=mock_model_manager)
+        img = np.random.randint(0, 255, (480, 640, 3), dtype=np.uint8)
+        features = svo_with_model.extract_features(img)
+        
+        # Mock LightGlue to return scores both above and below a threshold
+        scores = np.array([0.9, 0.95, 0.2, 0.8, 0.1])
+        matches = np.random.randint(0, 500, (5, 2))
+        mock_model_manager.run_lightglue.return_value = (matches, scores)
+        
+        # Set a high confidence threshold in the SVO logic to test filtering
+        with patch.object(svo_with_model, '_filter_matches_by_confidence', wraps=lambda m, s, t: (m[s>0.7], s[s>0.7])):
+             result_matches = svo_with_model.match_features(features, features)
+             assert len(result_matches.matches) == 3 # Only 3 scores are > 0.7
+
+    # --- Feature 07.03: Relative Pose Computation ---
+
+    def test_normalize_keypoints(self, svo, camera_params):
+        kpts = np.array([[960.0, 540.0], [0.0, 0.0]])
+        norm = svo._normalize_keypoints(kpts, camera_params)
+        # Center should be roughly 0, 0
+        assert np.allclose(norm[0], [0.0, 0.0])
+        # Top left should be negative
+        assert norm[1, 0] < 0
+        assert norm[1, 1] < 0
+
+    def test_estimate_essential_matrix_insufficient(self, svo, camera_params):
+        K = svo._get_camera_matrix(camera_params)
+        pts1 = np.random.rand(5, 2)
+        pts2 = np.random.rand(5, 2)
+        E, mask = svo._estimate_essential_matrix(pts1, pts2, K)
+        assert E is None
+
+    def test_estimate_essential_matrix_good(self, svo, camera_params):
+        K = svo._get_camera_matrix(camera_params)
+        pts1 = np.random.rand(100, 2) * 1000
+        pts2 = pts1 + np.random.normal(0, 1, (100, 2))
+        E, mask = svo._estimate_essential_matrix(pts1, pts2, K)
+        assert E is not None
+        assert E.shape == (3, 3)
+
+    def test_decompose_essential_matrix(self, svo, camera_params):
+        K = svo._get_camera_matrix(camera_params)
+        pts1 = np.random.rand(100, 2) * 1000
+        pts2 = pts1 + np.array([10.0, 0.0])
+        E, mask = svo._estimate_essential_matrix(pts1, pts2, K)
+        
+        R, t = svo._decompose_essential_matrix(E, pts1, pts2, K)
+        assert R is not None
+        assert t is not None
+        assert np.isclose(np.linalg.det(R), 1.0, atol=1e-3)
+        assert np.isclose(np.linalg.norm(t), 1.0, atol=1e-3)
+
+    def test_compute_tracking_quality(self, svo):
+        # Good
+        conf, good = svo._compute_tracking_quality(100, 150)
+        assert good is True
+        assert conf > 0.5
+        
+        # Degraded
+        conf, good = svo._compute_tracking_quality(30, 50)
+        assert good is True
+        assert conf < 0.5
+        
+        # Lost
+        conf, good = svo._compute_tracking_quality(10, 20)
+        assert good is False
+        assert conf == 0.0
+
+    @patch.object(SequentialVisualOdometry, 'extract_features')
+    @patch.object(SequentialVisualOdometry, 'match_features')
+    def test_compute_relative_pose_pipeline(self, mock_match, mock_extract, svo, camera_params):
+        img1 = np.zeros((100, 100))
+        img2 = np.zeros((100, 100))
+        
+        mock_extract.return_value = Features(keypoints=np.empty((0,2)), descriptors=np.empty((0,256)), scores=np.empty(0))
+        mock_match.return_value = Matches(
+            matches=np.zeros((100, 2)), scores=np.ones(100),
+            keypoints1=np.random.rand(100, 2) * 1000, keypoints2=np.random.rand(100, 2) * 1000
+        )
+        
+        pose = svo.compute_relative_pose(img1, img2, camera_params)
+        
+        assert mock_extract.call_count == 2
+        assert mock_match.call_count == 1
+        assert pose is not None
+        assert pose.tracking_good is not None
+
+    # --- Feature 07.03: Relative Pose Computation ---
+
+    def test_normalize_keypoints(self, svo, camera_params):
+        kpts = np.array([[960.0, 540.0], [0.0, 0.0]])
+        norm = svo._normalize_keypoints(kpts, camera_params)
+        # Center should be roughly 0, 0
+        assert np.allclose(norm[0], [0.0, 0.0])
+        # Top left should be negative
+        assert norm[1, 0] < 0
+        assert norm[1, 1] < 0
+
+    def test_estimate_essential_matrix_insufficient(self, svo, camera_params):
+        K = svo._get_camera_matrix(camera_params)
+        pts1 = np.random.rand(5, 2)
+        pts2 = np.random.rand(5, 2)
+        E, mask = svo._estimate_essential_matrix(pts1, pts2, K)
+        assert E is None
+
+    def test_estimate_essential_matrix_good(self, svo, camera_params):
+        K = svo._get_camera_matrix(camera_params)
+        pts1 = np.random.rand(100, 2) * 1000
+        pts2 = pts1 + np.random.normal(0, 1, (100, 2))
+        E, mask = svo._estimate_essential_matrix(pts1, pts2, K)
+        assert E is not None
+        assert E.shape == (3, 3)
+
+    def test_decompose_essential_matrix(self, svo, camera_params):
+        K = svo._get_camera_matrix(camera_params)
+        pts1 = np.random.rand(100, 2) * 1000
+        pts2 = pts1 + np.array([10.0, 0.0])
+        E, mask = svo._estimate_essential_matrix(pts1, pts2, K)
+        
+        R, t = svo._decompose_essential_matrix(E, pts1, pts2, K)
+        assert R is not None
+        assert t is not None
+        assert np.isclose(np.linalg.det(R), 1.0, atol=1e-3)
+        assert np.isclose(np.linalg.norm(t), 1.0, atol=1e-3)
+
+    def test_compute_tracking_quality(self, svo):
+        # Good
+        conf, good = svo._compute_tracking_quality(100, 150)
+        assert good is True
+        assert conf > 0.5
+        
+        # Degraded
+        conf, good = svo._compute_tracking_quality(30, 50)
+        assert good is True
+        assert conf < 0.5
+        
+        # Lost
+        conf, good = svo._compute_tracking_quality(10, 20)
+        assert good is False
+        assert conf == 0.0
+
+    @patch.object(SequentialVisualOdometry, 'extract_features')
+    @patch.object(SequentialVisualOdometry, 'match_features')
+    def test_compute_relative_pose_pipeline(self, mock_match, mock_extract, svo, camera_params):
+        img1 = np.zeros((100, 100))
+        img2 = np.zeros((100, 100))
+        
+        mock_extract.return_value = Features(keypoints=np.empty((0,2)), descriptors=np.empty((0,256)), scores=np.empty(0))
+        mock_match.return_value = Matches(
+            matches=np.zeros((100, 2)), scores=np.ones(100),
+            keypoints1=np.random.rand(100, 2) * 1000, keypoints2=np.random.rand(100, 2) * 1000
+        )
+        
+        pose = svo.compute_relative_pose(img1, img2, camera_params)
+        
+        assert mock_extract.call_count == 2
+        assert mock_match.call_count == 1
+        assert pose is not None
+        assert pose.tracking_good is not None