mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-04-23 00:46:37 +00:00
Denys Zaitsev
194 lines
8.2 KiB
Python
194 lines
8.2 KiB
Python
import pytest
|
|
import numpy as np
|
|
from unittest.mock import Mock, patch
|
|
|
|
from f09_local_geospatial_anchoring import LocalGeospatialAnchoring, LiteSAMConfig
|
|
from f04_satellite_data_manager import TileBounds
|
|
from f02_1_flight_lifecycle_manager import GPSPoint
|
|
|
|
@pytest.fixture
|
|
def anchoring():
|
|
return LocalGeospatialAnchoring()
|
|
|
|
@pytest.fixture
|
|
def dummy_uav_image():
|
|
return np.zeros((480, 640, 3), dtype=np.uint8)
|
|
|
|
@pytest.fixture
|
|
def dummy_sat_tile():
|
|
return np.zeros((512, 512, 3), dtype=np.uint8)
|
|
|
|
@pytest.fixture
|
|
def mock_tile_bounds():
|
|
return TileBounds(
|
|
nw=GPSPoint(lat=48.1, lon=37.0),
|
|
ne=GPSPoint(lat=48.1, lon=37.1),
|
|
sw=GPSPoint(lat=48.0, lon=37.0),
|
|
se=GPSPoint(lat=48.0, lon=37.1),
|
|
center=GPSPoint(lat=48.05, lon=37.05),
|
|
gsd=0.3
|
|
)
|
|
|
|
class TestLocalGeospatialAnchoring:
|
|
|
|
# --- Feature 09.01: Single Image Alignment ---
|
|
|
|
def test_pixel_to_gps(self, anchoring, mock_tile_bounds):
|
|
# Top Left (NW)
|
|
gps_nw = anchoring._pixel_to_gps(0, 0, mock_tile_bounds, 512, 512)
|
|
assert np.isclose(gps_nw.lat, 48.1)
|
|
assert np.isclose(gps_nw.lon, 37.0)
|
|
|
|
# Center
|
|
gps_center = anchoring._pixel_to_gps(256, 256, mock_tile_bounds, 512, 512)
|
|
assert np.isclose(gps_center.lat, 48.05)
|
|
assert np.isclose(gps_center.lon, 37.05)
|
|
|
|
# Bottom Right (SE)
|
|
gps_se = anchoring._pixel_to_gps(512, 512, mock_tile_bounds, 512, 512)
|
|
assert np.isclose(gps_se.lat, 48.0)
|
|
assert np.isclose(gps_se.lon, 37.1)
|
|
|
|
def test_compute_reprojection_error(self, anchoring):
|
|
H = np.eye(3) # Identity matrix
|
|
src = np.array([[10.0, 10.0], [20.0, 20.0]])
|
|
dst = np.array([[11.0, 10.0], [20.0, 22.0]])
|
|
|
|
# Errors: pt1 -> 1px (x), pt2 -> 2px (y). Mean = 1.5
|
|
err = anchoring._compute_reprojection_error(H, src, dst)
|
|
assert err == 1.5
|
|
|
|
def test_compute_homography_success(self, anchoring, dummy_uav_image, dummy_sat_tile):
|
|
res = anchoring.compute_homography(dummy_uav_image, dummy_sat_tile)
|
|
assert res is not None
|
|
|
|
H, stats = res
|
|
assert H.shape == (3, 3)
|
|
assert stats["inlier_count"] >= anchoring.config.min_inliers
|
|
assert stats["mre"] >= 0.0
|
|
|
|
def test_compute_homography_failure(self, anchoring, dummy_uav_image, dummy_sat_tile):
|
|
# Force poor RANSAC output via mock
|
|
with patch.object(anchoring, '_estimate_homography_ransac', return_value=(None, np.array([]))):
|
|
res = anchoring.compute_homography(dummy_uav_image, dummy_sat_tile)
|
|
assert res is None
|
|
|
|
def test_extract_gps_from_alignment(self, anchoring, mock_tile_bounds):
|
|
# Pure translation homography mapping UAV center (320, 240) to sat center (256, 256)
|
|
H = np.array([
|
|
[1.0, 0.0, 256.0 - 320.0],
|
|
[0.0, 1.0, 256.0 - 240.0],
|
|
[0.0, 0.0, 1.0]
|
|
])
|
|
|
|
gps = anchoring.extract_gps_from_alignment(H, mock_tile_bounds, (320, 240))
|
|
# Should map exactly to the tile center GPS
|
|
assert np.isclose(gps.lat, 48.05)
|
|
assert np.isclose(gps.lon, 37.05)
|
|
|
|
def test_compute_match_confidence(self, anchoring):
|
|
# High confidence criteria
|
|
assert anchoring.compute_match_confidence(0.7, 60, 0.3, 1.0) > 0.8
|
|
|
|
# Medium confidence criteria
|
|
conf_med = anchoring.compute_match_confidence(0.5, 40, 1.0, 0.8)
|
|
assert 0.5 <= conf_med <= 0.8
|
|
|
|
# Low confidence criteria (High error or low inliers)
|
|
assert anchoring.compute_match_confidence(0.3, 20, 1.5, 0.5) < 0.5
|
|
assert anchoring.compute_match_confidence(0.9, 10, 0.2, 1.0) == 0.0 # Below min inliers
|
|
|
|
def test_align_to_satellite(self, anchoring, dummy_uav_image, dummy_sat_tile, mock_tile_bounds):
|
|
result = anchoring.align_to_satellite(dummy_uav_image, dummy_sat_tile, mock_tile_bounds)
|
|
|
|
assert result is not None
|
|
assert result.matched is True
|
|
assert result.homography.shape == (3, 3)
|
|
assert -90 <= result.gps_center.lat <= 90
|
|
assert -180 <= result.gps_center.lon <= 180
|
|
assert 0.0 <= result.confidence <= 1.0
|
|
|
|
# --- Feature 09.02: Chunk Alignment ---
|
|
|
|
def test_extract_chunk_features(self, anchoring, dummy_uav_image):
|
|
features = anchoring._extract_chunk_features([dummy_uav_image, dummy_uav_image])
|
|
assert len(features) == 2
|
|
assert features[0].shape == (100, 256)
|
|
|
|
def test_aggregate_features(self, anchoring):
|
|
feat1 = np.ones((100, 256))
|
|
feat2 = np.ones((100, 256)) * 3
|
|
agg = anchoring._aggregate_features([feat1, feat2])
|
|
assert np.allclose(agg, np.ones((100, 256)) * 2)
|
|
|
|
def test_aggregate_correspondences(self, anchoring):
|
|
src1 = np.array([[1, 1], [2, 2]])
|
|
dst1 = np.array([[3, 3], [4, 4]])
|
|
src2 = np.array([[5, 5]])
|
|
dst2 = np.array([[6, 6]])
|
|
|
|
src_agg, dst_agg = anchoring._aggregate_correspondences([(src1, dst1), (src2, dst2)])
|
|
assert len(src_agg) == 3
|
|
assert len(dst_agg) == 3
|
|
assert np.array_equal(src_agg[2], [5, 5])
|
|
|
|
def test_compute_sim3_transform(self, anchoring, mock_tile_bounds):
|
|
# 90 deg rotation, scale 2.0, translation 10, 20
|
|
H = np.array([
|
|
[0.0, -2.0, 10.0],
|
|
[2.0, 0.0, 20.0],
|
|
[0.0, 0.0, 1.0]
|
|
])
|
|
|
|
sim3 = anchoring._compute_sim3_transform(H, mock_tile_bounds)
|
|
assert np.isclose(sim3.scale, 2.0)
|
|
assert np.isclose(sim3.translation[0], 10.0)
|
|
assert np.isclose(sim3.translation[1], 20.0)
|
|
assert np.isclose(sim3.rotation[0, 0], 0.0, atol=1e-5)
|
|
assert np.isclose(sim3.rotation[1, 0], 1.0, atol=1e-5)
|
|
|
|
def test_get_chunk_center_gps(self, anchoring, dummy_uav_image, mock_tile_bounds):
|
|
H = np.array([
|
|
[1.0, 0.0, 256.0 - 320.0],
|
|
[0.0, 1.0, 256.0 - 240.0],
|
|
[0.0, 0.0, 1.0]
|
|
])
|
|
chunk = [dummy_uav_image, dummy_uav_image, dummy_uav_image]
|
|
gps = anchoring._get_chunk_center_gps(H, mock_tile_bounds, chunk)
|
|
assert np.isclose(gps.lat, 48.05)
|
|
assert np.isclose(gps.lon, 37.05)
|
|
|
|
def test_validate_chunk_match(self, anchoring):
|
|
assert anchoring._validate_chunk_match(50, 0.8) is True
|
|
assert anchoring._validate_chunk_match(20, 0.8) is False # < 30 inliers
|
|
assert anchoring._validate_chunk_match(50, 0.5) is False # < 0.7 confidence
|
|
|
|
def test_align_chunk_to_satellite_success(self, anchoring, dummy_uav_image, dummy_sat_tile, mock_tile_bounds):
|
|
with patch.object(anchoring, 'compute_match_confidence', return_value=0.9):
|
|
chunk = [dummy_uav_image] * 5
|
|
res = anchoring.align_chunk_to_satellite(chunk, dummy_sat_tile, mock_tile_bounds)
|
|
assert res is not None
|
|
assert res.matched is True
|
|
assert res.inlier_count >= 30
|
|
assert res.transform is not None
|
|
|
|
def test_align_chunk_to_satellite_failure(self, anchoring, dummy_uav_image, dummy_sat_tile, mock_tile_bounds):
|
|
with patch.object(anchoring, '_estimate_chunk_homography', return_value=(None, {})):
|
|
chunk = [dummy_uav_image] * 5
|
|
res = anchoring.align_chunk_to_satellite(chunk, dummy_sat_tile, mock_tile_bounds)
|
|
assert res is None
|
|
|
|
def test_chunk_vs_single_image_robustness(self, anchoring, dummy_uav_image, dummy_sat_tile, mock_tile_bounds):
|
|
# Single image failure
|
|
with patch.object(anchoring, '_estimate_homography_ransac') as mock_ransac:
|
|
mock_ransac.return_value = (np.eye(3), np.array([[1]]*10 + [[0]]*90))
|
|
single_res = anchoring.align_to_satellite(dummy_uav_image, dummy_sat_tile, mock_tile_bounds)
|
|
assert single_res is None
|
|
|
|
# Chunk success
|
|
with patch.object(anchoring, '_estimate_homography_ransac') as mock_ransac:
|
|
mock_ransac.return_value = (np.eye(3), np.array([[1]]*50 + [[0]]*450))
|
|
with patch.object(anchoring, 'compute_match_confidence', return_value=0.8):
|
|
chunk = [dummy_uav_image] * 5
|
|
chunk_res = anchoring.align_chunk_to_satellite(chunk, dummy_sat_tile, mock_tile_bounds)
|
|
assert chunk_res is not None |