diff --git a/modules/common b/modules/common index 09811f6c..5c3894e0 160000 --- a/modules/common +++ b/modules/common @@ -1 +1 @@ -Subproject commit 09811f6c5ba5182509fef5b32dc1c346de5676ff +Subproject commit 5c3894e043c5129a1c0579c6f6acae2cc7a52a18 diff --git a/modules/detection_in_world.py b/modules/detection_in_world.py index cef6207d..a5c22943 100644 --- a/modules/detection_in_world.py +++ b/modules/detection_in_world.py @@ -52,3 +52,9 @@ def __init__( self.centre = centre self.label = label self.confidence = confidence + + def __str__(self) -> str: + """ + To string. + """ + return f"{self.__class__}, vertices: {self.vertices.tolist()}, centre: {self.centre}, label: {self.label}, confidence: {self.confidence}" diff --git a/modules/geolocation/geolocation.py b/modules/geolocation/geolocation.py index 5b344054..4006d447 100644 --- a/modules/geolocation/geolocation.py +++ b/modules/geolocation/geolocation.py @@ -9,6 +9,7 @@ from .. import detection_in_world from .. import detections_and_time from .. import merged_odometry_detections +from ..common.logger.modules import logger class Geolocation: @@ -25,6 +26,7 @@ def create( cls, camera_intrinsics: camera_properties.CameraIntrinsics, camera_drone_extrinsics: camera_properties.CameraDroneExtrinsics, + local_logger: logger.Logger, ) -> "tuple[bool, Geolocation | None]": """ camera_intrinsics: Camera information without any outside space. @@ -45,6 +47,9 @@ def create( # 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 @@ -59,6 +64,7 @@ def create( camera_drone_extrinsics, perspective_transform_sources, rotated_source_vectors, + local_logger, ) def __init__( @@ -67,6 +73,7 @@ def __init__( 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. @@ -76,22 +83,28 @@ def __init__( 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 @staticmethod def __ground_intersection_from_vector( - vec_camera_in_world_position: np.ndarray, vec_down: np.ndarray + 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 @@ -99,12 +112,16 @@ def __ground_intersection_from_vector( 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 < Geolocation.__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 @@ -118,9 +135,11 @@ def __get_perspective_transform_matrix( 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 @@ -141,6 +160,7 @@ def __get_perspective_transform_matrix( result, ground_point = self.__ground_intersection_from_vector( vec_camera_position, vec_down, + self.__logger, ) if not result: return False, None @@ -156,25 +176,31 @@ def __get_perspective_transform_matrix( dst, ) # All exceptions must be caught and logged as early as possible - # pylint: disable-next=bare-except - except: - # TODO: Logging + # 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 + 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() @@ -218,6 +244,7 @@ def __convert_detection_to_world_from_image( # 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 @@ -243,6 +270,7 @@ def run( # Camera position in world (NED system) # Cannot be underground if detections.odometry_local.position.down >= 0.0: + self.__logger.error("Drone is underground") return False, None drone_position_ned = np.array( @@ -262,6 +290,7 @@ def run( detections.odometry_local.orientation.orientation.roll, ) if not result: + self.__logger.error("Drone rotation matrix could not be created") return False, None # Get Pylance to stop complaining @@ -282,10 +311,12 @@ def run( 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/modules/geolocation/geolocation_worker.py b/modules/geolocation/geolocation_worker.py index b4195542..ac040a95 100644 --- a/modules/geolocation/geolocation_worker.py +++ b/modules/geolocation/geolocation_worker.py @@ -2,10 +2,14 @@ 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.logger.modules import logger def geolocation_worker( @@ -21,15 +25,26 @@ def geolocation_worker( input_queue and output_queue are data queues. controller is how the main process communicates to this worker process. """ - # TODO: Logging? # 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: - print("Worker failed to create class object") + local_logger.error("Worker failed to create class object") return # Get Pylance to stop complaining diff --git a/tests/unit/test_geolocation.py b/tests/unit/test_geolocation.py index f8c2dda8..84a85371 100644 --- a/tests/unit/test_geolocation.py +++ b/tests/unit/test_geolocation.py @@ -11,6 +11,7 @@ from modules import merged_odometry_detections from modules.geolocation import camera_properties from modules.geolocation import geolocation +from modules.common.logger.modules import logger FLOAT_PRECISION_TOLERANCE = 4 @@ -42,9 +43,14 @@ def basic_locator() -> geolocation.Geolocation: # type: ignore 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 @@ -73,9 +79,14 @@ def intermediate_locator() -> geolocation.Geolocation: # type: ignore 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 @@ -105,9 +116,14 @@ def advanced_locator() -> geolocation.Geolocation: # type: ignore 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 @@ -242,9 +258,14 @@ def test_normal(self) -> None: 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 @@ -260,6 +281,10 @@ 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) @@ -272,6 +297,7 @@ def test_above_origin_directly_down(self) -> None: ) = geolocation.Geolocation._Geolocation__ground_intersection_from_vector( # type: ignore vec_camera_in_world_position, vec_down, + test_logger, ) # Test @@ -284,6 +310,10 @@ 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) @@ -296,6 +326,7 @@ def test_non_origin_directly_down(self) -> None: ) = geolocation.Geolocation._Geolocation__ground_intersection_from_vector( # type: ignore vec_camera_in_world_position, vec_down, + test_logger, ) # Test @@ -308,6 +339,10 @@ 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) @@ -320,6 +355,7 @@ def test_above_origin_angled_down(self) -> None: ) = geolocation.Geolocation._Geolocation__ground_intersection_from_vector( # type: ignore vec_camera_in_world_position, vec_down, + test_logger, ) # Test @@ -332,6 +368,10 @@ 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) @@ -344,6 +384,7 @@ def test_non_origin_angled_down(self) -> None: ) = geolocation.Geolocation._Geolocation__ground_intersection_from_vector( # type: ignore vec_camera_in_world_position, vec_down, + test_logger, ) # Test @@ -356,6 +397,10 @@ 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) @@ -366,6 +411,7 @@ def test_bad_almost_horizontal(self) -> None: ) = geolocation.Geolocation._Geolocation__ground_intersection_from_vector( # type: ignore vec_camera_in_world_position, vec_horizontal, + test_logger, ) # Test @@ -377,6 +423,10 @@ 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) @@ -387,6 +437,7 @@ def test_bad_upwards(self) -> None: ) = geolocation.Geolocation._Geolocation__ground_intersection_from_vector( # type: ignore vec_camera_in_world_position, vec_up, + test_logger, ) # Test @@ -398,6 +449,10 @@ 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) @@ -408,6 +463,7 @@ def test_bad_underground(self) -> None: ) = geolocation.Geolocation._Geolocation__ground_intersection_from_vector( # type: ignore vec_underground, vec_down, + test_logger, ) # Test @@ -647,6 +703,10 @@ def test_normal1( 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( @@ -676,6 +736,7 @@ def test_normal1( ) = geolocation.Geolocation._Geolocation__convert_detection_to_world_from_image( # type: ignore detection_1, affine_matrix, + test_logger, ) # Test @@ -694,6 +755,10 @@ def test_normal2( 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( @@ -723,6 +788,7 @@ def test_normal2( ) = geolocation.Geolocation._Geolocation__convert_detection_to_world_from_image( # type: ignore detection_2, affine_matrix, + test_logger, ) # Test