From 3dabd9d5f6cf645dac5fa7256bc6cba37dbeac84 Mon Sep 17 00:00:00 2001
From: mgupta27 <mihir_gupta@rocketmail.com>
Date: Sun, 8 Sep 2024 13:57:41 -0400
Subject: [PATCH 1/6] Updated common submodule

---
 modules/common | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

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

From 5372e818d4967038ae899e7dce3ff8a252799aab Mon Sep 17 00:00:00 2001
From: mgupta27 <mihir_gupta@rocketmail.com>
Date: Sun, 8 Sep 2024 14:05:55 -0400
Subject: [PATCH 2/6] Added to string method for DetectionInWorld

---
 modules/detection_in_world.py | 6 ++++++
 1 file changed, 6 insertions(+)

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}"

From 45cbd261895c3be39cf5cb3057e3d9a0a177a56c Mon Sep 17 00:00:00 2001
From: mgupta27 <mihir_gupta@rocketmail.com>
Date: Sun, 8 Sep 2024 15:02:54 -0400
Subject: [PATCH 3/6] Created logger in geolocation worker

---
 modules/geolocation/geolocation.py        |  5 +++++
 modules/geolocation/geolocation_worker.py | 18 +++++++++++++++++-
 2 files changed, 22 insertions(+), 1 deletion(-)

diff --git a/modules/geolocation/geolocation.py b/modules/geolocation/geolocation.py
index 5b344054..61f2754b 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.
@@ -59,6 +61,7 @@ def create(
             camera_drone_extrinsics,
             perspective_transform_sources,
             rotated_source_vectors,
+            local_logger,
         )
 
     def __init__(
@@ -67,6 +70,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,6 +80,7 @@ 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(
diff --git a/modules/geolocation/geolocation_worker.py b/modules/geolocation/geolocation_worker.py
index b4195542..ea789356 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(
@@ -24,12 +28,24 @@ def geolocation_worker(
     # 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

From 0619d3aac49dd8277696efd5ade53638ea228009 Mon Sep 17 00:00:00 2001
From: mgupta27 <mihir_gupta@rocketmail.com>
Date: Sun, 8 Sep 2024 16:15:54 -0400
Subject: [PATCH 4/6] Added logging to geolocation

---
 modules/geolocation/geolocation.py | 20 +++++++++++++++++---
 tests/unit/test_geolocation.py     | 21 +++++++++++++++++++++
 2 files changed, 38 insertions(+), 3 deletions(-)

diff --git a/modules/geolocation/geolocation.py b/modules/geolocation/geolocation.py
index 61f2754b..7ed011c1 100644
--- a/modules/geolocation/geolocation.py
+++ b/modules/geolocation/geolocation.py
@@ -47,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
@@ -123,9 +126,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 is not a vector in R3")
             return False, None
 
         # Get the vectors in world space
@@ -148,6 +153,7 @@ def __get_perspective_transform_matrix(
                 vec_down,
             )
             if not result:
+                self.__logger.error("Could not get ground intersection from vector")
                 return False, None
 
             ground_points.append(ground_point)
@@ -161,10 +167,14 @@ 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=catching-non-exception
+        except cv2.error as e:
+            self.__logger.error(f"Could not get perspective transform matrix: {e}")
             return False, None
+        # 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 True, matrix
 
@@ -248,6 +258,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(
@@ -267,6 +278,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
@@ -290,7 +302,9 @@ def run(
             )
             # Partial data not allowed
             if not result:
+                self.__logger("Could not convert detection to world from image")
                 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_geolocation.py b/tests/unit/test_geolocation.py
index f8c2dda8..97e68d5b 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

From 9d003c7af409a77b5303703cbfde1a948e2db433 Mon Sep 17 00:00:00 2001
From: mgupta27 <mihir_gupta@rocketmail.com>
Date: Tue, 10 Sep 2024 17:04:34 -0400
Subject: [PATCH 5/6] Added logging and fixed tests

---
 modules/geolocation/geolocation.py | 26 +++++++++++++----
 tests/unit/test_geolocation.py     | 45 ++++++++++++++++++++++++++++++
 2 files changed, 66 insertions(+), 5 deletions(-)

diff --git a/modules/geolocation/geolocation.py b/modules/geolocation/geolocation.py
index 7ed011c1..fd7ba646 100644
--- a/modules/geolocation/geolocation.py
+++ b/modules/geolocation/geolocation.py
@@ -87,19 +87,24 @@ def __init__(
 
     @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):
+            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
@@ -107,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
@@ -130,7 +139,7 @@ def __get_perspective_transform_matrix(
             return False, None
 
         if not camera_properties.is_vector_r3(drone_position_ned):
-            self.__logger.error("Drone position is not a vector in R3")
+            self.__logger.error("Drone position in local space is not a vector in R3")
             return False, None
 
         # Get the vectors in world space
@@ -151,9 +160,9 @@ def __get_perspective_transform_matrix(
             result, ground_point = self.__ground_intersection_from_vector(
                 vec_camera_position,
                 vec_down,
+                self.__logger,
             )
             if not result:
-                self.__logger.error("Could not get ground intersection from vector")
                 return False, None
 
             ground_points.append(ground_point)
@@ -180,16 +189,22 @@ def __get_perspective_transform_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()
@@ -233,6 +248,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
@@ -299,10 +315,10 @@ 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:
-                self.__logger("Could not convert detection to world from image")
                 return False, None
             detections_in_world.append(detection_world)
             self.__logger.info(detection_world)
diff --git a/tests/unit/test_geolocation.py b/tests/unit/test_geolocation.py
index 97e68d5b..84a85371 100644
--- a/tests/unit/test_geolocation.py
+++ b/tests/unit/test_geolocation.py
@@ -281,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)
 
@@ -293,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
@@ -305,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)
 
@@ -317,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
@@ -329,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)
 
@@ -341,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
@@ -353,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)
 
@@ -365,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
@@ -377,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)
 
@@ -387,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
@@ -398,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)
 
@@ -408,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
@@ -419,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)
 
@@ -429,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
@@ -668,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(
@@ -697,6 +736,7 @@ def test_normal1(
         ) = geolocation.Geolocation._Geolocation__convert_detection_to_world_from_image(  # type: ignore
             detection_1,
             affine_matrix,
+            test_logger,
         )
 
         # Test
@@ -715,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(
@@ -744,6 +788,7 @@ def test_normal2(
         ) = geolocation.Geolocation._Geolocation__convert_detection_to_world_from_image(  # type: ignore
             detection_2,
             affine_matrix,
+            test_logger,
         )
 
         # Test

From 6e3627383d06b2a8b03088a72c9fa575f7578572 Mon Sep 17 00:00:00 2001
From: mgupta27 <mihir_gupta@rocketmail.com>
Date: Thu, 12 Sep 2024 23:15:24 -0400
Subject: [PATCH 6/6] Addressed review #1

---
 modules/geolocation/geolocation.py        | 8 ++------
 modules/geolocation/geolocation_worker.py | 1 -
 2 files changed, 2 insertions(+), 7 deletions(-)

diff --git a/modules/geolocation/geolocation.py b/modules/geolocation/geolocation.py
index fd7ba646..4006d447 100644
--- a/modules/geolocation/geolocation.py
+++ b/modules/geolocation/geolocation.py
@@ -99,7 +99,7 @@ def __ground_intersection_from_vector(
             return False, None
 
         if not camera_properties.is_vector_r3(vec_down):
-            logger.error("Rotated source vector in world space is not a vector in R3")
+            local_logger.error("Rotated source vector in world space is not a vector in R3")
             return False, None
 
         # Check camera above ground
@@ -176,14 +176,10 @@ def __get_perspective_transform_matrix(
                 dst,
             )
         # All exceptions must be caught and logged as early as possible
-        # pylint: disable-next=catching-non-exception
-        except cv2.error as e:
-            self.__logger.error(f"Could not get perspective transform matrix: {e}")
-            return False, None
-        # 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
 
diff --git a/modules/geolocation/geolocation_worker.py b/modules/geolocation/geolocation_worker.py
index ea789356..ac040a95 100644
--- a/modules/geolocation/geolocation_worker.py
+++ b/modules/geolocation/geolocation_worker.py
@@ -25,7 +25,6 @@ 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