Initial commit

test_f14_coordinate_transformer.py

@@ -0,0 +1,116 @@
+import pytest
+import numpy as np
+from unittest.mock import Mock
+import math
+
+from f14_coordinate_transformer import CoordinateTransformer, GPSPoint, Sim3Transform, OriginNotSetError
+
+@pytest.fixture
+def mock_camera_model():
+    model = Mock()
+    # By default, mock the camera's un-projection mapping a pixel directly forward (+Z)
+    model.pixel_to_ray.return_value = np.array([0.0, 0.0, 1.0])
+    return model
+
+@pytest.fixture
+def transformer(mock_camera_model):
+    return CoordinateTransformer(camera_model=mock_camera_model)
+
+class TestCoordinateTransformer:
+    
+    def test_enu_to_gps_without_origin_raises_error(self, transformer):
+        """Verifies that an exception is raised if global conversion is attempted without an ENU origin."""
+        with pytest.raises(OriginNotSetError, match="Origin not set for flight"):
+            transformer.enu_to_gps("invalid_flight", (10.0, 10.0, 5.0))
+            
+    def test_gps_to_enu_at_origin_returns_zero(self, transformer):
+        origin = GPSPoint(lat=48.0, lon=37.0, altitude_m=100.0)
+        flight_id = "test_flight"
+        transformer.set_enu_origin(flight_id, origin)
+        
+        enu = transformer.gps_to_enu(flight_id, origin)
+        assert np.isclose(enu[0], 0.0)
+        assert np.isclose(enu[1], 0.0)
+        assert np.isclose(enu[2], 0.0)
+
+    def test_enu_to_gps_conversion(self, transformer):
+        """Tests local Metric (ENU) to Global Geodetic (GPS) coordinate transformations."""
+        origin = GPSPoint(lat=48.0, lon=37.0, altitude_m=100.0)
+        flight_id = "test_flight"
+        transformer.set_enu_origin(flight_id, origin)
+
+        # Move 1000m East, 2000m North, 50m Up
+        x, y, z = 1000.0, 2000.0, 50.0
+        
+        result = transformer.enu_to_gps(flight_id, (x, y, z))
+        
+        assert result.altitude_m == 150.0
+        assert result.lat > 48.0
+        assert result.lon > 37.0
+        
+        # Validate spherical approximation logic
+        earth_radius = 6378137.0
+        expected_lat = 48.0 + math.degrees(y / earth_radius)
+        expected_lon = 37.0 + math.degrees(x / (earth_radius * math.cos(math.radians(48.0))))
+        
+        np.testing.assert_allclose(result.lat, expected_lat)
+        np.testing.assert_allclose(result.lon, expected_lon)
+
+    def test_ray_cloud_intersection_hit(self, transformer):
+        """Tests that the vector math correctly identifies the closest 3D point in front of the ray."""
+        ray_origin = np.array([0.0, 0.0, 0.0])
+        ray_dir = np.array([0.0, 0.0, 1.0]) # Pointing straight forward (+Z)
+        
+        point_cloud = np.array([
+            [0.0, 0.0, -10.0],  # Rejected: Behind the camera
+            [5.0, 5.0, 10.0],   # Rejected: Too far laterally (dist = sqrt(50) > 2.0)
+            [0.5, 0.5, 10.0]    # Selected: Hits within the 2.0m max_dist radius!
+        ])
+        
+        result = transformer._ray_cloud_intersection(ray_origin, ray_dir, point_cloud, max_dist=2.0)
+        
+        assert result is not None
+        np.testing.assert_array_equal(result, np.array([0.5, 0.5, 10.0]))
+
+    def test_ray_cloud_intersection_miss(self, transformer):
+        """Tests that intersection gracefully returns None when no map features align with the ray."""
+        ray_origin = np.array([0.0, 0.0, 0.0])
+        ray_dir = np.array([0.0, 0.0, 1.0])
+        
+        # All points too far from the central ray
+        point_cloud = np.array([
+            [10.0, 0.0, 5.0],
+            [0.0, 10.0, 5.0]
+        ])
+        
+        result = transformer._ray_cloud_intersection(ray_origin, ray_dir, point_cloud, max_dist=2.0)
+        assert result is None
+        
+    def test_pixel_to_gps_success(self, transformer, mock_camera_model):
+        """End-to-End test of the Object-Click to absolute GPS calculation passing through Sim(3) graphs."""
+        flight_id = "test_flight"
+        transformer.set_enu_origin(flight_id, GPSPoint(lat=48.0, lon=37.0, altitude_m=0.0))
+        
+        u, v = 320.0, 240.0
+        local_pose = np.eye(4) # Local V-SLAM camera at origin, looking down +Z
+        local_point_cloud = np.array([[0.0, 0.0, 20.0]]) # 3D Feature point directly on the target
+        
+        # Sim(3) transformation from global optimization (AC-2 correction)
+        sim3 = Sim3Transform(
+            translation=np.array([10.0, 20.0, 30.0]), 
+            rotation=np.eye(3),
+            scale=2.0
+        )
+        
+        response = transformer.pixel_to_gps(flight_id, u, v, local_pose, local_point_cloud, sim3)
+        
+        assert response is not None
+        # Accuracy is geometrically coupled with the Sim3 scale factor (5.0m base * 2.0 scale)
+        assert response.accuracy_meters == 10.0 
+        
+        # p_local = [0, 0, 20]
+        # p_metric = scale * (R * p_local) + T
+        # p_metric = 2.0 * [0, 0, 20] + [10, 20, 30] 
+        # p_metric = [10, 20, 70]
+        
+        assert response.gps.altitude_m == 70.0