Initial commit

test_f13_coordinate_transformer.py

@@ -0,0 +1,132 @@
+import pytest
+import numpy as np
+from datetime import datetime
+from unittest.mock import Mock
+
+from f02_1_flight_lifecycle_manager import GPSPoint, CameraParameters
+from f10_factor_graph_optimizer import Pose
+from f13_coordinate_transformer import CoordinateTransformer, OriginNotSetError, FlightConfig
+
+@pytest.fixture
+def transformer():
+    f10 = Mock()
+    f17 = Mock()
+    return CoordinateTransformer(f10_optimizer=f10, f17_config=f17)
+
+@pytest.fixture
+def dummy_camera():
+    return CameraParameters(
+        focal_length_mm=25.0, 
+        sensor_width_mm=36.0, 
+        resolution={"width": 1920, "height": 1080}
+    )
+
+@pytest.fixture
+def default_pose():
+    # Camera pointing straight down
+    R_down = np.array([
+        [1,  0,  0],
+        [0, -1,  0],
+        [0,  0, -1]
+    ])
+    return Pose(
+        frame_id=1, 
+        position=np.array([0.0, 0.0, 400.0]), 
+        orientation=R_down, 
+        timestamp=datetime.utcnow()
+    )
+
+class TestCoordinateTransformer:
+
+    # --- 13.01 ENU Coordinate Management ---
+
+    def test_enu_origin_lifecycle(self, transformer):
+        with pytest.raises(OriginNotSetError):
+            transformer.get_enu_origin("flight_1")
+            
+        origin = GPSPoint(lat=48.0, lon=37.0)
+        transformer.set_enu_origin("flight_1", origin)
+        
+        assert transformer.get_enu_origin("flight_1") == origin
+        
+        # Test isolation
+        with pytest.raises(OriginNotSetError):
+            transformer.get_enu_origin("flight_2")
+
+    def test_gps_to_enu(self, transformer):
+        origin = GPSPoint(lat=48.0, lon=37.0)
+        transformer.set_enu_origin("flight_1", origin)
+        
+        # At origin
+        east, north, up = transformer.gps_to_enu("flight_1", origin)
+        assert east == 0.0 and north == 0.0 and up == 0.0
+        
+        # 1 degree north (~111.3 km)
+        pt_north = GPSPoint(lat=49.0, lon=37.0)
+        _, n, _ = transformer.gps_to_enu("flight_1", pt_north)
+        assert np.isclose(n, 111319.5)
+
+    def test_enu_to_gps_roundtrip(self, transformer):
+        origin = GPSPoint(lat=48.0, lon=37.0)
+        transformer.set_enu_origin("flight_1", origin)
+        
+        test_gps = GPSPoint(lat=48.01, lon=37.02)
+        enu = transformer.gps_to_enu("flight_1", test_gps)
+        recovered = transformer.enu_to_gps("flight_1", enu)
+        
+        assert np.isclose(test_gps.lat, recovered.lat)
+        assert np.isclose(test_gps.lon, recovered.lon)
+
+    # --- 13.02 Pixel-GPS Projection ---
+
+    def test_pixel_to_gps_center(self, transformer, dummy_camera, default_pose):
+        origin = GPSPoint(lat=48.0, lon=37.0)
+        transformer.set_enu_origin("flight_1", origin)
+        
+        center_pixel = (1920 / 2, 1080 / 2)
+        gps = transformer.pixel_to_gps("flight_1", center_pixel, default_pose, dummy_camera, 400.0)
+        
+        # Camera looking straight down from ENU [0,0,400] should project exactly to origin [0,0,0]
+        assert np.isclose(gps.lat, origin.lat)
+        assert np.isclose(gps.lon, origin.lon)
+
+    def test_gps_to_pixel_roundtrip(self, transformer, dummy_camera, default_pose):
+        transformer.set_enu_origin("flight_1", GPSPoint(lat=48.0, lon=37.0))
+        
+        pixel = (1000.0, 600.0)
+        gps = transformer.pixel_to_gps("flight_1", pixel, default_pose, dummy_camera, 400.0)
+        recovered_pixel = transformer.gps_to_pixel("flight_1", gps, default_pose, dummy_camera, 400.0)
+        
+        assert np.isclose(pixel[0], recovered_pixel[0])
+        assert np.isclose(pixel[1], recovered_pixel[1])
+
+    def test_image_object_to_gps(self, transformer, dummy_camera, default_pose):
+        transformer.set_enu_origin("flight_1", GPSPoint(lat=48.0, lon=37.0))
+        
+        # Mock the external dependencies
+        transformer.f10.get_trajectory.return_value = {1: default_pose}
+        transformer.f17.get_flight_config.return_value = FlightConfig(camera_params=dummy_camera, altitude=400.0)
+        
+        gps = transformer.image_object_to_gps("flight_1", 1, (1920/2, 1080/2))
+        assert np.isclose(gps.lat, 48.0)
+        assert np.isclose(gps.lon, 37.0)
+
+    def test_transform_points(self, transformer):
+        pts = [(10.0, 10.0), (20.0, 20.0)]
+        
+        # Translation Affine Matrix
+        T = np.array([[1.0, 0.0, 5.0], [0.0, 1.0, 5.0]])
+        res = transformer.transform_points(pts, T)
+        
+        assert res[0] == (15.0, 15.0)
+        assert res[1] == (25.0, 25.0)
+
+    def test_calculate_meters_per_pixel(self, transformer):
+        m_px = transformer.calculate_meters_per_pixel(48.0, 19)
+        assert 0.15 < m_px < 0.25 # Expected ~0.20 m/px at 48N zoom 19
+            
+    def test_calculate_haversine_distance(self, transformer):
+        p1 = GPSPoint(lat=48.275292, lon=37.385220)
+        p2 = GPSPoint(lat=48.275001, lon=37.382922)
+        dist = transformer.calculate_haversine_distance(p1, p2)
+        assert 100.0 < dist < 200.0 # Expecting ~150-200m based on spec