diff --git a/modules/geolocation/rudimentary_geolocation.py b/modules/geolocation/rudimentary_geolocation.py new file mode 100644 index 00000000..f566ac14 --- /dev/null +++ b/modules/geolocation/rudimentary_geolocation.py @@ -0,0 +1,71 @@ +""" +Converts image space into world space in a more rudimentary sense +""" + +import numpy as np + +from .. import detection_in_world +from .. import merged_odometry_detections +from .geolocation import Geolocation + + +class RudimentaryGeolocation(Geolocation): + """ + Converts image space into world space. + """ + + 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 this + # 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 + + # 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( + 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 new file mode 100644 index 00000000..c35e7b34 --- /dev/null +++ b/tests/unit/test_rudimentary_geolocation.py @@ -0,0 +1,250 @@ +""" +Test rudimentary_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 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 rudimentary_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 +# pylint: disable=duplicate-code + + +@pytest.fixture +def basic_locator() -> rudimentary_geolocation.RudimentaryGeolocation: # 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 = 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_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 + + +class TestRudimentaryGeolocationRun: + """ + Run. + """ + + def test_basic( + self, + basic_locator: rudimentary_geolocation.RudimentaryGeolocation, + detection_1: detections_and_time.Detection, + detection_2: detections_and_time.Detection, + ) -> None: + """ + 2 detections. + """ + # 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, + -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_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