mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-04-22 06:56:36 +00:00
Denys Zaitsev
43 lines
1.5 KiB
Python
43 lines
1.5 KiB
Python
import pytest
|
|
import numpy as np
|
|
|
|
from h01_camera_model import CameraModel
|
|
from f02_1_flight_lifecycle_manager import CameraParameters
|
|
|
|
@pytest.fixture
|
|
def camera_model():
|
|
return CameraModel()
|
|
|
|
@pytest.fixture
|
|
def camera_params():
|
|
return CameraParameters(
|
|
focal_length_mm=25.0,
|
|
sensor_width_mm=36.0,
|
|
resolution={"width": 640, "height": 480}
|
|
)
|
|
|
|
class TestCameraModel:
|
|
|
|
def test_intrinsic_matrix_structure(self, camera_model, camera_params):
|
|
K = camera_model._get_intrinsics(camera_params)
|
|
# fx = 25 * 640 / 36 = 444.444
|
|
assert np.isclose(K[0, 0], 444.444, atol=1e-3)
|
|
assert np.isclose(K[0, 2], 320.0)
|
|
|
|
def test_pixel_to_ray_principal_point(self, camera_model, camera_params):
|
|
ray = camera_model.unproject((320.0, 240.0), 1.0, camera_params)
|
|
np.testing.assert_allclose(ray, np.array([0.0, 0.0, 1.0]))
|
|
|
|
def test_project_to_pixel(self, camera_model, camera_params):
|
|
point_3d = np.array([0.0, 0.0, 10.0])
|
|
u, v = camera_model.project(point_3d, camera_params)
|
|
|
|
assert u == 320.0
|
|
assert v == 240.0
|
|
|
|
def test_projection_unprojection_cycle(self, camera_model, camera_params):
|
|
original_u, original_v = 150.0, 400.0
|
|
ray = camera_model.unproject((original_u, original_v), 50.0, camera_params)
|
|
projected_u, projected_v = camera_model.project(ray, camera_params)
|
|
assert np.isclose(original_u, projected_u)
|
|
assert np.isclose(original_v, projected_v) |