From c943cc047e3524ae1feb02d4004a44c98da735a9 Mon Sep 17 00:00:00 2001 From: asterbot Date: Sun, 3 Nov 2024 15:58:51 -0500 Subject: [PATCH 01/12] Rudimentary Geolocation Added basic class we can build on and just copied the unit tests for now --- modules/rudimentary_geolocation/__init__.py | 0 .../camera_properties.py | 267 ++++ .../geolocation_worker.py | 64 + .../rudimentary_geolocation.py | 86 ++ tests/unit/test_rudimentary_geolocation.py | 1070 +++++++++++++++++ 5 files changed, 1487 insertions(+) create mode 100644 modules/rudimentary_geolocation/__init__.py create mode 100644 modules/rudimentary_geolocation/camera_properties.py create mode 100644 modules/rudimentary_geolocation/geolocation_worker.py create mode 100644 modules/rudimentary_geolocation/rudimentary_geolocation.py create mode 100644 tests/unit/test_rudimentary_geolocation.py diff --git a/modules/rudimentary_geolocation/__init__.py b/modules/rudimentary_geolocation/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/modules/rudimentary_geolocation/camera_properties.py b/modules/rudimentary_geolocation/camera_properties.py new file mode 100644 index 00000000..76f93999 --- /dev/null +++ b/modules/rudimentary_geolocation/camera_properties.py @@ -0,0 +1,267 @@ +""" +Camera intrinsic and extrinsic properties. +""" + +import numpy as np + + +def is_vector_r3(vec: np.ndarray) -> bool: + """ + Checks if the numpy array is a vector in R^3 . + """ + return vec.shape == (3,) + + +def is_matrix_r3x3(matrix: np.ndarray) -> bool: + """ + Checks if the numpy array is a matrix in R^{3x3} . + """ + return matrix.shape == (3, 3) + + +def create_rotation_matrix_from_orientation( + yaw: float, pitch: float, roll: float +) -> "tuple[bool, np.ndarray | None]": + """ + Creates a rotation matrix from yaw pitch roll in NED system (x forward, y right, z down). + Specifically, intrinsic (Tait-Bryan) rotations in the zyx/3-2-1 order. + See: https://en.wikipedia.org/wiki/Rotation_matrix#General_rotations + + yaw, pitch, roll are in radians from -pi to pi . + """ + if yaw < -np.pi or yaw > np.pi: + return False, None + + if pitch < -np.pi or pitch > np.pi: + return False, None + + if roll < -np.pi or roll > np.pi: + return False, None + + # fmt: off + yaw_matrix = np.array( + [ + [np.cos(yaw), -np.sin(yaw), 0.0], + [np.sin(yaw), np.cos(yaw), 0.0], + [ 0.0, 0.0, 1.0], + ], + dtype=np.float32, + ) + # fmt: on + + # fmt: off + pitch_matrix = np.array( + [ + [ np.cos(pitch), 0.0, np.sin(pitch)], + [ 0.0, 1.0, 0.0], + [-np.sin(pitch), 0.0, np.cos(pitch)], + ], + dtype=np.float32, + ) + # fmt: on + + # fmt: off + roll_matrix = np.array( + [ + [1.0, 0.0, 0.0], + [0.0, np.cos(roll), -np.sin(roll)], + [0.0, np.sin(roll), np.cos(roll)], + ], + dtype=np.float32, + ) + # fmt: on + + rotation_matrix = yaw_matrix @ pitch_matrix @ roll_matrix + + return True, rotation_matrix + + +class CameraIntrinsics: + """ + Camera space information. + + Image follows xy system with top-left as origin, pixel coordinates + refer to their top-left corner. + """ + + __create_key = object() + + @classmethod + def create( + cls, resolution_x: int, resolution_y: int, fov_x: float, fov_y: float + ) -> "tuple[bool, CameraIntrinsics | None]": + """ + Resolution is in pixels. + Field of view in radians horizontally and vertically across the image (edge to edge). + """ + if resolution_x < 0: + return False, None + + if resolution_y < 0: + return False, None + + if fov_x <= 0.0 or fov_x >= np.pi: + return False, None + + if fov_y <= 0.0 or fov_y >= np.pi: + return False, None + + u_scalar = np.tan(fov_x / 2) + if not np.isfinite(u_scalar): + return False, None + + v_scalar = np.tan(fov_y / 2) + if not np.isfinite(v_scalar): + return False, None + + return True, CameraIntrinsics( + cls.__create_key, + resolution_x, + resolution_y, + u_scalar, + v_scalar, + ) + + def __init__( + self, + class_private_create_key: object, + resolution_x: int, + resolution_y: int, + u_scalar: float, + v_scalar: float, + ) -> None: + """ + Private constructor, use create() method. + """ + assert class_private_create_key is CameraIntrinsics.__create_key, "Use create() method" + + self.resolution_x = resolution_x + self.resolution_y = resolution_y + + self.__vec_c = np.array([1.0, 0.0, 0.0], dtype=np.float32) + self.__vec_u = np.array([0.0, u_scalar, 0.0], dtype=np.float32) + self.__vec_v = np.array([0.0, 0.0, v_scalar], dtype=np.float32) + + @staticmethod + def __pixel_vector_from_image_space( + pixel: int, resolution: int, vec_base: np.ndarray + ) -> "tuple[bool, np.ndarray | None]": + """ + Get u or v vector from pixel coordinate. + """ + if pixel < 0: + return False, None + + if resolution < 0: + return False, None + + if pixel > resolution: + return False, None + + if not is_vector_r3(vec_base): + return False, None + + # Scaling factor with translation: 2 * p / r - 1 == (2 * p - r) / r + # Codomain is from -1 to 1 inclusive + scaling = float(2 * pixel - resolution) / float(resolution) + + vec_pixel = scaling * vec_base + + return True, vec_pixel + + def camera_space_from_image_space( + self, pixel_x: int, pixel_y: int + ) -> "tuple[bool, np.ndarray | None]": + """ + Pixel in image space to vector in camera space. + """ + if pixel_x < 0: + return False, None + + if pixel_y < 0: + return False, None + + result, vec_pixel_u = self.__pixel_vector_from_image_space( + pixel_x, + self.resolution_x, + self.__vec_u, + ) + if not result: + return False, None + + result, vec_pixel_v = self.__pixel_vector_from_image_space( + pixel_y, + self.resolution_y, + self.__vec_v, + ) + if not result: + return False, None + + # Get Pylance to stop complaining + assert vec_pixel_u is not None + assert vec_pixel_v is not None + + vec_camera = self.__vec_c + vec_pixel_u + vec_pixel_v + + return True, vec_camera + + +class CameraDroneExtrinsics: + """ + Camera in relation to drone. + """ + + __create_key = object() + + @classmethod + def create( + cls, + camera_position_xyz: "tuple[float, float, float]", + camera_orientation_ypr: "tuple[float, float, float]", + ) -> "tuple[bool, CameraDroneExtrinsics | None]": + """ + camera_position_xyz: Camera position is x, y, z. + camera_orientation_ypr: Camera orientation is yaw, pitch, roll. + + Both are relative to drone in NED system (x forward, y right, z down). + Specifically, intrinsic (Tait-Bryan) rotations in the zyx/3-2-1 order. + """ + # Unpack parameters + camera_x, camera_y, camera_z = camera_position_xyz + camera_yaw, camera_pitch, camera_roll = camera_orientation_ypr + + vec_camera_on_drone_position = np.array([camera_x, camera_y, camera_z], dtype=np.float32) + + result, camera_to_drone_rotation_matrix = create_rotation_matrix_from_orientation( + camera_yaw, + camera_pitch, + camera_roll, + ) + if not result: + return False, None + + # Get Pylance to stop complaining + assert camera_to_drone_rotation_matrix is not None + + if not is_matrix_r3x3(camera_to_drone_rotation_matrix): + return False, None + + return True, CameraDroneExtrinsics( + cls.__create_key, + vec_camera_on_drone_position, + camera_to_drone_rotation_matrix, + ) + + def __init__( + self, + class_private_create_key: object, + vec_camera_on_drone_position: np.ndarray, + camera_to_drone_rotation_matrix: np.ndarray, + ) -> None: + """ + Private constructor, use create() method. + """ + assert class_private_create_key is CameraDroneExtrinsics.__create_key, "Use create() method" + + self.vec_camera_on_drone_position = vec_camera_on_drone_position + self.camera_to_drone_rotation_matrix = camera_to_drone_rotation_matrix diff --git a/modules/rudimentary_geolocation/geolocation_worker.py b/modules/rudimentary_geolocation/geolocation_worker.py new file mode 100644 index 00000000..4a7d2436 --- /dev/null +++ b/modules/rudimentary_geolocation/geolocation_worker.py @@ -0,0 +1,64 @@ +""" +Convert bounding box data into ground data. +""" + +import os +import pathlib + +from utilities.workers import queue_proxy_wrapper +from utilities.workers import worker_controller +from . import camera_properties +from . import geolocation +from ..common.modules import logger + + +def geolocation_worker( + camera_intrinsics: camera_properties.CameraIntrinsics, + camera_drone_extrinsics: camera_properties.CameraDroneExtrinsics, + input_queue: queue_proxy_wrapper.QueueProxyWrapper, + output_queue: queue_proxy_wrapper.QueueProxyWrapper, + controller: worker_controller.WorkerController, +) -> None: + """ + Worker process. + + input_queue and output_queue are data queues. + controller is how the main process communicates to this worker process. + """ + # TODO: Handle errors better + + worker_name = pathlib.Path(__file__).stem + process_id = os.getpid() + result, local_logger = logger.Logger.create(f"{worker_name}_{process_id}", True) + if not result: + print("ERROR: Worker failed to create logger") + return + + assert local_logger is not None + + local_logger.info("Logger initialized") + + result, locator = geolocation.Geolocation.create( + camera_intrinsics, + camera_drone_extrinsics, + local_logger, + ) + if not result: + local_logger.error("Worker failed to create class object") + return + + # Get Pylance to stop complaining + assert locator is not None + + while not controller.is_exit_requested(): + controller.check_pause() + + input_data = input_queue.queue.get() + if input_data is None: + break + + result, value = locator.run(input_data) + if not result: + continue + + output_queue.queue.put(value) diff --git a/modules/rudimentary_geolocation/rudimentary_geolocation.py b/modules/rudimentary_geolocation/rudimentary_geolocation.py new file mode 100644 index 00000000..656b6dd0 --- /dev/null +++ b/modules/rudimentary_geolocation/rudimentary_geolocation.py @@ -0,0 +1,86 @@ +""" +Converts image space into world space. +""" + +import cv2 +import numpy as np + +from . import camera_properties +from .. import detection_in_world +from .. import detections_and_time +from .. import merged_odometry_detections +from ..common.modules import logger + + +class RudimentaryGeolocation: + """ + Converts image space into world space. + """ + + __create_key = object() + + __MIN_DOWN_COS_ANGLE = 0.1 # radians, ~84° + + @classmethod + def create( + cls, + camera_intrinsics: camera_properties.CameraIntrinsics, + camera_drone_extrinsics: camera_properties.CameraDroneExtrinsics, + local_logger: logger.Logger, + ) -> "tuple[bool, RudimentaryGeolocation | None]": + """ + camera_intrinsics: Camera information without any outside space. + camera_drone_extrinsics: Camera information related to the drone without any world space. + """ + # Centre of each edge + # list[list[float]] required for OpenCV + perspective_transform_sources = [ + [camera_intrinsics.resolution_x / 2, 0], + [camera_intrinsics.resolution_x / 2, camera_intrinsics.resolution_y], + [0, camera_intrinsics.resolution_y / 2], + [camera_intrinsics.resolution_x, camera_intrinsics.resolution_y / 2], + ] + + # Orientation in world space + rotated_source_vectors = [] + for source in perspective_transform_sources: + # Image space to camera space + result, value = camera_intrinsics.camera_space_from_image_space(source[0], source[1]) + if not result: + local_logger.error( + f"Rotated source vector could not be created for source: {source}" + ) + return False, None + + # Get Pylance to stop complaining + assert value is not None + + # Camera space to world space (orientation only) + vec_rotated_source = camera_drone_extrinsics.camera_to_drone_rotation_matrix @ value + rotated_source_vectors.append(vec_rotated_source) + + return True, RudimentaryGeolocation( + cls.__create_key, + camera_drone_extrinsics, + perspective_transform_sources, + rotated_source_vectors, + local_logger, + ) + + def __init__( + self, + class_private_create_key: object, + camera_drone_extrinsics: camera_properties.CameraDroneExtrinsics, + perspective_transform_sources: "list[list[float]]", + rotated_source_vectors: "list[np.ndarray]", + local_logger: logger.Logger, + ) -> None: + """ + Private constructor, use create() method. + """ + assert class_private_create_key is RudimentaryGeolocation.__create_key, "Use create() method" + + self.__camera_drone_extrinsics = camera_drone_extrinsics + self.__perspective_transform_sources = perspective_transform_sources + self.__rotated_source_vectors = rotated_source_vectors + self.__logger = local_logger diff --git a/tests/unit/test_rudimentary_geolocation.py b/tests/unit/test_rudimentary_geolocation.py new file mode 100644 index 00000000..809ed408 --- /dev/null +++ b/tests/unit/test_rudimentary_geolocation.py @@ -0,0 +1,1070 @@ +""" +Test geolocation. +""" + +import numpy as np +import pytest + +from modules import detection_in_world +from modules import detections_and_time +from modules import merged_odometry_detections +from modules.common.modules import logger +from modules.common.modules import drone_odometry_local +from modules.geolocation import camera_properties +from modules.geolocation import geolocation + +FLOAT_PRECISION_TOLERANCE = 4 + + +# Test functions use test fixture signature names and access class privates +# No enable +# pylint: disable=protected-access,redefined-outer-name + + +@pytest.fixture +def basic_locator() -> geolocation.Geolocation: # type: ignore + """ + Forwards pointing camera. + """ + result, camera_intrinsics = camera_properties.CameraIntrinsics.create( + 2000, + 2000, + np.pi / 2, + np.pi / 2, + ) + assert result + assert camera_intrinsics is not None + + result, camera_extrinsics = camera_properties.CameraDroneExtrinsics.create( + (0.0, 0.0, 0.0), + (0.0, 0.0, 0.0), + ) + assert result + assert camera_extrinsics is not None + + result, test_logger = logger.Logger.create("test_logger", False) + assert result + assert test_logger is not None + + result, locator = geolocation.Geolocation.create( + camera_intrinsics, + camera_extrinsics, + test_logger, + ) + assert result + assert locator is not None + + yield locator # type: ignore + + +@pytest.fixture +def intermediate_locator() -> geolocation.Geolocation: # type: ignore + """ + Downwards pointing camera offset towards front of drone. + """ + result, camera_intrinsics = camera_properties.CameraIntrinsics.create( + 2000, + 2000, + np.pi / 2, + np.pi / 2, + ) + assert result + assert camera_intrinsics is not None + + result, camera_extrinsics = camera_properties.CameraDroneExtrinsics.create( + (1.0, 0.0, 0.0), + (0.0, -np.pi / 2, 0.0), + ) + assert result + assert camera_extrinsics is not None + + result, test_logger = logger.Logger.create("test_logger", False) + assert result + assert test_logger is not None + + result, locator = geolocation.Geolocation.create( + camera_intrinsics, + camera_extrinsics, + test_logger, + ) + assert result + assert locator is not None + + yield locator # type: ignore + + +@pytest.fixture +def advanced_locator() -> geolocation.Geolocation: # type: ignore + """ + Camera angled at 75° upward. + Drone is expected to rotate it downwards. + """ + result, camera_intrinsics = camera_properties.CameraIntrinsics.create( + 2000, + 2000, + np.pi / 3, + np.pi / 3, + ) + assert result + assert camera_intrinsics is not None + + result, camera_extrinsics = camera_properties.CameraDroneExtrinsics.create( + (1.0, 0.0, 0.0), + (0.0, np.pi * 5 / 12, np.pi / 2), + ) + assert result + assert camera_extrinsics is not None + + result, test_logger = logger.Logger.create("test_logger", False) + assert result + assert test_logger is not None + + result, locator = geolocation.Geolocation.create( + camera_intrinsics, + camera_extrinsics, + test_logger, + ) + assert result + assert locator is not None + + yield locator # type: ignore + + +@pytest.fixture +def detection_bottom_right_point() -> detections_and_time.Detection: # type: ignore + """ + Bounding box is a single point. + """ + result, detection = detections_and_time.Detection.create( + np.array([2000.0, 2000.0, 2000.0, 2000.0], dtype=np.float32), + 0, + 0.01, + ) + assert result + assert detection is not None + + yield detection # type: ignore + + +@pytest.fixture +def detection_centre_left_point() -> detections_and_time.Detection: # type: ignore + """ + Bounding box is a single point. + """ + result, detection = detections_and_time.Detection.create( + np.array([0.0, 1000.0, 0.0, 1000.0], dtype=np.float32), + 0, + 0.1, + ) + assert result + assert detection is not None + + yield detection # type: ignore + + +@pytest.fixture +def detection_1() -> detections_and_time.Detection: # type: ignore + """ + Entire image. + """ + result, detection = detections_and_time.Detection.create( + np.array([0.0, 0.0, 2000.0, 2000.0], dtype=np.float32), + 1, + 1.0 / 1, + ) + assert result + assert detection is not None + + yield detection # type: ignore + + +@pytest.fixture +def detection_2() -> detections_and_time.Detection: # type: ignore + """ + Quadrant. + """ + result, detection = detections_and_time.Detection.create( + np.array([0.0, 0.0, 1000.0, 1000.0], dtype=np.float32), + 2, + 1.0 / 2, + ) + assert result + assert detection is not None + + yield detection # type: ignore + + +@pytest.fixture +def affine_matrix() -> np.ndarray: # type: ignore + """ + 3x3 homogeneous. + """ + # fmt: off + matrix = np.array( + [ + [0.0, 1.0, -1.0], + [2.0, 0.0, -1.0], + [0.0, 0.0, 1.0], + ], + dtype=np.float32, + ) + # fmt: on + + yield matrix # type: ignore + + +@pytest.fixture +def non_affine_matrix() -> np.ndarray: # type: ignore + """ + 3x3 homogeneous. + """ + # fmt: off + matrix = np.array( + [ + [0.0, 1.0, -1.0], + [2.0, 0.0, -1.0], + [1.0, 1.0, 1.0], + ], + dtype=np.float32, + ) + # fmt: on + + yield matrix # type: ignore + + +class TestGeolocationCreate: + """ + Test constructor. + """ + + def test_normal(self) -> None: + """ + Successful construction. + """ + result, camera_intrinsics = camera_properties.CameraIntrinsics.create( + 2000, + 2000, + np.pi / 2, + np.pi / 2, + ) + assert result + assert camera_intrinsics is not None + + result, camera_extrinsics = camera_properties.CameraDroneExtrinsics.create( + (0.0, 0.0, 0.0), + (0.0, 0.0, 0.0), + ) + assert result + assert camera_extrinsics is not None + + result, test_logger = logger.Logger.create("test_logger", False) + assert result + assert test_logger is not None + + result, locator = geolocation.Geolocation.create( + camera_intrinsics, + camera_extrinsics, + test_logger, + ) + assert result + assert locator is not None + + +class TestGroundIntersection: + """ + Test where vector intersects with ground. + """ + + def test_above_origin_directly_down(self) -> None: + """ + Above origin, directly down. + """ + # Setup + result, test_logger = logger.Logger.create("test_logger", False) + assert result + assert test_logger is not None + + vec_camera_in_world_position = np.array([0.0, 0.0, -100.0], dtype=np.float32) + vec_down = np.array([0.0, 0.0, 1.0], dtype=np.float32) + + expected = np.array([0.0, 0.0], dtype=np.float32) + + # Run + ( + result, + actual, + ) = geolocation.Geolocation._Geolocation__ground_intersection_from_vector( # type: ignore + vec_camera_in_world_position, + vec_down, + test_logger, + ) + + # Test + assert result + assert actual is not None + np.testing.assert_almost_equal(actual, expected, decimal=FLOAT_PRECISION_TOLERANCE) + + def test_non_origin_directly_down(self) -> None: + """ + Directly down. + """ + # Setup + result, test_logger = logger.Logger.create("test_logger", False) + assert result + assert test_logger is not None + + vec_camera_in_world_position = np.array([100.0, -100.0, -100.0], dtype=np.float32) + vec_down = np.array([0.0, 0.0, 1.0], dtype=np.float32) + + expected = np.array([100.0, -100.0], dtype=np.float32) + + # Run + ( + result, + actual, + ) = geolocation.Geolocation._Geolocation__ground_intersection_from_vector( # type: ignore + vec_camera_in_world_position, + vec_down, + test_logger, + ) + + # Test + assert result + assert actual is not None + np.testing.assert_almost_equal(actual, expected, decimal=FLOAT_PRECISION_TOLERANCE) + + def test_above_origin_angled_down(self) -> None: + """ + Above origin, angled down towards positive. + """ + # Setup + result, test_logger = logger.Logger.create("test_logger", False) + assert result + assert test_logger is not None + + vec_camera_in_world_position = np.array([0.0, 0.0, -100.0], dtype=np.float32) + vec_down = np.array([1.0, 1.0, 1.0], dtype=np.float32) + + expected = np.array([100.0, 100.0], dtype=np.float32) + + # Run + ( + result, + actual, + ) = geolocation.Geolocation._Geolocation__ground_intersection_from_vector( # type: ignore + vec_camera_in_world_position, + vec_down, + test_logger, + ) + + # Test + assert result + assert actual is not None + np.testing.assert_almost_equal(actual, expected, decimal=FLOAT_PRECISION_TOLERANCE) + + def test_non_origin_angled_down(self) -> None: + """ + Angled down towards origin. + """ + # Setup + result, test_logger = logger.Logger.create("test_logger", False) + assert result + assert test_logger is not None + + vec_camera_in_world_position = np.array([100.0, -100.0, -100.0], dtype=np.float32) + vec_down = np.array([-1.0, 1.0, 1.0], dtype=np.float32) + + expected = np.array([0.0, 0.0], dtype=np.float32) + + # Run + ( + result, + actual, + ) = geolocation.Geolocation._Geolocation__ground_intersection_from_vector( # type: ignore + vec_camera_in_world_position, + vec_down, + test_logger, + ) + + # Test + assert result + assert actual is not None + np.testing.assert_almost_equal(actual, expected, decimal=FLOAT_PRECISION_TOLERANCE) + + def test_bad_almost_horizontal(self) -> None: + """ + False, None . + """ + # Setup + result, test_logger = logger.Logger.create("test_logger", False) + assert result + assert test_logger is not None + + vec_camera_in_world_position = np.array([0.0, 0.0, -100.0], dtype=np.float32) + vec_horizontal = np.array([10.0, 0.0, 1.0], dtype=np.float32) + + # Run + ( + result, + actual, + ) = geolocation.Geolocation._Geolocation__ground_intersection_from_vector( # type: ignore + vec_camera_in_world_position, + vec_horizontal, + test_logger, + ) + + # Test + assert not result + assert actual is None + + def test_bad_upwards(self) -> None: + """ + False, None . + """ + # Setup + result, test_logger = logger.Logger.create("test_logger", False) + assert result + assert test_logger is not None + + vec_camera_in_world_position = np.array([0.0, 0.0, -100.0], dtype=np.float32) + vec_up = np.array([0.0, 0.0, -1.0], dtype=np.float32) + + # Run + ( + result, + actual, + ) = geolocation.Geolocation._Geolocation__ground_intersection_from_vector( # type: ignore + vec_camera_in_world_position, + vec_up, + test_logger, + ) + + # Test + assert not result + assert actual is None + + def test_bad_underground(self) -> None: + """ + False, None . + """ + # Setup + result, test_logger = logger.Logger.create("test_logger", False) + assert result + assert test_logger is not None + + vec_underground = np.array([0.0, 0.0, 1.0], dtype=np.float32) + vec_down = np.array([0.0, 0.0, 1.0], dtype=np.float32) + + # Run + ( + result, + actual, + ) = geolocation.Geolocation._Geolocation__ground_intersection_from_vector( # type: ignore + vec_underground, + vec_down, + test_logger, + ) + + # Test + assert not result + assert actual is None + + +class TestPerspectiveTransformMatrix: + """ + Test perspective transform creation. + """ + + def test_basic_above_origin_pointed_down(self, basic_locator: geolocation.Geolocation) -> None: + """ + Above origin, directly down. + """ + # Setup + result, drone_rotation_matrix = camera_properties.create_rotation_matrix_from_orientation( + 0.0, + -np.pi / 2, + 0.0, + ) + assert result + assert drone_rotation_matrix is not None + + drone_position_ned = np.array([0.0, 0.0, -100.0], dtype=np.float32) + + vec_ground_expected = np.array([-100.0, 100.0, 1.0], dtype=np.float32) + + # Run + ( + result, + actual, + ) = basic_locator._Geolocation__get_perspective_transform_matrix( # type: ignore + drone_rotation_matrix, + drone_position_ned, + ) + + # Test + assert result + assert actual is not None + + vec_ground = actual @ np.array([2000, 2000, 1]) + vec_ground_normalized = vec_ground / vec_ground[2] + np.testing.assert_almost_equal( + vec_ground_normalized, + vec_ground_expected, + decimal=FLOAT_PRECISION_TOLERANCE, + ) + + def test_intermediate_above_origin_pointing_north( + self, intermediate_locator: geolocation.Geolocation + ) -> None: + """ + Positioned so that the camera is above the origin directly down (but the drone is not). + """ + # Setup + result, drone_rotation_matrix = camera_properties.create_rotation_matrix_from_orientation( + 0.0, + 0.0, + 0.0, + ) + assert result + assert drone_rotation_matrix is not None + + drone_position_ned = np.array([-1.0, 0.0, -100.0], dtype=np.float32) + + vec_ground_expected = np.array([-100.0, 100.0, 1.0], dtype=np.float32) + + # Run + ( + result, + actual, + ) = intermediate_locator._Geolocation__get_perspective_transform_matrix( # type: ignore + drone_rotation_matrix, + drone_position_ned, + ) + + # Test + assert result + assert actual is not None + + vec_ground = actual @ np.array([2000, 2000, 1]) + vec_ground_normalized = vec_ground / vec_ground[2] + np.testing.assert_almost_equal( + vec_ground_normalized, + vec_ground_expected, + decimal=FLOAT_PRECISION_TOLERANCE, + ) + + def test_intermediate_above_origin_pointing_west( + self, intermediate_locator: geolocation.Geolocation + ) -> None: + """ + Positioned so that the camera is above the origin directly down (but the drone is not). + """ + # Setup + result, drone_rotation_matrix = camera_properties.create_rotation_matrix_from_orientation( + -np.pi / 2, + 0.0, + 0.0, + ) + assert result + assert drone_rotation_matrix is not None + + drone_position_ned = np.array([0.0, 1.0, -100.0], dtype=np.float32) + + vec_ground_expected = np.array([100.0, 100.0, 1.0], dtype=np.float32) + + # Run + ( + result, + actual, + ) = intermediate_locator._Geolocation__get_perspective_transform_matrix( # type: ignore + drone_rotation_matrix, + drone_position_ned, + ) + + # Test + assert result + assert actual is not None + + vec_ground = actual @ np.array([2000, 2000, 1]) + vec_ground_normalized = vec_ground / vec_ground[2] + np.testing.assert_almost_equal( + vec_ground_normalized, + vec_ground_expected, + decimal=FLOAT_PRECISION_TOLERANCE, + ) + + def test_advanced(self, advanced_locator: geolocation.Geolocation) -> None: + """ + Camera is north of origin with an angle from vertical. Also rotated. + """ + # Setup + result, drone_rotation_matrix = camera_properties.create_rotation_matrix_from_orientation( + 0.0, + np.pi / 12, + -np.pi, + ) + assert result + assert drone_rotation_matrix is not None + + drone_position_ned = np.array( + [ + 10.0 - np.cos(-np.pi / 12), # Camera at 10 units forward + 0.0, + -100.0 - np.sin(-np.pi / 12), # Camera at 100 units above ground + ], + dtype=np.float32, + ) + + vec_ground_sanity_expected = np.array([10.0, 0.0, 1.0], dtype=np.float32) + vec_ground_expected = np.array( + [ + 10.0 + 100.0 * np.sqrt(3), + 100.0, + 1.0, + ], + dtype=np.float32, + ) + + # Run + ( + result, + actual, + ) = advanced_locator._Geolocation__get_perspective_transform_matrix( # type: ignore + drone_rotation_matrix, + drone_position_ned, + ) + + # Test + assert result + assert actual is not None + + vec_ground_sanity = actual @ np.array([0, 1000, 1]) + vec_ground_sanity_actual = vec_ground_sanity / vec_ground_sanity[2] + np.testing.assert_almost_equal( + vec_ground_sanity_actual, + vec_ground_sanity_expected, + decimal=FLOAT_PRECISION_TOLERANCE, + ) + + vec_ground = actual @ np.array([2000, 2000, 1]) + vec_ground_normalized = vec_ground / vec_ground[2] + np.testing.assert_almost_equal( + vec_ground_normalized, + vec_ground_expected, + decimal=FLOAT_PRECISION_TOLERANCE, + ) + + def test_bad_direction(self, basic_locator: geolocation.Geolocation) -> None: + """ + Camera pointing forward. + """ + # Setup + result, drone_rotation_matrix = camera_properties.create_rotation_matrix_from_orientation( + 0.0, + 0.0, + 0.0, + ) + assert result + assert drone_rotation_matrix is not None + + drone_position_ned = np.array( + [ + 0.0, + 0.0, + -100.0, + ], + dtype=np.float32, + ) + + # Run + ( + result, + actual, + ) = basic_locator._Geolocation__get_perspective_transform_matrix( # type: ignore + drone_rotation_matrix, + drone_position_ned, + ) + + # Test + assert not result + assert actual is None + + +class TestGeolocationConvertDetection: + """ + Test extract and convert. + """ + + def test_normal1( + self, detection_1: detections_and_time.Detection, affine_matrix: np.ndarray + ) -> None: + """ + Normal detection and matrix. + """ + # Setup + result, test_logger = logger.Logger.create("test_logger", False) + assert result + assert test_logger is not None + + result, expected = detection_in_world.DetectionInWorld.create( + # fmt: off + np.array( + [ + [ -1.0, -1.0], + [ -1.0, 3999.0], + [1999.0, -1.0], + [1999.0, 3999.0], + ], + dtype=np.float32, + ), + # fmt: on + np.array( + [999.0, 1999.0], + dtype=np.float32, + ), + 1, + 1.0, + ) + assert result + assert expected is not None + + # Run + ( + result, + actual, + ) = geolocation.Geolocation._Geolocation__convert_detection_to_world_from_image( # type: ignore + detection_1, + affine_matrix, + test_logger, + ) + + # Test + assert result + assert actual is not None + + np.testing.assert_almost_equal(actual.vertices, expected.vertices) + np.testing.assert_almost_equal(actual.centre, expected.centre) + assert actual.label == expected.label + np.testing.assert_almost_equal(actual.confidence, expected.confidence) + + def test_normal2( + self, detection_2: detections_and_time.Detection, affine_matrix: np.ndarray + ) -> None: + """ + Normal detection and matrix. + """ + # Setup + result, test_logger = logger.Logger.create("test_logger", False) + assert result + assert test_logger is not None + + result, expected = detection_in_world.DetectionInWorld.create( + # fmt: off + np.array( + [ + [ -1.0, -1.0], + [ -1.0, 1999.0], + [999.0, -1.0], + [999.0, 1999.0], + ], + dtype=np.float32, + ), + # fmt: on + np.array( + [499.0, 999.0], + dtype=np.float32, + ), + 2, + 0.5, + ) + assert result + assert expected is not None + + # Run + ( + result, + actual, + ) = geolocation.Geolocation._Geolocation__convert_detection_to_world_from_image( # type: ignore + detection_2, + affine_matrix, + test_logger, + ) + + # Test + assert result + assert actual is not None + + np.testing.assert_almost_equal(actual.vertices, expected.vertices) + np.testing.assert_almost_equal(actual.centre, expected.centre) + assert actual.label == expected.label + np.testing.assert_almost_equal(actual.confidence, expected.confidence) + + +class TestGeolocationRun: + """ + Run. + """ + + def test_basic( + self, + basic_locator: geolocation.Geolocation, + detection_1: detections_and_time.Detection, + detection_2: detections_and_time.Detection, + ) -> None: + """ + 2 detections. + """ + # Setup + result, drone_position = drone_odometry_local.DronePositionLocal.create( + 0.0, + 0.0, + -100.0, + ) + assert result + assert drone_position is not None + + result, drone_orientation = drone_odometry_local.DroneOrientationLocal.create_new( + 0.0, + -np.pi / 2, + 0.0, + ) + assert result + assert drone_orientation is not None + + result, drone_odometry = drone_odometry_local.DroneOdometryLocal.create( + drone_position, + drone_orientation, + ) + assert result + assert drone_odometry is not None + + result, merged_detections = merged_odometry_detections.MergedOdometryDetections.create( + drone_odometry, + [ + detection_1, + detection_2, + ], + ) + assert result + assert merged_detections is not None + + result, expected_detection_1 = detection_in_world.DetectionInWorld.create( + # fmt: off + np.array( + [ + [ 100.0, -100.0], + [ 100.0, 100.0], + [-100.0, -100.0], + [-100.0, 100.0], + ], + dtype=np.float32, + ), + # fmt: on + np.array( + [0.0, 0.0], + dtype=np.float32, + ), + 1, + 1.0, + ) + assert result + assert expected_detection_1 is not None + + result, expected_detection_2 = detection_in_world.DetectionInWorld.create( + # fmt: off + np.array( + [ + [ 100.0, -100.0], + [ 100.0, 0.0], + [ 0.0, -100.0], + [ 0.0, 0.0], + ], + dtype=np.float32, + ), + # fmt: on + np.array( + [50.0, -50.0], + dtype=np.float32, + ), + 2, + 0.5, + ) + assert result + assert expected_detection_2 is not None + + expected_list = [ + expected_detection_1, + expected_detection_2, + ] + + # Run + result, actual_list = basic_locator.run(merged_detections) + + # Test + assert result + assert actual_list is not None + + assert len(actual_list) == len(expected_list) + for i, actual in enumerate(actual_list): + np.testing.assert_almost_equal(actual.vertices, expected_list[i].vertices) + np.testing.assert_almost_equal(actual.centre, expected_list[i].centre) + assert actual.label == expected_list[i].label + np.testing.assert_almost_equal(actual.confidence, expected_list[i].confidence) + + def test_advanced( + self, + advanced_locator: geolocation.Geolocation, + detection_bottom_right_point: detections_and_time.Detection, + detection_centre_left_point: detections_and_time.Detection, + ) -> None: + """ + 2 point detections. + """ + # Setup + result, drone_position = drone_odometry_local.DronePositionLocal.create( + 10.0 - np.cos(-np.pi / 12), # Camera at 10m north, drone at relative + 0.0, + -100.0 - np.sin(-np.pi / 12), # Camera at 100m above ground + ) + assert result + assert drone_position is not None + + result, drone_orientation = drone_odometry_local.DroneOrientationLocal.create_new( + 0.0, + np.pi / 12, + -np.pi, + ) + assert result + assert drone_orientation is not None + + result, drone_odometry = drone_odometry_local.DroneOdometryLocal.create( + drone_position, + drone_orientation, + ) + assert result + assert drone_odometry is not None + + result, merged_detections = merged_odometry_detections.MergedOdometryDetections.create( + drone_odometry, + [ + detection_bottom_right_point, + detection_centre_left_point, + ], + ) + assert result + assert merged_detections is not None + + result, expected_bottom_right = detection_in_world.DetectionInWorld.create( + # fmt: off + np.array( + [ + [10.0 + 100.0 * np.sqrt(3), 100.0], + [10.0 + 100.0 * np.sqrt(3), 100.0], + [10.0 + 100.0 * np.sqrt(3), 100.0], + [10.0 + 100.0 * np.sqrt(3), 100.0], + ], + dtype=np.float32, + ), + # fmt: on + np.array( + [10.0 + 100.0 * np.sqrt(3), 100.0], + dtype=np.float32, + ), + 0, + 0.01, + ) + assert result + assert expected_bottom_right is not None + + result, expected_centre_left = detection_in_world.DetectionInWorld.create( + # fmt: off + np.array( + [ + [10.0, 0.0], + [10.0, 0.0], + [10.0, 0.0], + [10.0, 0.0], + ], + dtype=np.float32, + ), + # fmt: on + np.array( + [10.0, 0.0], + dtype=np.float32, + ), + 0, + 0.1, + ) + assert result + assert expected_centre_left is not None + + expected_list = [ + expected_bottom_right, + expected_centre_left, + ] + + # Run + result, actual_list = advanced_locator.run(merged_detections) + + # Test + assert result + assert actual_list is not None + + assert len(actual_list) == len(expected_list) + for i, actual in enumerate(actual_list): + np.testing.assert_almost_equal( + actual.vertices, + expected_list[i].vertices, + FLOAT_PRECISION_TOLERANCE, + ) + np.testing.assert_almost_equal( + actual.centre, + expected_list[i].centre, + FLOAT_PRECISION_TOLERANCE, + ) + assert actual.label == expected_list[i].label + np.testing.assert_almost_equal(actual.confidence, expected_list[i].confidence) + + def test_bad_direction( + self, basic_locator: geolocation.Geolocation, detection_1: detections_and_time.Detection + ) -> None: + """ + Bad direction. + """ + # Setup + result, drone_position = drone_odometry_local.DronePositionLocal.create( + 0.0, + 0.0, + -100.0, + ) + assert result + assert drone_position is not None + + result, drone_orientation = drone_odometry_local.DroneOrientationLocal.create_new( + 0.0, + 0.0, + 0.0, + ) + assert result + assert drone_orientation is not None + + result, drone_odometry = drone_odometry_local.DroneOdometryLocal.create( + drone_position, + drone_orientation, + ) + assert result + assert drone_odometry is not None + + result, merged_detections = merged_odometry_detections.MergedOdometryDetections.create( + drone_odometry, + [detection_1], + ) + assert result + assert merged_detections is not None + + # Run + result, actual_list = basic_locator.run(merged_detections) + + # Test + assert not result + assert actual_list is None From 6d0202bcfda64e28fefb23f8535f3dcb26b1e5e1 Mon Sep 17 00:00:00 2001 From: asterbot Date: Sun, 3 Nov 2024 16:21:23 -0500 Subject: [PATCH 02/12] Rudimentary Geolocation Fixed imports and moved files in right folders --- modules/geolocation/geolocation.py | 2 +- .../geolocation/rudimentary_geolocation.py | 86 +++++++++++++++++++ modules/merged_odometry_detections.py | 2 +- tests/unit/test_geolocation.py | 4 +- tests/unit/test_rudimentary_geolocation.py | 72 ++++++++-------- 5 files changed, 126 insertions(+), 40 deletions(-) create mode 100644 modules/geolocation/rudimentary_geolocation.py diff --git a/modules/geolocation/geolocation.py b/modules/geolocation/geolocation.py index 4006d447..80115f27 100644 --- a/modules/geolocation/geolocation.py +++ b/modules/geolocation/geolocation.py @@ -9,7 +9,7 @@ from .. import detection_in_world from .. import detections_and_time from .. import merged_odometry_detections -from ..common.logger.modules import logger +from ..common.modules.logger import logger class Geolocation: diff --git a/modules/geolocation/rudimentary_geolocation.py b/modules/geolocation/rudimentary_geolocation.py new file mode 100644 index 00000000..e32ee67f --- /dev/null +++ b/modules/geolocation/rudimentary_geolocation.py @@ -0,0 +1,86 @@ +""" +Converts image space into world space. +""" + +import cv2 +import numpy as np + +from . import camera_properties +from .. import detection_in_world +from .. import detections_and_time +from .. import merged_odometry_detections +from ..common.modules.logger import logger + + +class RudimentaryGeolocation: + """ + Converts image space into world space. + """ + + __create_key = object() + + __MIN_DOWN_COS_ANGLE = 0.1 # radians, ~84° + + @classmethod + def create( + cls, + camera_intrinsics: camera_properties.CameraIntrinsics, + camera_drone_extrinsics: camera_properties.CameraDroneExtrinsics, + local_logger: logger.Logger, + ) -> "tuple[bool, RudimentaryGeolocation | None]": + """ + camera_intrinsics: Camera information without any outside space. + camera_drone_extrinsics: Camera information related to the drone without any world space. + """ + # Centre of each edge + # list[list[float]] required for OpenCV + perspective_transform_sources = [ + [camera_intrinsics.resolution_x / 2, 0], + [camera_intrinsics.resolution_x / 2, camera_intrinsics.resolution_y], + [0, camera_intrinsics.resolution_y / 2], + [camera_intrinsics.resolution_x, camera_intrinsics.resolution_y / 2], + ] + + # Orientation in world space + rotated_source_vectors = [] + for source in perspective_transform_sources: + # Image space to camera space + result, value = camera_intrinsics.camera_space_from_image_space(source[0], source[1]) + if not result: + local_logger.error( + f"Rotated source vector could not be created for source: {source}" + ) + return False, None + + # Get Pylance to stop complaining + assert value is not None + + # Camera space to world space (orientation only) + vec_rotated_source = camera_drone_extrinsics.camera_to_drone_rotation_matrix @ value + rotated_source_vectors.append(vec_rotated_source) + + return True, RudimentaryGeolocation( + cls.__create_key, + camera_drone_extrinsics, + perspective_transform_sources, + rotated_source_vectors, + local_logger, + ) + + def __init__( + self, + class_private_create_key: object, + camera_drone_extrinsics: camera_properties.CameraDroneExtrinsics, + perspective_transform_sources: "list[list[float]]", + rotated_source_vectors: "list[np.ndarray]", + local_logger: logger.Logger, + ) -> None: + """ + Private constructor, use create() method. + """ + assert class_private_create_key is RudimentaryGeolocation.__create_key, "Use create() method" + + self.__camera_drone_extrinsics = camera_drone_extrinsics + self.__perspective_transform_sources = perspective_transform_sources + self.__rotated_source_vectors = rotated_source_vectors + self.__logger = local_logger diff --git a/modules/merged_odometry_detections.py b/modules/merged_odometry_detections.py index bae95a1d..fb7e4385 100644 --- a/modules/merged_odometry_detections.py +++ b/modules/merged_odometry_detections.py @@ -3,7 +3,7 @@ """ from . import detections_and_time -from .common.mavlink.modules import drone_odometry_local +from .common.modules.mavlink import drone_odometry_local class MergedOdometryDetections: diff --git a/tests/unit/test_geolocation.py b/tests/unit/test_geolocation.py index d0e9d1e2..3b1c6fe6 100644 --- a/tests/unit/test_geolocation.py +++ b/tests/unit/test_geolocation.py @@ -8,8 +8,8 @@ from modules import detection_in_world from modules import detections_and_time from modules import merged_odometry_detections -from modules.common.logger.modules import logger -from modules.common.mavlink.modules import drone_odometry_local +from modules.common.modules.logger import logger +from modules.common.modules.mavlink import drone_odometry_local from modules.geolocation import camera_properties from modules.geolocation import geolocation diff --git a/tests/unit/test_rudimentary_geolocation.py b/tests/unit/test_rudimentary_geolocation.py index 809ed408..b0ea7482 100644 --- a/tests/unit/test_rudimentary_geolocation.py +++ b/tests/unit/test_rudimentary_geolocation.py @@ -1,5 +1,5 @@ """ -Test geolocation. +Test rudimentary_geolocation. """ import numpy as np @@ -8,10 +8,10 @@ from modules import detection_in_world from modules import detections_and_time from modules import merged_odometry_detections -from modules.common.modules import logger -from modules.common.modules import drone_odometry_local +from modules.common.modules.logger import logger +from modules.common.modules.mavlink import drone_odometry_local from modules.geolocation import camera_properties -from modules.geolocation import geolocation +from modules.geolocation import rudimentary_geolocation FLOAT_PRECISION_TOLERANCE = 4 @@ -22,7 +22,7 @@ @pytest.fixture -def basic_locator() -> geolocation.Geolocation: # type: ignore +def basic_locator() -> rudimentary_geolocation.RudimentaryGeolocation: # type: ignore """ Forwards pointing camera. """ @@ -46,7 +46,7 @@ def basic_locator() -> geolocation.Geolocation: # type: ignore assert result assert test_logger is not None - result, locator = geolocation.Geolocation.create( + result, locator = rudimentary_geolocation.RudimentaryGeolocation.create( camera_intrinsics, camera_extrinsics, test_logger, @@ -58,7 +58,7 @@ def basic_locator() -> geolocation.Geolocation: # type: ignore @pytest.fixture -def intermediate_locator() -> geolocation.Geolocation: # type: ignore +def intermediate_locator() -> rudimentary_geolocation.RudimentaryGeolocation: # type: ignore """ Downwards pointing camera offset towards front of drone. """ @@ -82,7 +82,7 @@ def intermediate_locator() -> geolocation.Geolocation: # type: ignore assert result assert test_logger is not None - result, locator = geolocation.Geolocation.create( + result, locator = rudimentary_geolocation.RudimentaryGeolocation.create( camera_intrinsics, camera_extrinsics, test_logger, @@ -94,7 +94,7 @@ def intermediate_locator() -> geolocation.Geolocation: # type: ignore @pytest.fixture -def advanced_locator() -> geolocation.Geolocation: # type: ignore +def advanced_locator() -> rudimentary_geolocation.RudimentaryGeolocation: # type: ignore """ Camera angled at 75° upward. Drone is expected to rotate it downwards. @@ -119,7 +119,7 @@ def advanced_locator() -> geolocation.Geolocation: # type: ignore assert result assert test_logger is not None - result, locator = geolocation.Geolocation.create( + result, locator = rudimentary_geolocation.RudimentaryGeolocation.create( camera_intrinsics, camera_extrinsics, test_logger, @@ -232,7 +232,7 @@ def non_affine_matrix() -> np.ndarray: # type: ignore yield matrix # type: ignore -class TestGeolocationCreate: +class TestRudimentaryGeolocationCreate: """ Test constructor. """ @@ -261,7 +261,7 @@ def test_normal(self) -> None: assert result assert test_logger is not None - result, locator = geolocation.Geolocation.create( + result, locator = rudimentary_geolocation.RudimentaryGeolocation.create( camera_intrinsics, camera_extrinsics, test_logger, @@ -293,7 +293,7 @@ def test_above_origin_directly_down(self) -> None: ( result, actual, - ) = geolocation.Geolocation._Geolocation__ground_intersection_from_vector( # type: ignore + ) = rudimentary_geolocation.RudimentaryGeolocation._RudimentaryGeolocation__ground_intersection_from_vector( # type: ignore vec_camera_in_world_position, vec_down, test_logger, @@ -322,7 +322,7 @@ def test_non_origin_directly_down(self) -> None: ( result, actual, - ) = geolocation.Geolocation._Geolocation__ground_intersection_from_vector( # type: ignore + ) = rudimentary_geolocation.RudimentaryGeolocation._RudimentaryGeolocation__ground_intersection_from_vector( # type: ignore vec_camera_in_world_position, vec_down, test_logger, @@ -351,7 +351,7 @@ def test_above_origin_angled_down(self) -> None: ( result, actual, - ) = geolocation.Geolocation._Geolocation__ground_intersection_from_vector( # type: ignore + ) = rudimentary_geolocation.RudimentaryGeolocation._RudimentaryGeolocation__ground_intersection_from_vector( # type: ignore vec_camera_in_world_position, vec_down, test_logger, @@ -380,7 +380,7 @@ def test_non_origin_angled_down(self) -> None: ( result, actual, - ) = geolocation.Geolocation._Geolocation__ground_intersection_from_vector( # type: ignore + ) = rudimentary_geolocation.RudimentaryGeolocation._RudimentaryGeolocation__ground_intersection_from_vector( # type: ignore vec_camera_in_world_position, vec_down, test_logger, @@ -407,7 +407,7 @@ def test_bad_almost_horizontal(self) -> None: ( result, actual, - ) = geolocation.Geolocation._Geolocation__ground_intersection_from_vector( # type: ignore + ) = rudimentary_geolocation.RudimentaryGeolocation._RudimentaryGeolocation__ground_intersection_from_vector( # type: ignore vec_camera_in_world_position, vec_horizontal, test_logger, @@ -433,7 +433,7 @@ def test_bad_upwards(self) -> None: ( result, actual, - ) = geolocation.Geolocation._Geolocation__ground_intersection_from_vector( # type: ignore + ) = rudimentary_geolocation.RudimentaryGeolocation._RudimentaryGeolocation__ground_intersection_from_vector( # type: ignore vec_camera_in_world_position, vec_up, test_logger, @@ -459,7 +459,7 @@ def test_bad_underground(self) -> None: ( result, actual, - ) = geolocation.Geolocation._Geolocation__ground_intersection_from_vector( # type: ignore + ) = rudimentary_geolocation.RudimentaryGeolocation._RudimentaryGeolocation__ground_intersection_from_vector( # type: ignore vec_underground, vec_down, test_logger, @@ -475,7 +475,7 @@ class TestPerspectiveTransformMatrix: Test perspective transform creation. """ - def test_basic_above_origin_pointed_down(self, basic_locator: geolocation.Geolocation) -> None: + def test_basic_above_origin_pointed_down(self, basic_locator: rudimentary_geolocation.RudimentaryGeolocation) -> None: """ Above origin, directly down. """ @@ -496,7 +496,7 @@ def test_basic_above_origin_pointed_down(self, basic_locator: geolocation.Geoloc ( result, actual, - ) = basic_locator._Geolocation__get_perspective_transform_matrix( # type: ignore + ) = basic_locator._RudimentaryGeolocation__get_perspective_transform_matrix( # type: ignore drone_rotation_matrix, drone_position_ned, ) @@ -514,7 +514,7 @@ def test_basic_above_origin_pointed_down(self, basic_locator: geolocation.Geoloc ) def test_intermediate_above_origin_pointing_north( - self, intermediate_locator: geolocation.Geolocation + self, intermediate_locator: rudimentary_geolocation.RudimentaryGeolocation ) -> None: """ Positioned so that the camera is above the origin directly down (but the drone is not). @@ -536,7 +536,7 @@ def test_intermediate_above_origin_pointing_north( ( result, actual, - ) = intermediate_locator._Geolocation__get_perspective_transform_matrix( # type: ignore + ) = intermediate_locator._RudimentaryGeolocation__get_perspective_transform_matrix( # type: ignore drone_rotation_matrix, drone_position_ned, ) @@ -554,7 +554,7 @@ def test_intermediate_above_origin_pointing_north( ) def test_intermediate_above_origin_pointing_west( - self, intermediate_locator: geolocation.Geolocation + self, intermediate_locator: rudimentary_geolocation.RudimentaryGeolocation ) -> None: """ Positioned so that the camera is above the origin directly down (but the drone is not). @@ -576,7 +576,7 @@ def test_intermediate_above_origin_pointing_west( ( result, actual, - ) = intermediate_locator._Geolocation__get_perspective_transform_matrix( # type: ignore + ) = intermediate_locator._RudimentaryGeolocation__get_perspective_transform_matrix( # type: ignore drone_rotation_matrix, drone_position_ned, ) @@ -593,7 +593,7 @@ def test_intermediate_above_origin_pointing_west( decimal=FLOAT_PRECISION_TOLERANCE, ) - def test_advanced(self, advanced_locator: geolocation.Geolocation) -> None: + def test_advanced(self, advanced_locator: rudimentary_geolocation.RudimentaryGeolocation) -> None: """ Camera is north of origin with an angle from vertical. Also rotated. """ @@ -629,7 +629,7 @@ def test_advanced(self, advanced_locator: geolocation.Geolocation) -> None: ( result, actual, - ) = advanced_locator._Geolocation__get_perspective_transform_matrix( # type: ignore + ) = advanced_locator._RudimentaryGeolocation__get_perspective_transform_matrix( # type: ignore drone_rotation_matrix, drone_position_ned, ) @@ -654,7 +654,7 @@ def test_advanced(self, advanced_locator: geolocation.Geolocation) -> None: decimal=FLOAT_PRECISION_TOLERANCE, ) - def test_bad_direction(self, basic_locator: geolocation.Geolocation) -> None: + def test_bad_direction(self, basic_locator: rudimentary_geolocation.RudimentaryGeolocation) -> None: """ Camera pointing forward. """ @@ -680,7 +680,7 @@ def test_bad_direction(self, basic_locator: geolocation.Geolocation) -> None: ( result, actual, - ) = basic_locator._Geolocation__get_perspective_transform_matrix( # type: ignore + ) = basic_locator._RudimentaryGeolocation__get_perspective_transform_matrix( # type: ignore drone_rotation_matrix, drone_position_ned, ) @@ -690,7 +690,7 @@ def test_bad_direction(self, basic_locator: geolocation.Geolocation) -> None: assert actual is None -class TestGeolocationConvertDetection: +class TestRudimentaryGeolocationConvertDetection: """ Test extract and convert. """ @@ -732,7 +732,7 @@ def test_normal1( ( result, actual, - ) = geolocation.Geolocation._Geolocation__convert_detection_to_world_from_image( # type: ignore + ) = rudimentary_geolocation.RudimentaryGeolocation._RudimentaryGeolocation__convert_detection_to_world_from_image( # type: ignore detection_1, affine_matrix, test_logger, @@ -784,7 +784,7 @@ def test_normal2( ( result, actual, - ) = geolocation.Geolocation._Geolocation__convert_detection_to_world_from_image( # type: ignore + ) = rudimentary_geolocation.RudimentaryGeolocation._RudimentaryGeolocation__convert_detection_to_world_from_image( # type: ignore detection_2, affine_matrix, test_logger, @@ -800,14 +800,14 @@ def test_normal2( np.testing.assert_almost_equal(actual.confidence, expected.confidence) -class TestGeolocationRun: +class TestRudimentaryGeolocationRun: """ Run. """ def test_basic( self, - basic_locator: geolocation.Geolocation, + basic_locator: rudimentary_geolocation.RudimentaryGeolocation, detection_1: detections_and_time.Detection, detection_2: detections_and_time.Detection, ) -> None: @@ -913,7 +913,7 @@ def test_basic( def test_advanced( self, - advanced_locator: geolocation.Geolocation, + advanced_locator: rudimentary_geolocation.RudimentaryGeolocation, detection_bottom_right_point: detections_and_time.Detection, detection_centre_left_point: detections_and_time.Detection, ) -> None: @@ -1026,7 +1026,7 @@ def test_advanced( np.testing.assert_almost_equal(actual.confidence, expected_list[i].confidence) def test_bad_direction( - self, basic_locator: geolocation.Geolocation, detection_1: detections_and_time.Detection + self, basic_locator: rudimentary_geolocation.RudimentaryGeolocation, detection_1: detections_and_time.Detection ) -> None: """ Bad direction. From 3b6efbc769b8d7b2952336586df8a6bff2f7fbf4 Mon Sep 17 00:00:00 2001 From: asterbot Date: Sun, 3 Nov 2024 16:24:27 -0500 Subject: [PATCH 03/12] Removed unecessary folder --- modules/rudimentary_geolocation/__init__.py | 0 .../camera_properties.py | 267 ------------------ .../geolocation_worker.py | 64 ----- .../rudimentary_geolocation.py | 86 ------ 4 files changed, 417 deletions(-) delete mode 100644 modules/rudimentary_geolocation/__init__.py delete mode 100644 modules/rudimentary_geolocation/camera_properties.py delete mode 100644 modules/rudimentary_geolocation/geolocation_worker.py delete mode 100644 modules/rudimentary_geolocation/rudimentary_geolocation.py diff --git a/modules/rudimentary_geolocation/__init__.py b/modules/rudimentary_geolocation/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/modules/rudimentary_geolocation/camera_properties.py b/modules/rudimentary_geolocation/camera_properties.py deleted file mode 100644 index 76f93999..00000000 --- a/modules/rudimentary_geolocation/camera_properties.py +++ /dev/null @@ -1,267 +0,0 @@ -""" -Camera intrinsic and extrinsic properties. -""" - -import numpy as np - - -def is_vector_r3(vec: np.ndarray) -> bool: - """ - Checks if the numpy array is a vector in R^3 . - """ - return vec.shape == (3,) - - -def is_matrix_r3x3(matrix: np.ndarray) -> bool: - """ - Checks if the numpy array is a matrix in R^{3x3} . - """ - return matrix.shape == (3, 3) - - -def create_rotation_matrix_from_orientation( - yaw: float, pitch: float, roll: float -) -> "tuple[bool, np.ndarray | None]": - """ - Creates a rotation matrix from yaw pitch roll in NED system (x forward, y right, z down). - Specifically, intrinsic (Tait-Bryan) rotations in the zyx/3-2-1 order. - See: https://en.wikipedia.org/wiki/Rotation_matrix#General_rotations - - yaw, pitch, roll are in radians from -pi to pi . - """ - if yaw < -np.pi or yaw > np.pi: - return False, None - - if pitch < -np.pi or pitch > np.pi: - return False, None - - if roll < -np.pi or roll > np.pi: - return False, None - - # fmt: off - yaw_matrix = np.array( - [ - [np.cos(yaw), -np.sin(yaw), 0.0], - [np.sin(yaw), np.cos(yaw), 0.0], - [ 0.0, 0.0, 1.0], - ], - dtype=np.float32, - ) - # fmt: on - - # fmt: off - pitch_matrix = np.array( - [ - [ np.cos(pitch), 0.0, np.sin(pitch)], - [ 0.0, 1.0, 0.0], - [-np.sin(pitch), 0.0, np.cos(pitch)], - ], - dtype=np.float32, - ) - # fmt: on - - # fmt: off - roll_matrix = np.array( - [ - [1.0, 0.0, 0.0], - [0.0, np.cos(roll), -np.sin(roll)], - [0.0, np.sin(roll), np.cos(roll)], - ], - dtype=np.float32, - ) - # fmt: on - - rotation_matrix = yaw_matrix @ pitch_matrix @ roll_matrix - - return True, rotation_matrix - - -class CameraIntrinsics: - """ - Camera space information. - - Image follows xy system with top-left as origin, pixel coordinates - refer to their top-left corner. - """ - - __create_key = object() - - @classmethod - def create( - cls, resolution_x: int, resolution_y: int, fov_x: float, fov_y: float - ) -> "tuple[bool, CameraIntrinsics | None]": - """ - Resolution is in pixels. - Field of view in radians horizontally and vertically across the image (edge to edge). - """ - if resolution_x < 0: - return False, None - - if resolution_y < 0: - return False, None - - if fov_x <= 0.0 or fov_x >= np.pi: - return False, None - - if fov_y <= 0.0 or fov_y >= np.pi: - return False, None - - u_scalar = np.tan(fov_x / 2) - if not np.isfinite(u_scalar): - return False, None - - v_scalar = np.tan(fov_y / 2) - if not np.isfinite(v_scalar): - return False, None - - return True, CameraIntrinsics( - cls.__create_key, - resolution_x, - resolution_y, - u_scalar, - v_scalar, - ) - - def __init__( - self, - class_private_create_key: object, - resolution_x: int, - resolution_y: int, - u_scalar: float, - v_scalar: float, - ) -> None: - """ - Private constructor, use create() method. - """ - assert class_private_create_key is CameraIntrinsics.__create_key, "Use create() method" - - self.resolution_x = resolution_x - self.resolution_y = resolution_y - - self.__vec_c = np.array([1.0, 0.0, 0.0], dtype=np.float32) - self.__vec_u = np.array([0.0, u_scalar, 0.0], dtype=np.float32) - self.__vec_v = np.array([0.0, 0.0, v_scalar], dtype=np.float32) - - @staticmethod - def __pixel_vector_from_image_space( - pixel: int, resolution: int, vec_base: np.ndarray - ) -> "tuple[bool, np.ndarray | None]": - """ - Get u or v vector from pixel coordinate. - """ - if pixel < 0: - return False, None - - if resolution < 0: - return False, None - - if pixel > resolution: - return False, None - - if not is_vector_r3(vec_base): - return False, None - - # Scaling factor with translation: 2 * p / r - 1 == (2 * p - r) / r - # Codomain is from -1 to 1 inclusive - scaling = float(2 * pixel - resolution) / float(resolution) - - vec_pixel = scaling * vec_base - - return True, vec_pixel - - def camera_space_from_image_space( - self, pixel_x: int, pixel_y: int - ) -> "tuple[bool, np.ndarray | None]": - """ - Pixel in image space to vector in camera space. - """ - if pixel_x < 0: - return False, None - - if pixel_y < 0: - return False, None - - result, vec_pixel_u = self.__pixel_vector_from_image_space( - pixel_x, - self.resolution_x, - self.__vec_u, - ) - if not result: - return False, None - - result, vec_pixel_v = self.__pixel_vector_from_image_space( - pixel_y, - self.resolution_y, - self.__vec_v, - ) - if not result: - return False, None - - # Get Pylance to stop complaining - assert vec_pixel_u is not None - assert vec_pixel_v is not None - - vec_camera = self.__vec_c + vec_pixel_u + vec_pixel_v - - return True, vec_camera - - -class CameraDroneExtrinsics: - """ - Camera in relation to drone. - """ - - __create_key = object() - - @classmethod - def create( - cls, - camera_position_xyz: "tuple[float, float, float]", - camera_orientation_ypr: "tuple[float, float, float]", - ) -> "tuple[bool, CameraDroneExtrinsics | None]": - """ - camera_position_xyz: Camera position is x, y, z. - camera_orientation_ypr: Camera orientation is yaw, pitch, roll. - - Both are relative to drone in NED system (x forward, y right, z down). - Specifically, intrinsic (Tait-Bryan) rotations in the zyx/3-2-1 order. - """ - # Unpack parameters - camera_x, camera_y, camera_z = camera_position_xyz - camera_yaw, camera_pitch, camera_roll = camera_orientation_ypr - - vec_camera_on_drone_position = np.array([camera_x, camera_y, camera_z], dtype=np.float32) - - result, camera_to_drone_rotation_matrix = create_rotation_matrix_from_orientation( - camera_yaw, - camera_pitch, - camera_roll, - ) - if not result: - return False, None - - # Get Pylance to stop complaining - assert camera_to_drone_rotation_matrix is not None - - if not is_matrix_r3x3(camera_to_drone_rotation_matrix): - return False, None - - return True, CameraDroneExtrinsics( - cls.__create_key, - vec_camera_on_drone_position, - camera_to_drone_rotation_matrix, - ) - - def __init__( - self, - class_private_create_key: object, - vec_camera_on_drone_position: np.ndarray, - camera_to_drone_rotation_matrix: np.ndarray, - ) -> None: - """ - Private constructor, use create() method. - """ - assert class_private_create_key is CameraDroneExtrinsics.__create_key, "Use create() method" - - self.vec_camera_on_drone_position = vec_camera_on_drone_position - self.camera_to_drone_rotation_matrix = camera_to_drone_rotation_matrix diff --git a/modules/rudimentary_geolocation/geolocation_worker.py b/modules/rudimentary_geolocation/geolocation_worker.py deleted file mode 100644 index 4a7d2436..00000000 --- a/modules/rudimentary_geolocation/geolocation_worker.py +++ /dev/null @@ -1,64 +0,0 @@ -""" -Convert bounding box data into ground data. -""" - -import os -import pathlib - -from utilities.workers import queue_proxy_wrapper -from utilities.workers import worker_controller -from . import camera_properties -from . import geolocation -from ..common.modules import logger - - -def geolocation_worker( - camera_intrinsics: camera_properties.CameraIntrinsics, - camera_drone_extrinsics: camera_properties.CameraDroneExtrinsics, - input_queue: queue_proxy_wrapper.QueueProxyWrapper, - output_queue: queue_proxy_wrapper.QueueProxyWrapper, - controller: worker_controller.WorkerController, -) -> None: - """ - Worker process. - - input_queue and output_queue are data queues. - controller is how the main process communicates to this worker process. - """ - # TODO: Handle errors better - - worker_name = pathlib.Path(__file__).stem - process_id = os.getpid() - result, local_logger = logger.Logger.create(f"{worker_name}_{process_id}", True) - if not result: - print("ERROR: Worker failed to create logger") - return - - assert local_logger is not None - - local_logger.info("Logger initialized") - - result, locator = geolocation.Geolocation.create( - camera_intrinsics, - camera_drone_extrinsics, - local_logger, - ) - if not result: - local_logger.error("Worker failed to create class object") - return - - # Get Pylance to stop complaining - assert locator is not None - - while not controller.is_exit_requested(): - controller.check_pause() - - input_data = input_queue.queue.get() - if input_data is None: - break - - result, value = locator.run(input_data) - if not result: - continue - - output_queue.queue.put(value) diff --git a/modules/rudimentary_geolocation/rudimentary_geolocation.py b/modules/rudimentary_geolocation/rudimentary_geolocation.py deleted file mode 100644 index 656b6dd0..00000000 --- a/modules/rudimentary_geolocation/rudimentary_geolocation.py +++ /dev/null @@ -1,86 +0,0 @@ -""" -Converts image space into world space. -""" - -import cv2 -import numpy as np - -from . import camera_properties -from .. import detection_in_world -from .. import detections_and_time -from .. import merged_odometry_detections -from ..common.modules import logger - - -class RudimentaryGeolocation: - """ - Converts image space into world space. - """ - - __create_key = object() - - __MIN_DOWN_COS_ANGLE = 0.1 # radians, ~84° - - @classmethod - def create( - cls, - camera_intrinsics: camera_properties.CameraIntrinsics, - camera_drone_extrinsics: camera_properties.CameraDroneExtrinsics, - local_logger: logger.Logger, - ) -> "tuple[bool, RudimentaryGeolocation | None]": - """ - camera_intrinsics: Camera information without any outside space. - camera_drone_extrinsics: Camera information related to the drone without any world space. - """ - # Centre of each edge - # list[list[float]] required for OpenCV - perspective_transform_sources = [ - [camera_intrinsics.resolution_x / 2, 0], - [camera_intrinsics.resolution_x / 2, camera_intrinsics.resolution_y], - [0, camera_intrinsics.resolution_y / 2], - [camera_intrinsics.resolution_x, camera_intrinsics.resolution_y / 2], - ] - - # Orientation in world space - rotated_source_vectors = [] - for source in perspective_transform_sources: - # Image space to camera space - result, value = camera_intrinsics.camera_space_from_image_space(source[0], source[1]) - if not result: - local_logger.error( - f"Rotated source vector could not be created for source: {source}" - ) - return False, None - - # Get Pylance to stop complaining - assert value is not None - - # Camera space to world space (orientation only) - vec_rotated_source = camera_drone_extrinsics.camera_to_drone_rotation_matrix @ value - rotated_source_vectors.append(vec_rotated_source) - - return True, RudimentaryGeolocation( - cls.__create_key, - camera_drone_extrinsics, - perspective_transform_sources, - rotated_source_vectors, - local_logger, - ) - - def __init__( - self, - class_private_create_key: object, - camera_drone_extrinsics: camera_properties.CameraDroneExtrinsics, - perspective_transform_sources: "list[list[float]]", - rotated_source_vectors: "list[np.ndarray]", - local_logger: logger.Logger, - ) -> None: - """ - Private constructor, use create() method. - """ - assert class_private_create_key is RudimentaryGeolocation.__create_key, "Use create() method" - - self.__camera_drone_extrinsics = camera_drone_extrinsics - self.__perspective_transform_sources = perspective_transform_sources - self.__rotated_source_vectors = rotated_source_vectors - self.__logger = local_logger From d7e7bc665b69a5167b45b6dd8d3801db86141699 Mon Sep 17 00:00:00 2001 From: asterbot Date: Tue, 5 Nov 2024 21:04:44 -0500 Subject: [PATCH 04/12] Figured out correct matrix and found required unit tests. Need to make it more rudimentary --- .../geolocation/rudimentary_geolocation.py | 229 ++++++++++++++++++ tests/unit/test_rudimentary_geolocation.py | 158 ------------ 2 files changed, 229 insertions(+), 158 deletions(-) diff --git a/modules/geolocation/rudimentary_geolocation.py b/modules/geolocation/rudimentary_geolocation.py index e32ee67f..212e5b7b 100644 --- a/modules/geolocation/rudimentary_geolocation.py +++ b/modules/geolocation/rudimentary_geolocation.py @@ -84,3 +84,232 @@ def __init__( self.__perspective_transform_sources = perspective_transform_sources self.__rotated_source_vectors = rotated_source_vectors self.__logger = local_logger + + # TODO: We need to convert both these fucntions to work for our case + @staticmethod + def __ground_intersection_from_vector( + vec_camera_in_world_position: np.ndarray, + vec_down: np.ndarray, + local_logger: logger.Logger, + ) -> "tuple[bool, np.ndarray | None]": + """ + Get 2D coordinates of where the downwards pointing vector intersects the ground. + """ + if not camera_properties.is_vector_r3(vec_camera_in_world_position): + local_logger.error("Camera position in world space is not a vector in R3") + return False, None + + if not camera_properties.is_vector_r3(vec_down): + local_logger.error("Rotated source vector in world space is not a vector in R3") + return False, None + + # Check camera above ground + if vec_camera_in_world_position[2] > 0.0: + local_logger.error("Camera is underground") + return False, None + + # Ensure vector is pointing down by checking angle + # cos(angle) = a dot b / (||a|| * ||b||) + vec_z = np.array([0.0, 0.0, 1.0], dtype=np.float32) + cos_angle = np.dot(vec_down, vec_z) / np.linalg.norm(vec_down) + if cos_angle < RudimentaryGeolocation.__MIN_DOWN_COS_ANGLE: + local_logger.error( + f"Rotated source vector in world space is not pointing down, cos(angle) = {cos_angle}" + ) + return False, None + + # Find scalar multiple for the vector to touch the ground (z/3rd component is 0) + # Solve for s: o3 + s * d3 = 0 + scaling = -vec_camera_in_world_position[2] / vec_down[2] + if scaling < 0.0: + local_logger.error(f"Scaling value is negative, scaling = {scaling}") + return False, None + + vec_ground = vec_camera_in_world_position + scaling * vec_down + + return True, vec_ground[:2] + + def __get_perspective_transform_matrix( + self, drone_rotation_matrix: np.ndarray, drone_position_ned: np.ndarray + ) -> "tuple[bool, np.ndarray | None]": + """ + Calculates the destination points, then uses OpenCV to get the matrix. + """ + if not camera_properties.is_matrix_r3x3(drone_rotation_matrix): + self.__logger.error("Drone rotation matrix is not a 3 x 3 matrix") + return False, None + + if not camera_properties.is_vector_r3(drone_position_ned): + self.__logger.error("Drone position in local space is not a vector in R3") + return False, None + + # Get the vectors in world space + vec_downs = [] + for vector in self.__rotated_source_vectors: + vec_down = drone_rotation_matrix @ vector + vec_downs.append(vec_down) + + # Get the camera position in world space + vec_camera_position = ( + drone_position_ned + + drone_rotation_matrix @ self.__camera_drone_extrinsics.vec_camera_on_drone_position + ) + + # Find the points on the ground + ground_points = [] + for vec_down in vec_downs: + result, ground_point = self.__ground_intersection_from_vector( + vec_camera_position, + vec_down, + self.__logger, + ) + if not result: + return False, None + + ground_points.append(ground_point) + + # Get the image to ground mapping + src = np.array(self.__perspective_transform_sources, dtype=np.float32) + dst = np.array(ground_points, dtype=np.float32) + try: + matrix = cv2.getPerspectiveTransform( # type: ignore + src, + dst, + ) + # All exceptions must be caught and logged as early as possible + # pylint: disable-next=broad-exception-caught + except Exception as e: + self.__logger.error(f"Could not get perspective transform matrix: {e}") + return False, None + + return True, matrix + + @staticmethod + def __convert_detection_to_world_from_image( + detection: detections_and_time.Detection, + perspective_transform_matrix: np.ndarray, + local_logger: logger.Logger, + ) -> "tuple[bool, detection_in_world.DetectionInWorld | None]": + """ + Applies the transform matrix to the detection. + perspective_transform_matrix: Element in last row and column must be 1 . + """ + if not camera_properties.is_matrix_r3x3(perspective_transform_matrix): + local_logger.error("Perspective transform matrix is not a 3 x 3 matrix") + return False, None + + if not np.allclose(perspective_transform_matrix[2][2], 1.0): + local_logger.error( + "Perspective transform matrix bottom right element is not close to 1.0" + ) + return False, None + + centre = detection.get_centre() + top_left, top_right, bottom_left, bottom_right = detection.get_corners() + + input_centre = np.array([centre[0], centre[1], 1.0], dtype=np.float32) + # More efficient to multiply a matrix than looping over the points + # Transpose to columns from rows + input_vertices = np.array( + [ + [top_left[0], top_left[1], 1.0], + [top_right[0], top_right[1], 1.0], + [bottom_left[0], bottom_left[1], 1.0], + [bottom_right[0], bottom_right[1], 1.0], + ], + dtype=np.float32, + ).T + + # Single row/column does not need transpose + output_centre = perspective_transform_matrix @ input_centre + # Transpose back to rows from columns + output_vertices = (perspective_transform_matrix @ input_vertices).T + + # Normalize by last element + # Homogeneous/perspective divide: + # https://en.wikipedia.org/wiki/Transformation_matrix#Perspective_projection + ground_centre = np.array( + [ + output_centre[0] / output_centre[2], + output_centre[1] / output_centre[2], + ], + dtype=np.float32, + ) + + # Normalize each row by its last element + + # Slice to get the last element of each row + vec_last_element = output_vertices[:, 2] + + # Divide each row by vector element: + # https://www.w3resource.com/python-exercises/numpy/python-numpy-exercise-96.php + output_normalized = output_vertices / vec_last_element[:, None] + if not np.isfinite(output_normalized).all(): + local_logger.error("Normalized output is infinite") + return False, None + + # Slice to remove the last element of each row + ground_vertices = output_normalized[:, :2] + + result, detection_world = detection_in_world.DetectionInWorld.create( + ground_vertices, + ground_centre, + detection.label, + detection.confidence, + ) + if not result: + return False, None + + return True, detection_world + + def run( + self, detections: merged_odometry_detections.MergedOdometryDetections + ) -> "tuple[bool, list[detection_in_world.DetectionInWorld] | None]": + """ + Runs detections in world space + """ + if detections.odometry_local.position.down >= 0.0: + self.__logger.error("Drone is underground") + return False, None + + drone_position_ned = np.array( + [ + detections.odometry_local.position.north, + detections.odometry_local.position.east, + detections.odometry_local.position.down, + ], + dtype=np.float32, + ) + + # Since camera points down, the rotation matrix will be + # Calculated assuming pitch=-pi/2 and yaw=roll=0 + drone_rotation_matrix = np.array( + [ + [0.0, 0.0,-1.0], + [0.0, 1.0, 0.0], + [1.0, 0.0, 0.0], + ], + dtype=np.float32, + ) + + result, perspective_transform_matrix = self.__get_perspective_transform_matrix( + drone_rotation_matrix, + drone_position_ned, + ) + if not result: + return False, None + + detections_in_world = [] + for detection in detections.detections: + result, detection_world = self.__convert_detection_to_world_from_image( + detection, + perspective_transform_matrix, + self.__logger, + ) + # Partial data not allowed + if not result: + return False, None + detections_in_world.append(detection_world) + self.__logger.info(detection_world) + + return True, detections_in_world diff --git a/tests/unit/test_rudimentary_geolocation.py b/tests/unit/test_rudimentary_geolocation.py index b0ea7482..d5058149 100644 --- a/tests/unit/test_rudimentary_geolocation.py +++ b/tests/unit/test_rudimentary_geolocation.py @@ -910,161 +910,3 @@ def test_basic( np.testing.assert_almost_equal(actual.centre, expected_list[i].centre) assert actual.label == expected_list[i].label np.testing.assert_almost_equal(actual.confidence, expected_list[i].confidence) - - def test_advanced( - self, - advanced_locator: rudimentary_geolocation.RudimentaryGeolocation, - detection_bottom_right_point: detections_and_time.Detection, - detection_centre_left_point: detections_and_time.Detection, - ) -> None: - """ - 2 point detections. - """ - # Setup - result, drone_position = drone_odometry_local.DronePositionLocal.create( - 10.0 - np.cos(-np.pi / 12), # Camera at 10m north, drone at relative - 0.0, - -100.0 - np.sin(-np.pi / 12), # Camera at 100m above ground - ) - assert result - assert drone_position is not None - - result, drone_orientation = drone_odometry_local.DroneOrientationLocal.create_new( - 0.0, - np.pi / 12, - -np.pi, - ) - assert result - assert drone_orientation is not None - - result, drone_odometry = drone_odometry_local.DroneOdometryLocal.create( - drone_position, - drone_orientation, - ) - assert result - assert drone_odometry is not None - - result, merged_detections = merged_odometry_detections.MergedOdometryDetections.create( - drone_odometry, - [ - detection_bottom_right_point, - detection_centre_left_point, - ], - ) - assert result - assert merged_detections is not None - - result, expected_bottom_right = detection_in_world.DetectionInWorld.create( - # fmt: off - np.array( - [ - [10.0 + 100.0 * np.sqrt(3), 100.0], - [10.0 + 100.0 * np.sqrt(3), 100.0], - [10.0 + 100.0 * np.sqrt(3), 100.0], - [10.0 + 100.0 * np.sqrt(3), 100.0], - ], - dtype=np.float32, - ), - # fmt: on - np.array( - [10.0 + 100.0 * np.sqrt(3), 100.0], - dtype=np.float32, - ), - 0, - 0.01, - ) - assert result - assert expected_bottom_right is not None - - result, expected_centre_left = detection_in_world.DetectionInWorld.create( - # fmt: off - np.array( - [ - [10.0, 0.0], - [10.0, 0.0], - [10.0, 0.0], - [10.0, 0.0], - ], - dtype=np.float32, - ), - # fmt: on - np.array( - [10.0, 0.0], - dtype=np.float32, - ), - 0, - 0.1, - ) - assert result - assert expected_centre_left is not None - - expected_list = [ - expected_bottom_right, - expected_centre_left, - ] - - # Run - result, actual_list = advanced_locator.run(merged_detections) - - # Test - assert result - assert actual_list is not None - - assert len(actual_list) == len(expected_list) - for i, actual in enumerate(actual_list): - np.testing.assert_almost_equal( - actual.vertices, - expected_list[i].vertices, - FLOAT_PRECISION_TOLERANCE, - ) - np.testing.assert_almost_equal( - actual.centre, - expected_list[i].centre, - FLOAT_PRECISION_TOLERANCE, - ) - assert actual.label == expected_list[i].label - np.testing.assert_almost_equal(actual.confidence, expected_list[i].confidence) - - def test_bad_direction( - self, basic_locator: rudimentary_geolocation.RudimentaryGeolocation, detection_1: detections_and_time.Detection - ) -> None: - """ - Bad direction. - """ - # Setup - result, drone_position = drone_odometry_local.DronePositionLocal.create( - 0.0, - 0.0, - -100.0, - ) - assert result - assert drone_position is not None - - result, drone_orientation = drone_odometry_local.DroneOrientationLocal.create_new( - 0.0, - 0.0, - 0.0, - ) - assert result - assert drone_orientation is not None - - result, drone_odometry = drone_odometry_local.DroneOdometryLocal.create( - drone_position, - drone_orientation, - ) - assert result - assert drone_odometry is not None - - result, merged_detections = merged_odometry_detections.MergedOdometryDetections.create( - drone_odometry, - [detection_1], - ) - assert result - assert merged_detections is not None - - # Run - result, actual_list = basic_locator.run(merged_detections) - - # Test - assert not result - assert actual_list is None From 817e7a32e90b0c75d12040b5428eedec4f330732 Mon Sep 17 00:00:00 2001 From: asterbot Date: Tue, 5 Nov 2024 21:17:44 -0500 Subject: [PATCH 05/12] rebase with main --- documentation/main_multiprocess_example.py | 8 ++-- .../add_random/add_random.py | 2 +- .../add_random/add_random_worker.py | 2 +- .../concatenator/concatenator.py | 2 +- .../concatenator/concatenator_worker.py | 2 +- .../multiprocess_example/countup/countup.py | 2 +- .../countup/countup_worker.py | 2 +- main_2024.py | 8 ++-- modules/data_merge/data_merge_worker.py | 2 +- modules/detect_target/detect_target_worker.py | 2 +- modules/flight_interface/flight_interface.py | 46 +++++++++---------- .../flight_interface_worker.py | 2 +- modules/geolocation/geolocation.py | 6 +-- modules/geolocation/geolocation_worker.py | 2 +- modules/odometry_and_time.py | 2 +- modules/video_input/video_input.py | 2 +- tests/integration/test_data_merge_worker.py | 16 ++++--- .../test_flight_interface_hardware.py | 2 +- tests/integration/test_geolocation_worker.py | 8 ++-- tests/test_search_pattern.py | 12 +++-- tests/unit/test_decision.py | 16 ++++--- tests/unit/test_geolocation.py | 17 ++++--- utilities/workers/worker_manager.py | 2 +- 23 files changed, 91 insertions(+), 74 deletions(-) diff --git a/documentation/main_multiprocess_example.py b/documentation/main_multiprocess_example.py index 6efa155d..8280cb1b 100644 --- a/documentation/main_multiprocess_example.py +++ b/documentation/main_multiprocess_example.py @@ -11,9 +11,9 @@ from documentation.multiprocess_example.add_random import add_random_worker from documentation.multiprocess_example.concatenator import concatenator_worker from documentation.multiprocess_example.countup import countup_worker -from modules.common.logger.modules import logger -from modules.common.logger.modules import logger_setup_main -from modules.common.logger.read_yaml.modules import read_yaml +from modules.common.modules.logger import logger +from modules.common.modules.logger import logger_main_setup +from modules.common.modules.read_yaml import read_yaml from utilities.workers import queue_proxy_wrapper from utilities.workers import worker_controller from utilities.workers import worker_manager @@ -44,7 +44,7 @@ def main() -> int: assert config is not None # Setup main logger - result, main_logger, _ = logger_setup_main.setup_main_logger(config) + result, main_logger, _ = logger_main_setup.setup_main_logger(config) if not result: print("ERROR: Failed to create main logger") return -1 diff --git a/documentation/multiprocess_example/add_random/add_random.py b/documentation/multiprocess_example/add_random/add_random.py index b6f16718..e3cfee62 100644 --- a/documentation/multiprocess_example/add_random/add_random.py +++ b/documentation/multiprocess_example/add_random/add_random.py @@ -5,7 +5,7 @@ import time import random -from modules.common.logger.modules import logger +from modules.common.modules.logger import logger from .. import intermediate_struct diff --git a/documentation/multiprocess_example/add_random/add_random_worker.py b/documentation/multiprocess_example/add_random/add_random_worker.py index 190ff640..8c610d36 100644 --- a/documentation/multiprocess_example/add_random/add_random_worker.py +++ b/documentation/multiprocess_example/add_random/add_random_worker.py @@ -5,7 +5,7 @@ import os import pathlib -from modules.common.logger.modules import logger +from modules.common.modules.logger import logger from utilities.workers import queue_proxy_wrapper from utilities.workers import worker_controller from . import add_random diff --git a/documentation/multiprocess_example/concatenator/concatenator.py b/documentation/multiprocess_example/concatenator/concatenator.py index df9f3a8a..f6460128 100644 --- a/documentation/multiprocess_example/concatenator/concatenator.py +++ b/documentation/multiprocess_example/concatenator/concatenator.py @@ -4,7 +4,7 @@ import time -from modules.common.logger.modules import logger +from modules.common.modules.logger import logger from .. import intermediate_struct diff --git a/documentation/multiprocess_example/concatenator/concatenator_worker.py b/documentation/multiprocess_example/concatenator/concatenator_worker.py index aa821a4d..7a436e97 100644 --- a/documentation/multiprocess_example/concatenator/concatenator_worker.py +++ b/documentation/multiprocess_example/concatenator/concatenator_worker.py @@ -5,7 +5,7 @@ import os import pathlib -from modules.common.logger.modules import logger +from modules.common.modules.logger import logger from utilities.workers import queue_proxy_wrapper from utilities.workers import worker_controller from . import concatenator diff --git a/documentation/multiprocess_example/countup/countup.py b/documentation/multiprocess_example/countup/countup.py index 4ddb7ca1..bc978bad 100644 --- a/documentation/multiprocess_example/countup/countup.py +++ b/documentation/multiprocess_example/countup/countup.py @@ -4,7 +4,7 @@ import time -from modules.common.logger.modules import logger +from modules.common.modules.logger import logger class Countup: diff --git a/documentation/multiprocess_example/countup/countup_worker.py b/documentation/multiprocess_example/countup/countup_worker.py index 7960e1d7..5718d311 100644 --- a/documentation/multiprocess_example/countup/countup_worker.py +++ b/documentation/multiprocess_example/countup/countup_worker.py @@ -5,7 +5,7 @@ import os import pathlib -from modules.common.logger.modules import logger +from modules.common.modules.logger import logger from utilities.workers import queue_proxy_wrapper from utilities.workers import worker_controller from . import countup diff --git a/main_2024.py b/main_2024.py index 66ad913d..f6297226 100644 --- a/main_2024.py +++ b/main_2024.py @@ -19,9 +19,9 @@ from modules.data_merge import data_merge_worker from modules.geolocation import geolocation_worker from modules.geolocation import camera_properties -from modules.common.logger.modules import logger -from modules.common.logger.modules import logger_setup_main -from modules.common.logger.read_yaml.modules import read_yaml +from modules.common.modules.logger import logger +from modules.common.modules.logger import logger_main_setup +from modules.common.modules.read_yaml import read_yaml from utilities.workers import queue_proxy_wrapper from utilities.workers import worker_controller from utilities.workers import worker_manager @@ -64,7 +64,7 @@ def main() -> int: assert config_logger is not None # Setup main logger - result, main_logger, logging_path = logger_setup_main.setup_main_logger(config_logger) + result, main_logger, logging_path = logger_main_setup.setup_main_logger(config_logger) if not result: print("ERROR: Failed to create main logger") return -1 diff --git a/modules/data_merge/data_merge_worker.py b/modules/data_merge/data_merge_worker.py index d3357dc5..989ed6e7 100644 --- a/modules/data_merge/data_merge_worker.py +++ b/modules/data_merge/data_merge_worker.py @@ -11,7 +11,7 @@ from .. import detections_and_time from .. import merged_odometry_detections from .. import odometry_and_time -from ..common.logger.modules import logger +from ..common.modules.logger import logger def data_merge_worker( diff --git a/modules/detect_target/detect_target_worker.py b/modules/detect_target/detect_target_worker.py index 7c1efe56..7d03b9fa 100644 --- a/modules/detect_target/detect_target_worker.py +++ b/modules/detect_target/detect_target_worker.py @@ -8,7 +8,7 @@ from utilities.workers import queue_proxy_wrapper from utilities.workers import worker_controller from . import detect_target_factory -from ..common.logger.modules import logger +from ..common.modules.logger import logger def detect_target_worker( diff --git a/modules/flight_interface/flight_interface.py b/modules/flight_interface/flight_interface.py index 9786f413..e1b47d86 100644 --- a/modules/flight_interface/flight_interface.py +++ b/modules/flight_interface/flight_interface.py @@ -4,11 +4,11 @@ from .. import decision_command from .. import odometry_and_time -from ..common.logger.modules import logger -from ..common.mavlink.modules import drone_odometry -from ..common.mavlink.modules import drone_odometry_local -from ..common.mavlink.modules import flight_controller -from ..common.mavlink.modules import local_global_conversion +from ..common.modules.logger import logger +from ..common.modules import position_global +from ..common.modules import position_local +from ..common.modules.mavlink import flight_controller +from ..common.modules.mavlink import local_global_conversion class FlightInterface: @@ -39,21 +39,23 @@ def create( # Get Pylance to stop complaining assert controller is not None - result, home_location = controller.get_home_location(timeout_home) + result, home_position = controller.get_home_position(timeout_home) if not result: - local_logger.error("home_location could not be created", True) + local_logger.error("home_position could not be created", True) return False, None # Get Pylance to stop complaining - assert home_location is not None + assert home_position is not None - return True, FlightInterface(cls.__create_key, controller, home_location, local_logger) + local_logger.info(str(home_position), True) + + return True, FlightInterface(cls.__create_key, controller, home_position, local_logger) def __init__( self, class_private_create_key: object, controller: flight_controller.FlightController, - home_location: drone_odometry.DronePosition, + home_position: position_global.PositionGlobal, local_logger: logger.Logger, ) -> None: """ @@ -62,11 +64,9 @@ def __init__( assert class_private_create_key is FlightInterface.__create_key, "Use create() method" self.controller = controller - self.__home_location = home_location + self.__home_position = home_position self.__logger = local_logger - self.__logger.info(str(self.__home_location), True) - def run(self) -> "tuple[bool, odometry_and_time.OdometryAndTime | None]": """ Returns a possible OdometryAndTime with current timestamp. @@ -79,8 +79,8 @@ def run(self) -> "tuple[bool, odometry_and_time.OdometryAndTime | None]": assert odometry is not None result, odometry_local = local_global_conversion.drone_odometry_local_from_global( + self.__home_position, odometry, - self.__home_location, ) if not result: return False, None @@ -117,7 +117,7 @@ def apply_decision(self, cmd: decision_command.DecisionCommand) -> bool: # Convert current global position to local NED coordinates. result, current_local_odometry = ( local_global_conversion.drone_odometry_local_from_global( - current_odometry, self.__home_location + self.__home_position, current_odometry ) ) if not result or current_local_odometry is None: @@ -128,15 +128,15 @@ def apply_decision(self, cmd: decision_command.DecisionCommand) -> bool: target_east = current_local_odometry.position.east + command_position[1] target_down = current_local_odometry.position.down + command_position[2] - result, target_local_position = drone_odometry_local.DronePositionLocal.create( + result, target_local_position = position_local.PositionLocal.create( target_north, target_east, target_down ) if not result or target_local_position is None: return False result, target_global_position = ( - local_global_conversion.drone_position_global_from_local( - self.__home_location, target_local_position + local_global_conversion.position_global_from_position_local( + self.__home_position, target_local_position ) ) if not result or target_global_position is None: @@ -148,7 +148,7 @@ def apply_decision(self, cmd: decision_command.DecisionCommand) -> bool: if command_type == decision_command.DecisionCommand.CommandType.MOVE_TO_ABSOLUTE_POSITION: # Move to absolute position. # Note that command_position[2] is the absolute altitude not relative altitude. - result, target_position = drone_odometry.DronePosition.create( + result, target_position = position_global.PositionGlobal.create( command_position[0], command_position[1], command_position[2] ) @@ -171,7 +171,7 @@ def apply_decision(self, cmd: decision_command.DecisionCommand) -> bool: # Convert current global position to local NED coordinates result, current_local_odometry = ( local_global_conversion.drone_odometry_local_from_global( - current_odometry, self.__home_location + self.__home_position, current_odometry ) ) if not result or current_local_odometry is None: @@ -183,15 +183,15 @@ def apply_decision(self, cmd: decision_command.DecisionCommand) -> bool: target_down = current_local_odometry.position.down + command_position[2] # Create target local position. - result, target_local_position = drone_odometry_local.DronePositionLocal.create( + result, target_local_position = position_local.PositionLocal.create( target_north, target_east, target_down ) if not result or target_local_position is None: return False # Convert target local position to global position. result, target_global_position = ( - local_global_conversion.drone_position_global_from_local( - self.__home_location, target_local_position + local_global_conversion.position_global_from_position_local( + self.__home_position, target_local_position ) ) if not result or target_global_position is None: diff --git a/modules/flight_interface/flight_interface_worker.py b/modules/flight_interface/flight_interface_worker.py index 0c81dab9..2ead7bae 100644 --- a/modules/flight_interface/flight_interface_worker.py +++ b/modules/flight_interface/flight_interface_worker.py @@ -9,7 +9,7 @@ from utilities.workers import queue_proxy_wrapper from utilities.workers import worker_controller from . import flight_interface -from ..common.logger.modules import logger +from ..common.modules.logger import logger def flight_interface_worker( diff --git a/modules/geolocation/geolocation.py b/modules/geolocation/geolocation.py index 80115f27..63641eb5 100644 --- a/modules/geolocation/geolocation.py +++ b/modules/geolocation/geolocation.py @@ -285,9 +285,9 @@ def run( # Generate projective perspective matrix # Camera rotation in world result, drone_rotation_matrix = camera_properties.create_rotation_matrix_from_orientation( - detections.odometry_local.orientation.orientation.yaw, - detections.odometry_local.orientation.orientation.pitch, - detections.odometry_local.orientation.orientation.roll, + detections.odometry_local.orientation.yaw, + detections.odometry_local.orientation.pitch, + detections.odometry_local.orientation.roll, ) if not result: self.__logger.error("Drone rotation matrix could not be created") diff --git a/modules/geolocation/geolocation_worker.py b/modules/geolocation/geolocation_worker.py index ac040a95..11781688 100644 --- a/modules/geolocation/geolocation_worker.py +++ b/modules/geolocation/geolocation_worker.py @@ -9,7 +9,7 @@ from utilities.workers import worker_controller from . import camera_properties from . import geolocation -from ..common.logger.modules import logger +from ..common.modules.logger import logger def geolocation_worker( diff --git a/modules/odometry_and_time.py b/modules/odometry_and_time.py index 9494b864..6ea03766 100644 --- a/modules/odometry_and_time.py +++ b/modules/odometry_and_time.py @@ -4,7 +4,7 @@ import time -from .common.mavlink.modules import drone_odometry_local +from .common.modules.mavlink import drone_odometry_local class OdometryAndTime: diff --git a/modules/video_input/video_input.py b/modules/video_input/video_input.py index 28ebe114..faaef9ac 100644 --- a/modules/video_input/video_input.py +++ b/modules/video_input/video_input.py @@ -2,8 +2,8 @@ Combines image and timestamp together. """ -from ..common.camera.modules import camera_device from .. import image_and_time +from ..common.modules.camera import camera_device class VideoInput: diff --git a/tests/integration/test_data_merge_worker.py b/tests/integration/test_data_merge_worker.py index 2f7b705b..bdcf6b7e 100644 --- a/tests/integration/test_data_merge_worker.py +++ b/tests/integration/test_data_merge_worker.py @@ -10,7 +10,9 @@ from modules import detections_and_time from modules import merged_odometry_detections from modules import odometry_and_time -from modules.common.mavlink.modules import drone_odometry_local +from modules.common.modules import orientation +from modules.common.modules import position_local +from modules.common.modules.mavlink import drone_odometry_local from modules.data_merge import data_merge_worker from utilities.workers import queue_proxy_wrapper from utilities.workers import worker_controller @@ -44,15 +46,17 @@ def simulate_flight_input_worker( Place the odometry into the queue. """ # Timestamp is stored in latitude - result, position = drone_odometry_local.DronePositionLocal.create(timestamp, 0.0, -1.0) + result, drone_position = position_local.PositionLocal.create(timestamp, 0.0, -1.0) assert result - assert position is not None + assert drone_position is not None - result, orientation = drone_odometry_local.DroneOrientationLocal.create_new(0.0, 0.0, 0.0) + result, drone_orientation = orientation.Orientation.create(0.0, 0.0, 0.0) assert result - assert orientation is not None + assert drone_orientation is not None - result, odometry = drone_odometry_local.DroneOdometryLocal.create(position, orientation) + result, odometry = drone_odometry_local.DroneOdometryLocal.create( + drone_position, drone_orientation + ) assert result assert odometry is not None diff --git a/tests/integration/test_flight_interface_hardware.py b/tests/integration/test_flight_interface_hardware.py index e38c0cba..e0050441 100644 --- a/tests/integration/test_flight_interface_hardware.py +++ b/tests/integration/test_flight_interface_hardware.py @@ -5,7 +5,7 @@ import pathlib from modules.flight_interface import flight_interface -from modules.common.logger.modules import logger +from modules.common.modules.logger import logger MAVLINK_CONNECTION_ADDRESS = "tcp:localhost:14550" diff --git a/tests/integration/test_geolocation_worker.py b/tests/integration/test_geolocation_worker.py index e6f59236..db984c99 100644 --- a/tests/integration/test_geolocation_worker.py +++ b/tests/integration/test_geolocation_worker.py @@ -9,7 +9,9 @@ from modules import detections_and_time from modules import merged_odometry_detections -from modules.common.mavlink.modules import drone_odometry_local +from modules.common.modules import orientation +from modules.common.modules import position_local +from modules.common.modules.mavlink import drone_odometry_local from modules.geolocation import camera_properties from modules.geolocation import geolocation_worker from utilities.workers import queue_proxy_wrapper @@ -22,7 +24,7 @@ def simulate_previous_worker(in_queue: queue_proxy_wrapper.QueueProxyWrapper) -> """ Place the image into the queue. """ - result_simulate, drone_position = drone_odometry_local.DronePositionLocal.create( + result_simulate, drone_position = position_local.PositionLocal.create( 0.0, 0.0, -100.0, @@ -30,7 +32,7 @@ def simulate_previous_worker(in_queue: queue_proxy_wrapper.QueueProxyWrapper) -> assert result_simulate assert drone_position is not None - result_simulate, drone_orientation = drone_odometry_local.DroneOrientationLocal.create_new( + result_simulate, drone_orientation = orientation.Orientation.create( 0.0, -np.pi / 2, 0.0, diff --git a/tests/test_search_pattern.py b/tests/test_search_pattern.py index 1550a248..e78cba83 100644 --- a/tests/test_search_pattern.py +++ b/tests/test_search_pattern.py @@ -7,7 +7,9 @@ from modules import decision_command from modules import odometry_and_time -from modules.common.mavlink.modules import drone_odometry_local +from modules.common.modules import orientation +from modules.common.modules import position_local +from modules.common.modules.mavlink import drone_odometry_local from modules.decision import search_pattern DISTANCE_SQUARED_THRESHOLD = 2.0 @@ -65,9 +67,11 @@ def create_drone_odometry( """ Creates an OdometryAndTime instance representing the drone's current position. """ - position = drone_odometry_local.DronePositionLocal.create(pos_y, pos_x, -depth)[1] - orientation = drone_odometry_local.DroneOrientationLocal.create_new(0.0, 0.0, 0.0)[1] - odometry_data = drone_odometry_local.DroneOdometryLocal.create(position, orientation)[1] + drone_position = position_local.PositionLocal.create(pos_y, pos_x, -depth)[1] + drone_orientation = orientation.Orientation.create(0.0, 0.0, 0.0)[1] + odometry_data = drone_odometry_local.DroneOdometryLocal.create( + drone_position, drone_orientation + )[1] odometry = odometry_and_time.OdometryAndTime.create(odometry_data)[1] return odometry diff --git a/tests/unit/test_decision.py b/tests/unit/test_decision.py index 3bd5561f..3b3cf68f 100644 --- a/tests/unit/test_decision.py +++ b/tests/unit/test_decision.py @@ -8,7 +8,9 @@ from modules import decision_command from modules import object_in_world from modules import odometry_and_time -from modules.common.mavlink.modules import drone_odometry_local +from modules.common.modules import orientation +from modules.common.modules import position_local +from modules.common.modules.mavlink import drone_odometry_local LANDING_PAD_LOCATION_TOLERANCE = 2 @@ -89,19 +91,21 @@ def drone_odometry_and_time() -> odometry_and_time.OdometryAndTime: # type: ign Create an OdometryAndTime instance with the drone positioned within tolerance of landing pad. """ # Creating the position within tolerance of the specified landing pad. - result, position = drone_odometry_local.DronePositionLocal.create( + result, drone_position = position_local.PositionLocal.create( BEST_PAD_LOCATION_X - DRONE_OFFSET_FROM_PAD, BEST_PAD_LOCATION_Y - DRONE_OFFSET_FROM_PAD, -5.0, ) assert result - assert position is not None + assert drone_position is not None - result, orientation = drone_odometry_local.DroneOrientationLocal.create_new(0.0, 0.0, 0.0) + result, drone_orientation = orientation.Orientation.create(0.0, 0.0, 0.0) assert result - assert orientation is not None + assert drone_orientation is not None - result, odometry_data = drone_odometry_local.DroneOdometryLocal.create(position, orientation) + result, odometry_data = drone_odometry_local.DroneOdometryLocal.create( + drone_position, drone_orientation + ) assert result assert odometry_data is not None diff --git a/tests/unit/test_geolocation.py b/tests/unit/test_geolocation.py index 3b1c6fe6..b73f9e39 100644 --- a/tests/unit/test_geolocation.py +++ b/tests/unit/test_geolocation.py @@ -8,11 +8,14 @@ from modules import detection_in_world from modules import detections_and_time from modules import merged_odometry_detections +from modules.common.modules import orientation +from modules.common.modules import position_local from modules.common.modules.logger import logger from modules.common.modules.mavlink import drone_odometry_local from modules.geolocation import camera_properties from modules.geolocation import geolocation + FLOAT_PRECISION_TOLERANCE = 4 @@ -815,7 +818,7 @@ def test_basic( 2 detections. """ # Setup - result, drone_position = drone_odometry_local.DronePositionLocal.create( + result, drone_position = position_local.PositionLocal.create( 0.0, 0.0, -100.0, @@ -823,7 +826,7 @@ def test_basic( assert result assert drone_position is not None - result, drone_orientation = drone_odometry_local.DroneOrientationLocal.create_new( + result, drone_orientation = orientation.Orientation.create( 0.0, -np.pi / 2, 0.0, @@ -921,7 +924,7 @@ def test_advanced( 2 point detections. """ # Setup - result, drone_position = drone_odometry_local.DronePositionLocal.create( + result, drone_position = position_local.PositionLocal.create( 10.0 - np.cos(-np.pi / 12), # Camera at 10m north, drone at relative 0.0, -100.0 - np.sin(-np.pi / 12), # Camera at 100m above ground @@ -929,7 +932,7 @@ def test_advanced( assert result assert drone_position is not None - result, drone_orientation = drone_odometry_local.DroneOrientationLocal.create_new( + result, drone_orientation = orientation.Orientation.create( 0.0, np.pi / 12, -np.pi, @@ -1032,7 +1035,7 @@ def test_bad_direction( Bad direction. """ # Setup - result, drone_position = drone_odometry_local.DronePositionLocal.create( + result, drone_position = position_local.PositionLocal.create( 0.0, 0.0, -100.0, @@ -1040,7 +1043,7 @@ def test_bad_direction( assert result assert drone_position is not None - result, drone_orientation = drone_odometry_local.DroneOrientationLocal.create_new( + result, drone_orientation = orientation.Orientation.create( 0.0, 0.0, 0.0, @@ -1067,4 +1070,4 @@ def test_bad_direction( # Test assert not result - assert actual_list is None + assert actual_list is None \ No newline at end of file diff --git a/utilities/workers/worker_manager.py b/utilities/workers/worker_manager.py index 4a5a26a2..be7480a7 100644 --- a/utilities/workers/worker_manager.py +++ b/utilities/workers/worker_manager.py @@ -4,7 +4,7 @@ import multiprocessing as mp -from modules.common.logger.modules import logger +from modules.common.modules.logger import logger from utilities.workers import worker_controller from utilities.workers import queue_proxy_wrapper From 647311ee7c9cb705d5ba2d31c427f0af03d35a80 Mon Sep 17 00:00:00 2001 From: asterbot Date: Tue, 5 Nov 2024 22:11:04 -0500 Subject: [PATCH 06/12] fixed unit test imports to align with common-lib refactor --- tests/unit/test_rudimentary_geolocation.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/tests/unit/test_rudimentary_geolocation.py b/tests/unit/test_rudimentary_geolocation.py index d5058149..283b5337 100644 --- a/tests/unit/test_rudimentary_geolocation.py +++ b/tests/unit/test_rudimentary_geolocation.py @@ -8,6 +8,8 @@ from modules import detection_in_world from modules import detections_and_time from modules import merged_odometry_detections +from modules.common.modules import orientation +from modules.common.modules import position_local from modules.common.modules.logger import logger from modules.common.modules.mavlink import drone_odometry_local from modules.geolocation import camera_properties @@ -815,7 +817,7 @@ def test_basic( 2 detections. """ # Setup - result, drone_position = drone_odometry_local.DronePositionLocal.create( + result, drone_position = position_local.PositionLocal.create( 0.0, 0.0, -100.0, @@ -823,7 +825,7 @@ def test_basic( assert result assert drone_position is not None - result, drone_orientation = drone_odometry_local.DroneOrientationLocal.create_new( + result, drone_orientation = orientation.Orientation.create( 0.0, -np.pi / 2, 0.0, @@ -909,4 +911,4 @@ def test_basic( np.testing.assert_almost_equal(actual.vertices, expected_list[i].vertices) np.testing.assert_almost_equal(actual.centre, expected_list[i].centre) assert actual.label == expected_list[i].label - np.testing.assert_almost_equal(actual.confidence, expected_list[i].confidence) + np.testing.assert_almost_equal(actual.confidence, expected_list[i].confidence) \ No newline at end of file From 44fba8cd74855f1c324803a961aa2425b56503e1 Mon Sep 17 00:00:00 2001 From: asterbot Date: Mon, 11 Nov 2024 23:34:49 -0500 Subject: [PATCH 07/12] Refactored to make it inherit from geolocation --- .../geolocation/rudimentary_geolocation.py | 256 +----------------- tests/unit/test_rudimentary_geolocation.py | 28 +- 2 files changed, 18 insertions(+), 266 deletions(-) diff --git a/modules/geolocation/rudimentary_geolocation.py b/modules/geolocation/rudimentary_geolocation.py index 212e5b7b..a758374d 100644 --- a/modules/geolocation/rudimentary_geolocation.py +++ b/modules/geolocation/rudimentary_geolocation.py @@ -1,267 +1,19 @@ """ -Converts image space into world space. +Converts image space into world space in a more rudimentary sense """ -import cv2 import numpy as np -from . import camera_properties from .. import detection_in_world -from .. import detections_and_time from .. import merged_odometry_detections from ..common.modules.logger import logger +from .geolocation import Geolocation - -class RudimentaryGeolocation: +class RudimentaryGeolocation(Geolocation): """ Converts image space into world space. """ - __create_key = object() - - __MIN_DOWN_COS_ANGLE = 0.1 # radians, ~84° - - @classmethod - def create( - cls, - camera_intrinsics: camera_properties.CameraIntrinsics, - camera_drone_extrinsics: camera_properties.CameraDroneExtrinsics, - local_logger: logger.Logger, - ) -> "tuple[bool, RudimentaryGeolocation | None]": - """ - camera_intrinsics: Camera information without any outside space. - camera_drone_extrinsics: Camera information related to the drone without any world space. - """ - # Centre of each edge - # list[list[float]] required for OpenCV - perspective_transform_sources = [ - [camera_intrinsics.resolution_x / 2, 0], - [camera_intrinsics.resolution_x / 2, camera_intrinsics.resolution_y], - [0, camera_intrinsics.resolution_y / 2], - [camera_intrinsics.resolution_x, camera_intrinsics.resolution_y / 2], - ] - - # Orientation in world space - rotated_source_vectors = [] - for source in perspective_transform_sources: - # Image space to camera space - result, value = camera_intrinsics.camera_space_from_image_space(source[0], source[1]) - if not result: - local_logger.error( - f"Rotated source vector could not be created for source: {source}" - ) - return False, None - - # Get Pylance to stop complaining - assert value is not None - - # Camera space to world space (orientation only) - vec_rotated_source = camera_drone_extrinsics.camera_to_drone_rotation_matrix @ value - rotated_source_vectors.append(vec_rotated_source) - - return True, RudimentaryGeolocation( - cls.__create_key, - camera_drone_extrinsics, - perspective_transform_sources, - rotated_source_vectors, - local_logger, - ) - - def __init__( - self, - class_private_create_key: object, - camera_drone_extrinsics: camera_properties.CameraDroneExtrinsics, - perspective_transform_sources: "list[list[float]]", - rotated_source_vectors: "list[np.ndarray]", - local_logger: logger.Logger, - ) -> None: - """ - Private constructor, use create() method. - """ - assert class_private_create_key is RudimentaryGeolocation.__create_key, "Use create() method" - - self.__camera_drone_extrinsics = camera_drone_extrinsics - self.__perspective_transform_sources = perspective_transform_sources - self.__rotated_source_vectors = rotated_source_vectors - self.__logger = local_logger - - # TODO: We need to convert both these fucntions to work for our case - @staticmethod - def __ground_intersection_from_vector( - vec_camera_in_world_position: np.ndarray, - vec_down: np.ndarray, - local_logger: logger.Logger, - ) -> "tuple[bool, np.ndarray | None]": - """ - Get 2D coordinates of where the downwards pointing vector intersects the ground. - """ - if not camera_properties.is_vector_r3(vec_camera_in_world_position): - local_logger.error("Camera position in world space is not a vector in R3") - return False, None - - if not camera_properties.is_vector_r3(vec_down): - local_logger.error("Rotated source vector in world space is not a vector in R3") - return False, None - - # Check camera above ground - if vec_camera_in_world_position[2] > 0.0: - local_logger.error("Camera is underground") - return False, None - - # Ensure vector is pointing down by checking angle - # cos(angle) = a dot b / (||a|| * ||b||) - vec_z = np.array([0.0, 0.0, 1.0], dtype=np.float32) - cos_angle = np.dot(vec_down, vec_z) / np.linalg.norm(vec_down) - if cos_angle < RudimentaryGeolocation.__MIN_DOWN_COS_ANGLE: - local_logger.error( - f"Rotated source vector in world space is not pointing down, cos(angle) = {cos_angle}" - ) - return False, None - - # Find scalar multiple for the vector to touch the ground (z/3rd component is 0) - # Solve for s: o3 + s * d3 = 0 - scaling = -vec_camera_in_world_position[2] / vec_down[2] - if scaling < 0.0: - local_logger.error(f"Scaling value is negative, scaling = {scaling}") - return False, None - - vec_ground = vec_camera_in_world_position + scaling * vec_down - - return True, vec_ground[:2] - - def __get_perspective_transform_matrix( - self, drone_rotation_matrix: np.ndarray, drone_position_ned: np.ndarray - ) -> "tuple[bool, np.ndarray | None]": - """ - Calculates the destination points, then uses OpenCV to get the matrix. - """ - if not camera_properties.is_matrix_r3x3(drone_rotation_matrix): - self.__logger.error("Drone rotation matrix is not a 3 x 3 matrix") - return False, None - - if not camera_properties.is_vector_r3(drone_position_ned): - self.__logger.error("Drone position in local space is not a vector in R3") - return False, None - - # Get the vectors in world space - vec_downs = [] - for vector in self.__rotated_source_vectors: - vec_down = drone_rotation_matrix @ vector - vec_downs.append(vec_down) - - # Get the camera position in world space - vec_camera_position = ( - drone_position_ned - + drone_rotation_matrix @ self.__camera_drone_extrinsics.vec_camera_on_drone_position - ) - - # Find the points on the ground - ground_points = [] - for vec_down in vec_downs: - result, ground_point = self.__ground_intersection_from_vector( - vec_camera_position, - vec_down, - self.__logger, - ) - if not result: - return False, None - - ground_points.append(ground_point) - - # Get the image to ground mapping - src = np.array(self.__perspective_transform_sources, dtype=np.float32) - dst = np.array(ground_points, dtype=np.float32) - try: - matrix = cv2.getPerspectiveTransform( # type: ignore - src, - dst, - ) - # All exceptions must be caught and logged as early as possible - # pylint: disable-next=broad-exception-caught - except Exception as e: - self.__logger.error(f"Could not get perspective transform matrix: {e}") - return False, None - - return True, matrix - - @staticmethod - def __convert_detection_to_world_from_image( - detection: detections_and_time.Detection, - perspective_transform_matrix: np.ndarray, - local_logger: logger.Logger, - ) -> "tuple[bool, detection_in_world.DetectionInWorld | None]": - """ - Applies the transform matrix to the detection. - perspective_transform_matrix: Element in last row and column must be 1 . - """ - if not camera_properties.is_matrix_r3x3(perspective_transform_matrix): - local_logger.error("Perspective transform matrix is not a 3 x 3 matrix") - return False, None - - if not np.allclose(perspective_transform_matrix[2][2], 1.0): - local_logger.error( - "Perspective transform matrix bottom right element is not close to 1.0" - ) - return False, None - - centre = detection.get_centre() - top_left, top_right, bottom_left, bottom_right = detection.get_corners() - - input_centre = np.array([centre[0], centre[1], 1.0], dtype=np.float32) - # More efficient to multiply a matrix than looping over the points - # Transpose to columns from rows - input_vertices = np.array( - [ - [top_left[0], top_left[1], 1.0], - [top_right[0], top_right[1], 1.0], - [bottom_left[0], bottom_left[1], 1.0], - [bottom_right[0], bottom_right[1], 1.0], - ], - dtype=np.float32, - ).T - - # Single row/column does not need transpose - output_centre = perspective_transform_matrix @ input_centre - # Transpose back to rows from columns - output_vertices = (perspective_transform_matrix @ input_vertices).T - - # Normalize by last element - # Homogeneous/perspective divide: - # https://en.wikipedia.org/wiki/Transformation_matrix#Perspective_projection - ground_centre = np.array( - [ - output_centre[0] / output_centre[2], - output_centre[1] / output_centre[2], - ], - dtype=np.float32, - ) - - # Normalize each row by its last element - - # Slice to get the last element of each row - vec_last_element = output_vertices[:, 2] - - # Divide each row by vector element: - # https://www.w3resource.com/python-exercises/numpy/python-numpy-exercise-96.php - output_normalized = output_vertices / vec_last_element[:, None] - if not np.isfinite(output_normalized).all(): - local_logger.error("Normalized output is infinite") - return False, None - - # Slice to remove the last element of each row - ground_vertices = output_normalized[:, :2] - - result, detection_world = detection_in_world.DetectionInWorld.create( - ground_vertices, - ground_centre, - detection.label, - detection.confidence, - ) - if not result: - return False, None - - return True, detection_world - def run( self, detections: merged_odometry_detections.MergedOdometryDetections ) -> "tuple[bool, list[detection_in_world.DetectionInWorld] | None]": @@ -281,7 +33,7 @@ def run( dtype=np.float32, ) - # Since camera points down, the rotation matrix will be + # Since camera points down, the rotation matrix will be this # Calculated assuming pitch=-pi/2 and yaw=roll=0 drone_rotation_matrix = np.array( [ diff --git a/tests/unit/test_rudimentary_geolocation.py b/tests/unit/test_rudimentary_geolocation.py index 283b5337..34a10ffb 100644 --- a/tests/unit/test_rudimentary_geolocation.py +++ b/tests/unit/test_rudimentary_geolocation.py @@ -295,7 +295,7 @@ def test_above_origin_directly_down(self) -> None: ( result, actual, - ) = rudimentary_geolocation.RudimentaryGeolocation._RudimentaryGeolocation__ground_intersection_from_vector( # type: ignore + ) = rudimentary_geolocation.RudimentaryGeolocation._Geolocation__ground_intersection_from_vector( # type: ignore vec_camera_in_world_position, vec_down, test_logger, @@ -324,7 +324,7 @@ def test_non_origin_directly_down(self) -> None: ( result, actual, - ) = rudimentary_geolocation.RudimentaryGeolocation._RudimentaryGeolocation__ground_intersection_from_vector( # type: ignore + ) = rudimentary_geolocation.RudimentaryGeolocation._Geolocation__ground_intersection_from_vector( # type: ignore vec_camera_in_world_position, vec_down, test_logger, @@ -353,7 +353,7 @@ def test_above_origin_angled_down(self) -> None: ( result, actual, - ) = rudimentary_geolocation.RudimentaryGeolocation._RudimentaryGeolocation__ground_intersection_from_vector( # type: ignore + ) = rudimentary_geolocation.RudimentaryGeolocation._Geolocation__ground_intersection_from_vector( # type: ignore vec_camera_in_world_position, vec_down, test_logger, @@ -382,7 +382,7 @@ def test_non_origin_angled_down(self) -> None: ( result, actual, - ) = rudimentary_geolocation.RudimentaryGeolocation._RudimentaryGeolocation__ground_intersection_from_vector( # type: ignore + ) = rudimentary_geolocation.RudimentaryGeolocation._Geolocation__ground_intersection_from_vector( # type: ignore vec_camera_in_world_position, vec_down, test_logger, @@ -409,7 +409,7 @@ def test_bad_almost_horizontal(self) -> None: ( result, actual, - ) = rudimentary_geolocation.RudimentaryGeolocation._RudimentaryGeolocation__ground_intersection_from_vector( # type: ignore + ) = rudimentary_geolocation.RudimentaryGeolocation._Geolocation__ground_intersection_from_vector( # type: ignore vec_camera_in_world_position, vec_horizontal, test_logger, @@ -435,7 +435,7 @@ def test_bad_upwards(self) -> None: ( result, actual, - ) = rudimentary_geolocation.RudimentaryGeolocation._RudimentaryGeolocation__ground_intersection_from_vector( # type: ignore + ) = rudimentary_geolocation.RudimentaryGeolocation._Geolocation__ground_intersection_from_vector( # type: ignore vec_camera_in_world_position, vec_up, test_logger, @@ -461,7 +461,7 @@ def test_bad_underground(self) -> None: ( result, actual, - ) = rudimentary_geolocation.RudimentaryGeolocation._RudimentaryGeolocation__ground_intersection_from_vector( # type: ignore + ) = rudimentary_geolocation.RudimentaryGeolocation._Geolocation__ground_intersection_from_vector( # type: ignore vec_underground, vec_down, test_logger, @@ -498,7 +498,7 @@ def test_basic_above_origin_pointed_down(self, basic_locator: rudimentary_geoloc ( result, actual, - ) = basic_locator._RudimentaryGeolocation__get_perspective_transform_matrix( # type: ignore + ) = basic_locator._Geolocation__get_perspective_transform_matrix( # type: ignore drone_rotation_matrix, drone_position_ned, ) @@ -538,7 +538,7 @@ def test_intermediate_above_origin_pointing_north( ( result, actual, - ) = intermediate_locator._RudimentaryGeolocation__get_perspective_transform_matrix( # type: ignore + ) = intermediate_locator._Geolocation__get_perspective_transform_matrix( # type: ignore drone_rotation_matrix, drone_position_ned, ) @@ -578,7 +578,7 @@ def test_intermediate_above_origin_pointing_west( ( result, actual, - ) = intermediate_locator._RudimentaryGeolocation__get_perspective_transform_matrix( # type: ignore + ) = intermediate_locator._Geolocation__get_perspective_transform_matrix( # type: ignore drone_rotation_matrix, drone_position_ned, ) @@ -631,7 +631,7 @@ def test_advanced(self, advanced_locator: rudimentary_geolocation.RudimentaryGeo ( result, actual, - ) = advanced_locator._RudimentaryGeolocation__get_perspective_transform_matrix( # type: ignore + ) = advanced_locator._Geolocation__get_perspective_transform_matrix( # type: ignore drone_rotation_matrix, drone_position_ned, ) @@ -682,7 +682,7 @@ def test_bad_direction(self, basic_locator: rudimentary_geolocation.RudimentaryG ( result, actual, - ) = basic_locator._RudimentaryGeolocation__get_perspective_transform_matrix( # type: ignore + ) = basic_locator._Geolocation__get_perspective_transform_matrix( # type: ignore drone_rotation_matrix, drone_position_ned, ) @@ -734,7 +734,7 @@ def test_normal1( ( result, actual, - ) = rudimentary_geolocation.RudimentaryGeolocation._RudimentaryGeolocation__convert_detection_to_world_from_image( # type: ignore + ) = rudimentary_geolocation.RudimentaryGeolocation._Geolocation__convert_detection_to_world_from_image( # type: ignore detection_1, affine_matrix, test_logger, @@ -786,7 +786,7 @@ def test_normal2( ( result, actual, - ) = rudimentary_geolocation.RudimentaryGeolocation._RudimentaryGeolocation__convert_detection_to_world_from_image( # type: ignore + ) = rudimentary_geolocation.RudimentaryGeolocation._Geolocation__convert_detection_to_world_from_image( # type: ignore detection_2, affine_matrix, test_logger, From f4cf186090a5ece03f1f7b03a6bfc81639f67e88 Mon Sep 17 00:00:00 2001 From: asterbot Date: Mon, 11 Nov 2024 23:41:00 -0500 Subject: [PATCH 08/12] Linters --- modules/geolocation/rudimentary_geolocation.py | 4 ++-- tests/unit/test_rudimentary_geolocation.py | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/modules/geolocation/rudimentary_geolocation.py b/modules/geolocation/rudimentary_geolocation.py index a758374d..5b1987dd 100644 --- a/modules/geolocation/rudimentary_geolocation.py +++ b/modules/geolocation/rudimentary_geolocation.py @@ -6,9 +6,9 @@ from .. import detection_in_world from .. import merged_odometry_detections -from ..common.modules.logger import logger from .geolocation import Geolocation + class RudimentaryGeolocation(Geolocation): """ Converts image space into world space. @@ -37,7 +37,7 @@ def run( # Calculated assuming pitch=-pi/2 and yaw=roll=0 drone_rotation_matrix = np.array( [ - [0.0, 0.0,-1.0], + [0.0, 0.0, -1.0], [0.0, 1.0, 0.0], [1.0, 0.0, 0.0], ], diff --git a/tests/unit/test_rudimentary_geolocation.py b/tests/unit/test_rudimentary_geolocation.py index 34a10ffb..109864c4 100644 --- a/tests/unit/test_rudimentary_geolocation.py +++ b/tests/unit/test_rudimentary_geolocation.py @@ -911,4 +911,4 @@ def test_basic( np.testing.assert_almost_equal(actual.vertices, expected_list[i].vertices) np.testing.assert_almost_equal(actual.centre, expected_list[i].centre) assert actual.label == expected_list[i].label - np.testing.assert_almost_equal(actual.confidence, expected_list[i].confidence) \ No newline at end of file + np.testing.assert_almost_equal(actual.confidence, expected_list[i].confidence) From fe9631a3100dbba2f607ff46693f14ed3e795103 Mon Sep 17 00:00:00 2001 From: asterbot Date: Mon, 11 Nov 2024 23:47:58 -0500 Subject: [PATCH 09/12] Running black linter --- tests/unit/test_geolocation.py | 2 +- tests/unit/test_rudimentary_geolocation.py | 12 +++++++++--- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/tests/unit/test_geolocation.py b/tests/unit/test_geolocation.py index b73f9e39..47a41e4b 100644 --- a/tests/unit/test_geolocation.py +++ b/tests/unit/test_geolocation.py @@ -1070,4 +1070,4 @@ def test_bad_direction( # Test assert not result - assert actual_list is None \ No newline at end of file + assert actual_list is None diff --git a/tests/unit/test_rudimentary_geolocation.py b/tests/unit/test_rudimentary_geolocation.py index 109864c4..bf8b18b4 100644 --- a/tests/unit/test_rudimentary_geolocation.py +++ b/tests/unit/test_rudimentary_geolocation.py @@ -477,7 +477,9 @@ class TestPerspectiveTransformMatrix: Test perspective transform creation. """ - def test_basic_above_origin_pointed_down(self, basic_locator: rudimentary_geolocation.RudimentaryGeolocation) -> None: + def test_basic_above_origin_pointed_down( + self, basic_locator: rudimentary_geolocation.RudimentaryGeolocation + ) -> None: """ Above origin, directly down. """ @@ -595,7 +597,9 @@ def test_intermediate_above_origin_pointing_west( decimal=FLOAT_PRECISION_TOLERANCE, ) - def test_advanced(self, advanced_locator: rudimentary_geolocation.RudimentaryGeolocation) -> None: + def test_advanced( + self, advanced_locator: rudimentary_geolocation.RudimentaryGeolocation + ) -> None: """ Camera is north of origin with an angle from vertical. Also rotated. """ @@ -656,7 +660,9 @@ def test_advanced(self, advanced_locator: rudimentary_geolocation.RudimentaryGeo decimal=FLOAT_PRECISION_TOLERANCE, ) - def test_bad_direction(self, basic_locator: rudimentary_geolocation.RudimentaryGeolocation) -> None: + def test_bad_direction( + self, basic_locator: rudimentary_geolocation.RudimentaryGeolocation + ) -> None: """ Camera pointing forward. """ From f6d0f9dfc13de5182ce12eb742252034ba83723d Mon Sep 17 00:00:00 2001 From: asterbot Date: Mon, 11 Nov 2024 23:59:17 -0500 Subject: [PATCH 10/12] Pylint fix --- modules/geolocation/rudimentary_geolocation.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/modules/geolocation/rudimentary_geolocation.py b/modules/geolocation/rudimentary_geolocation.py index 5b1987dd..3b693912 100644 --- a/modules/geolocation/rudimentary_geolocation.py +++ b/modules/geolocation/rudimentary_geolocation.py @@ -50,7 +50,11 @@ def run( ) if not result: return False, None + + # Get Pylance to stop complaining + assert perspective_transform_matrix is not None + # pylint: disable=duplicate-code detections_in_world = [] for detection in detections.detections: result, detection_world = self.__convert_detection_to_world_from_image( From 2bd09a8b249d919ae915dd7bb205e49dd8d0d5bc Mon Sep 17 00:00:00 2001 From: asterbot Date: Tue, 12 Nov 2024 00:03:19 -0500 Subject: [PATCH 11/12] black linting --- modules/geolocation/rudimentary_geolocation.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/geolocation/rudimentary_geolocation.py b/modules/geolocation/rudimentary_geolocation.py index 3b693912..f566ac14 100644 --- a/modules/geolocation/rudimentary_geolocation.py +++ b/modules/geolocation/rudimentary_geolocation.py @@ -50,7 +50,7 @@ def run( ) if not result: return False, None - + # Get Pylance to stop complaining assert perspective_transform_matrix is not None From 87a7c86af47f627a2bf96e95b0d181dee71fef2e Mon Sep 17 00:00:00 2001 From: asterbot Date: Tue, 12 Nov 2024 00:18:18 -0500 Subject: [PATCH 12/12] Removed unecessary unit tests --- tests/unit/test_rudimentary_geolocation.py | 764 ++------------------- 1 file changed, 47 insertions(+), 717 deletions(-) diff --git a/tests/unit/test_rudimentary_geolocation.py b/tests/unit/test_rudimentary_geolocation.py index bf8b18b4..c35e7b34 100644 --- a/tests/unit/test_rudimentary_geolocation.py +++ b/tests/unit/test_rudimentary_geolocation.py @@ -21,6 +21,7 @@ # Test functions use test fixture signature names and access class privates # No enable # pylint: disable=protected-access,redefined-outer-name +# pylint: disable=duplicate-code @pytest.fixture @@ -59,111 +60,6 @@ def basic_locator() -> rudimentary_geolocation.RudimentaryGeolocation: # type: yield locator # type: ignore -@pytest.fixture -def intermediate_locator() -> rudimentary_geolocation.RudimentaryGeolocation: # type: ignore - """ - Downwards pointing camera offset towards front of drone. - """ - result, camera_intrinsics = camera_properties.CameraIntrinsics.create( - 2000, - 2000, - np.pi / 2, - np.pi / 2, - ) - assert result - assert camera_intrinsics is not None - - result, camera_extrinsics = camera_properties.CameraDroneExtrinsics.create( - (1.0, 0.0, 0.0), - (0.0, -np.pi / 2, 0.0), - ) - assert result - assert camera_extrinsics is not None - - result, test_logger = logger.Logger.create("test_logger", False) - assert result - assert test_logger is not None - - result, locator = rudimentary_geolocation.RudimentaryGeolocation.create( - camera_intrinsics, - camera_extrinsics, - test_logger, - ) - assert result - assert locator is not None - - yield locator # type: ignore - - -@pytest.fixture -def advanced_locator() -> rudimentary_geolocation.RudimentaryGeolocation: # type: ignore - """ - Camera angled at 75° upward. - Drone is expected to rotate it downwards. - """ - result, camera_intrinsics = camera_properties.CameraIntrinsics.create( - 2000, - 2000, - np.pi / 3, - np.pi / 3, - ) - assert result - assert camera_intrinsics is not None - - result, camera_extrinsics = camera_properties.CameraDroneExtrinsics.create( - (1.0, 0.0, 0.0), - (0.0, np.pi * 5 / 12, np.pi / 2), - ) - assert result - assert camera_extrinsics is not None - - result, test_logger = logger.Logger.create("test_logger", False) - assert result - assert test_logger is not None - - result, locator = rudimentary_geolocation.RudimentaryGeolocation.create( - camera_intrinsics, - camera_extrinsics, - test_logger, - ) - assert result - assert locator is not None - - yield locator # type: ignore - - -@pytest.fixture -def detection_bottom_right_point() -> detections_and_time.Detection: # type: ignore - """ - Bounding box is a single point. - """ - result, detection = detections_and_time.Detection.create( - np.array([2000.0, 2000.0, 2000.0, 2000.0], dtype=np.float32), - 0, - 0.01, - ) - assert result - assert detection is not None - - yield detection # type: ignore - - -@pytest.fixture -def detection_centre_left_point() -> detections_and_time.Detection: # type: ignore - """ - Bounding box is a single point. - """ - result, detection = detections_and_time.Detection.create( - np.array([0.0, 1000.0, 0.0, 1000.0], dtype=np.float32), - 0, - 0.1, - ) - assert result - assert detection is not None - - yield detection # type: ignore - - @pytest.fixture def detection_1() -> detections_and_time.Detection: # type: ignore """ @@ -196,618 +92,6 @@ def detection_2() -> detections_and_time.Detection: # type: ignore yield detection # type: ignore -@pytest.fixture -def affine_matrix() -> np.ndarray: # type: ignore - """ - 3x3 homogeneous. - """ - # fmt: off - matrix = np.array( - [ - [0.0, 1.0, -1.0], - [2.0, 0.0, -1.0], - [0.0, 0.0, 1.0], - ], - dtype=np.float32, - ) - # fmt: on - - yield matrix # type: ignore - - -@pytest.fixture -def non_affine_matrix() -> np.ndarray: # type: ignore - """ - 3x3 homogeneous. - """ - # fmt: off - matrix = np.array( - [ - [0.0, 1.0, -1.0], - [2.0, 0.0, -1.0], - [1.0, 1.0, 1.0], - ], - dtype=np.float32, - ) - # fmt: on - - yield matrix # type: ignore - - -class TestRudimentaryGeolocationCreate: - """ - Test constructor. - """ - - def test_normal(self) -> None: - """ - Successful construction. - """ - result, camera_intrinsics = camera_properties.CameraIntrinsics.create( - 2000, - 2000, - np.pi / 2, - np.pi / 2, - ) - assert result - assert camera_intrinsics is not None - - result, camera_extrinsics = camera_properties.CameraDroneExtrinsics.create( - (0.0, 0.0, 0.0), - (0.0, 0.0, 0.0), - ) - assert result - assert camera_extrinsics is not None - - result, test_logger = logger.Logger.create("test_logger", False) - assert result - assert test_logger is not None - - result, locator = rudimentary_geolocation.RudimentaryGeolocation.create( - camera_intrinsics, - camera_extrinsics, - test_logger, - ) - assert result - assert locator is not None - - -class TestGroundIntersection: - """ - Test where vector intersects with ground. - """ - - def test_above_origin_directly_down(self) -> None: - """ - Above origin, directly down. - """ - # Setup - result, test_logger = logger.Logger.create("test_logger", False) - assert result - assert test_logger is not None - - vec_camera_in_world_position = np.array([0.0, 0.0, -100.0], dtype=np.float32) - vec_down = np.array([0.0, 0.0, 1.0], dtype=np.float32) - - expected = np.array([0.0, 0.0], dtype=np.float32) - - # Run - ( - result, - actual, - ) = rudimentary_geolocation.RudimentaryGeolocation._Geolocation__ground_intersection_from_vector( # type: ignore - vec_camera_in_world_position, - vec_down, - test_logger, - ) - - # Test - assert result - assert actual is not None - np.testing.assert_almost_equal(actual, expected, decimal=FLOAT_PRECISION_TOLERANCE) - - def test_non_origin_directly_down(self) -> None: - """ - Directly down. - """ - # Setup - result, test_logger = logger.Logger.create("test_logger", False) - assert result - assert test_logger is not None - - vec_camera_in_world_position = np.array([100.0, -100.0, -100.0], dtype=np.float32) - vec_down = np.array([0.0, 0.0, 1.0], dtype=np.float32) - - expected = np.array([100.0, -100.0], dtype=np.float32) - - # Run - ( - result, - actual, - ) = rudimentary_geolocation.RudimentaryGeolocation._Geolocation__ground_intersection_from_vector( # type: ignore - vec_camera_in_world_position, - vec_down, - test_logger, - ) - - # Test - assert result - assert actual is not None - np.testing.assert_almost_equal(actual, expected, decimal=FLOAT_PRECISION_TOLERANCE) - - def test_above_origin_angled_down(self) -> None: - """ - Above origin, angled down towards positive. - """ - # Setup - result, test_logger = logger.Logger.create("test_logger", False) - assert result - assert test_logger is not None - - vec_camera_in_world_position = np.array([0.0, 0.0, -100.0], dtype=np.float32) - vec_down = np.array([1.0, 1.0, 1.0], dtype=np.float32) - - expected = np.array([100.0, 100.0], dtype=np.float32) - - # Run - ( - result, - actual, - ) = rudimentary_geolocation.RudimentaryGeolocation._Geolocation__ground_intersection_from_vector( # type: ignore - vec_camera_in_world_position, - vec_down, - test_logger, - ) - - # Test - assert result - assert actual is not None - np.testing.assert_almost_equal(actual, expected, decimal=FLOAT_PRECISION_TOLERANCE) - - def test_non_origin_angled_down(self) -> None: - """ - Angled down towards origin. - """ - # Setup - result, test_logger = logger.Logger.create("test_logger", False) - assert result - assert test_logger is not None - - vec_camera_in_world_position = np.array([100.0, -100.0, -100.0], dtype=np.float32) - vec_down = np.array([-1.0, 1.0, 1.0], dtype=np.float32) - - expected = np.array([0.0, 0.0], dtype=np.float32) - - # Run - ( - result, - actual, - ) = rudimentary_geolocation.RudimentaryGeolocation._Geolocation__ground_intersection_from_vector( # type: ignore - vec_camera_in_world_position, - vec_down, - test_logger, - ) - - # Test - assert result - assert actual is not None - np.testing.assert_almost_equal(actual, expected, decimal=FLOAT_PRECISION_TOLERANCE) - - def test_bad_almost_horizontal(self) -> None: - """ - False, None . - """ - # Setup - result, test_logger = logger.Logger.create("test_logger", False) - assert result - assert test_logger is not None - - vec_camera_in_world_position = np.array([0.0, 0.0, -100.0], dtype=np.float32) - vec_horizontal = np.array([10.0, 0.0, 1.0], dtype=np.float32) - - # Run - ( - result, - actual, - ) = rudimentary_geolocation.RudimentaryGeolocation._Geolocation__ground_intersection_from_vector( # type: ignore - vec_camera_in_world_position, - vec_horizontal, - test_logger, - ) - - # Test - assert not result - assert actual is None - - def test_bad_upwards(self) -> None: - """ - False, None . - """ - # Setup - result, test_logger = logger.Logger.create("test_logger", False) - assert result - assert test_logger is not None - - vec_camera_in_world_position = np.array([0.0, 0.0, -100.0], dtype=np.float32) - vec_up = np.array([0.0, 0.0, -1.0], dtype=np.float32) - - # Run - ( - result, - actual, - ) = rudimentary_geolocation.RudimentaryGeolocation._Geolocation__ground_intersection_from_vector( # type: ignore - vec_camera_in_world_position, - vec_up, - test_logger, - ) - - # Test - assert not result - assert actual is None - - def test_bad_underground(self) -> None: - """ - False, None . - """ - # Setup - result, test_logger = logger.Logger.create("test_logger", False) - assert result - assert test_logger is not None - - vec_underground = np.array([0.0, 0.0, 1.0], dtype=np.float32) - vec_down = np.array([0.0, 0.0, 1.0], dtype=np.float32) - - # Run - ( - result, - actual, - ) = rudimentary_geolocation.RudimentaryGeolocation._Geolocation__ground_intersection_from_vector( # type: ignore - vec_underground, - vec_down, - test_logger, - ) - - # Test - assert not result - assert actual is None - - -class TestPerspectiveTransformMatrix: - """ - Test perspective transform creation. - """ - - def test_basic_above_origin_pointed_down( - self, basic_locator: rudimentary_geolocation.RudimentaryGeolocation - ) -> None: - """ - Above origin, directly down. - """ - # Setup - result, drone_rotation_matrix = camera_properties.create_rotation_matrix_from_orientation( - 0.0, - -np.pi / 2, - 0.0, - ) - assert result - assert drone_rotation_matrix is not None - - drone_position_ned = np.array([0.0, 0.0, -100.0], dtype=np.float32) - - vec_ground_expected = np.array([-100.0, 100.0, 1.0], dtype=np.float32) - - # Run - ( - result, - actual, - ) = basic_locator._Geolocation__get_perspective_transform_matrix( # type: ignore - drone_rotation_matrix, - drone_position_ned, - ) - - # Test - assert result - assert actual is not None - - vec_ground = actual @ np.array([2000, 2000, 1]) - vec_ground_normalized = vec_ground / vec_ground[2] - np.testing.assert_almost_equal( - vec_ground_normalized, - vec_ground_expected, - decimal=FLOAT_PRECISION_TOLERANCE, - ) - - def test_intermediate_above_origin_pointing_north( - self, intermediate_locator: rudimentary_geolocation.RudimentaryGeolocation - ) -> None: - """ - Positioned so that the camera is above the origin directly down (but the drone is not). - """ - # Setup - result, drone_rotation_matrix = camera_properties.create_rotation_matrix_from_orientation( - 0.0, - 0.0, - 0.0, - ) - assert result - assert drone_rotation_matrix is not None - - drone_position_ned = np.array([-1.0, 0.0, -100.0], dtype=np.float32) - - vec_ground_expected = np.array([-100.0, 100.0, 1.0], dtype=np.float32) - - # Run - ( - result, - actual, - ) = intermediate_locator._Geolocation__get_perspective_transform_matrix( # type: ignore - drone_rotation_matrix, - drone_position_ned, - ) - - # Test - assert result - assert actual is not None - - vec_ground = actual @ np.array([2000, 2000, 1]) - vec_ground_normalized = vec_ground / vec_ground[2] - np.testing.assert_almost_equal( - vec_ground_normalized, - vec_ground_expected, - decimal=FLOAT_PRECISION_TOLERANCE, - ) - - def test_intermediate_above_origin_pointing_west( - self, intermediate_locator: rudimentary_geolocation.RudimentaryGeolocation - ) -> None: - """ - Positioned so that the camera is above the origin directly down (but the drone is not). - """ - # Setup - result, drone_rotation_matrix = camera_properties.create_rotation_matrix_from_orientation( - -np.pi / 2, - 0.0, - 0.0, - ) - assert result - assert drone_rotation_matrix is not None - - drone_position_ned = np.array([0.0, 1.0, -100.0], dtype=np.float32) - - vec_ground_expected = np.array([100.0, 100.0, 1.0], dtype=np.float32) - - # Run - ( - result, - actual, - ) = intermediate_locator._Geolocation__get_perspective_transform_matrix( # type: ignore - drone_rotation_matrix, - drone_position_ned, - ) - - # Test - assert result - assert actual is not None - - vec_ground = actual @ np.array([2000, 2000, 1]) - vec_ground_normalized = vec_ground / vec_ground[2] - np.testing.assert_almost_equal( - vec_ground_normalized, - vec_ground_expected, - decimal=FLOAT_PRECISION_TOLERANCE, - ) - - def test_advanced( - self, advanced_locator: rudimentary_geolocation.RudimentaryGeolocation - ) -> None: - """ - Camera is north of origin with an angle from vertical. Also rotated. - """ - # Setup - result, drone_rotation_matrix = camera_properties.create_rotation_matrix_from_orientation( - 0.0, - np.pi / 12, - -np.pi, - ) - assert result - assert drone_rotation_matrix is not None - - drone_position_ned = np.array( - [ - 10.0 - np.cos(-np.pi / 12), # Camera at 10 units forward - 0.0, - -100.0 - np.sin(-np.pi / 12), # Camera at 100 units above ground - ], - dtype=np.float32, - ) - - vec_ground_sanity_expected = np.array([10.0, 0.0, 1.0], dtype=np.float32) - vec_ground_expected = np.array( - [ - 10.0 + 100.0 * np.sqrt(3), - 100.0, - 1.0, - ], - dtype=np.float32, - ) - - # Run - ( - result, - actual, - ) = advanced_locator._Geolocation__get_perspective_transform_matrix( # type: ignore - drone_rotation_matrix, - drone_position_ned, - ) - - # Test - assert result - assert actual is not None - - vec_ground_sanity = actual @ np.array([0, 1000, 1]) - vec_ground_sanity_actual = vec_ground_sanity / vec_ground_sanity[2] - np.testing.assert_almost_equal( - vec_ground_sanity_actual, - vec_ground_sanity_expected, - decimal=FLOAT_PRECISION_TOLERANCE, - ) - - vec_ground = actual @ np.array([2000, 2000, 1]) - vec_ground_normalized = vec_ground / vec_ground[2] - np.testing.assert_almost_equal( - vec_ground_normalized, - vec_ground_expected, - decimal=FLOAT_PRECISION_TOLERANCE, - ) - - def test_bad_direction( - self, basic_locator: rudimentary_geolocation.RudimentaryGeolocation - ) -> None: - """ - Camera pointing forward. - """ - # Setup - result, drone_rotation_matrix = camera_properties.create_rotation_matrix_from_orientation( - 0.0, - 0.0, - 0.0, - ) - assert result - assert drone_rotation_matrix is not None - - drone_position_ned = np.array( - [ - 0.0, - 0.0, - -100.0, - ], - dtype=np.float32, - ) - - # Run - ( - result, - actual, - ) = basic_locator._Geolocation__get_perspective_transform_matrix( # type: ignore - drone_rotation_matrix, - drone_position_ned, - ) - - # Test - assert not result - assert actual is None - - -class TestRudimentaryGeolocationConvertDetection: - """ - Test extract and convert. - """ - - def test_normal1( - self, detection_1: detections_and_time.Detection, affine_matrix: np.ndarray - ) -> None: - """ - Normal detection and matrix. - """ - # Setup - result, test_logger = logger.Logger.create("test_logger", False) - assert result - assert test_logger is not None - - result, expected = detection_in_world.DetectionInWorld.create( - # fmt: off - np.array( - [ - [ -1.0, -1.0], - [ -1.0, 3999.0], - [1999.0, -1.0], - [1999.0, 3999.0], - ], - dtype=np.float32, - ), - # fmt: on - np.array( - [999.0, 1999.0], - dtype=np.float32, - ), - 1, - 1.0, - ) - assert result - assert expected is not None - - # Run - ( - result, - actual, - ) = rudimentary_geolocation.RudimentaryGeolocation._Geolocation__convert_detection_to_world_from_image( # type: ignore - detection_1, - affine_matrix, - test_logger, - ) - - # Test - assert result - assert actual is not None - - np.testing.assert_almost_equal(actual.vertices, expected.vertices) - np.testing.assert_almost_equal(actual.centre, expected.centre) - assert actual.label == expected.label - np.testing.assert_almost_equal(actual.confidence, expected.confidence) - - def test_normal2( - self, detection_2: detections_and_time.Detection, affine_matrix: np.ndarray - ) -> None: - """ - Normal detection and matrix. - """ - # Setup - result, test_logger = logger.Logger.create("test_logger", False) - assert result - assert test_logger is not None - - result, expected = detection_in_world.DetectionInWorld.create( - # fmt: off - np.array( - [ - [ -1.0, -1.0], - [ -1.0, 1999.0], - [999.0, -1.0], - [999.0, 1999.0], - ], - dtype=np.float32, - ), - # fmt: on - np.array( - [499.0, 999.0], - dtype=np.float32, - ), - 2, - 0.5, - ) - assert result - assert expected is not None - - # Run - ( - result, - actual, - ) = rudimentary_geolocation.RudimentaryGeolocation._Geolocation__convert_detection_to_world_from_image( # type: ignore - detection_2, - affine_matrix, - test_logger, - ) - - # Test - assert result - assert actual is not None - - np.testing.assert_almost_equal(actual.vertices, expected.vertices) - np.testing.assert_almost_equal(actual.centre, expected.centre) - assert actual.label == expected.label - np.testing.assert_almost_equal(actual.confidence, expected.confidence) - - class TestRudimentaryGeolocationRun: """ Run. @@ -918,3 +202,49 @@ def test_basic( np.testing.assert_almost_equal(actual.centre, expected_list[i].centre) assert actual.label == expected_list[i].label np.testing.assert_almost_equal(actual.confidence, expected_list[i].confidence) + + def test_bad_direction( + self, + basic_locator: rudimentary_geolocation.RudimentaryGeolocation, + detection_1: detections_and_time.Detection, + ) -> None: + """ + Bad direction. + """ + # Setup + result, drone_position = position_local.PositionLocal.create( + 0.0, + 0.0, + -100.0, + ) + assert result + assert drone_position is not None + + result, drone_orientation = orientation.Orientation.create( + 0.0, + 0.0, + 0.0, + ) + assert result + assert drone_orientation is not None + + result, drone_odometry = drone_odometry_local.DroneOdometryLocal.create( + drone_position, + drone_orientation, + ) + assert result + assert drone_odometry is not None + + result, merged_detections = merged_odometry_detections.MergedOdometryDetections.create( + drone_odometry, + [detection_1], + ) + assert result + assert merged_detections is not None + + # Run + result, actual_list = basic_locator.run(merged_detections) + + # Test + assert not result + assert actual_list is None