Improve Mapping

uavf_2025/gnc/commander_node.py

@@ -275,16 +275,16 @@ def get_dropzone_bounds_mlocal(self) -> list[tuple[float, float]]:
         """
         return [gps_to_local(x, self.global_home) for x in self.dropzone_bounds]
 
-    def get_roi_corners_local(self) -> list[tuple[float, float]]:
+    def get_roi_corners_local(self):
         """
         Return the top left, top right, and bottom left corners of the ROI in local coordinates
         (in that order).
         """
-        return [
+        return (
             gps_to_local(self.mapping_bounds[2], self.global_home),
             gps_to_local(self.mapping_bounds[3], self.global_home),
             gps_to_local(self.mapping_bounds[1], self.global_home),
-        ]
+        )
 
     def get_pose_position(self) -> np.ndarray:
         """

uavf_2025/gnc/mapping_tsp.py

@@ -1,13 +1,14 @@
-import cv2
 import math
+from pathlib import Path
+from time import strftime
+
+import cv2
 import networkx as nx
 import numpy as np
-from pathlib import Path
 from PIL import Image
-from time import strftime
+from line_profiler import profile
 
-HEATMAP_DIM = 1000
-FT_PER_METER = 3.28084
+HEATMAP_DIM = 100
 
 
 class MappingPathPlanner:
@@ -17,10 +18,13 @@ class MappingPathPlanner:
 
     def __init__(
         self,
-        heatmap: np.array,
-        roi_corners: tuple[tuple[float, float]],
+        heatmap: np.ndarray,
+        roi_corners: tuple[
+            tuple[float, float], tuple[float, float], tuple[float, float]
+        ],
         m_height: float,
         min_unmapped: float,
+        ft_per_meter: float = 3.28084,
     ) -> None:
         """
         heatmap: Grayscale image where black pixels indicate unmapped areas
@@ -32,6 +36,8 @@ def __init__(
         self._roi_corners = roi_corners
         self._m_height = m_height
         self._min_unmapped = min_unmapped
+        self._ft_per_meter = ft_per_meter
+
         self._G = nx.Graph()
         self._s = 0
         self._local_path = []
@@ -42,23 +48,24 @@ def _initialize_graph_params(self) -> None:
         Calculate s value such that ROI is segmented into squares that are equal to or smaller
         than the area covered by the camera.
         """
-        ft_height = self._m_height * FT_PER_METER
+        ft_height = self._m_height * self._ft_per_meter
 
         camera_y = 45 * ft_height / 50
 
         x_ft_per_pixel = (
             np.linalg.norm(np.subtract(self._roi_corners[0], self._roi_corners[1]))
-            * FT_PER_METER
+            * self._ft_per_meter
         ) / HEATMAP_DIM
         y_ft_per_pixel = (
             np.linalg.norm(np.subtract(self._roi_corners[0], self._roi_corners[2]))
-            * FT_PER_METER
+            * self._ft_per_meter
         ) / HEATMAP_DIM
 
         camera_length = camera_y / max(x_ft_per_pixel, y_ft_per_pixel)
 
         self._s = int(camera_length / math.sqrt(2))
 
+    @profile
     def _create_vertices(self) -> None:
         """
         Vertices are in the format (x, y), which means that when represented in pixels
@@ -80,8 +87,8 @@ def _create_vertices(self) -> None:
                         (self._pixel_to_local((v_x, v_y))), heatmap_coord=(v_x, v_y)
                     )
 
-    def _divide_roi(self) -> None:
-        x_y_slices = []
+    def _divide_roi(self):
+        x_y_slices: list[int] = []
         x_y = self._s
         while x_y < HEATMAP_DIM:
             x_y_slices.append(x_y)
@@ -90,6 +97,7 @@ def _divide_roi(self) -> None:
 
         return x_y_slices
 
+    @profile
     def _create_edges(self):
         V = list(self._G.nodes)
         total_V = len(V)
@@ -99,13 +107,13 @@ def _create_edges(self):
                     V[i], V[j], weight=np.linalg.norm(np.subtract(V[i], V[j]))
                 )
 
-    def _generate_tsp_cycle(self):
+    def _generate_tsp_cycle(self) -> list[tuple[float, float]]:
         return nx.approximation.traveling_salesman_problem(
             self._G, method=nx.approximation.christofides
-        )
+        )  # type: ignore
 
     def _cycle_to_path(
-        self, current_pos: tuple[float], cycle: list[tuple[float, float]]
+        self, current_pos: tuple[float, ...], cycle: list[tuple[float, float]]
     ) -> list[tuple[float, float]]:
         """
         Change Traveling Salesman Problem cycle to path starting at the vertex closest to the
@@ -118,28 +126,30 @@ def _cycle_to_path(
         cycle.pop()
         return cycle[start_idx:] + cycle[0:start_idx]
 
+    @profile
     def construct_path(
-        self, current_pos: tuple[float]
+        self, current_pos: tuple[float, float] | tuple[float, float, float]
     ) -> list[tuple[float, float, float]]:
         self._create_vertices()
         if self._G.number_of_nodes() <= 1:
             self._local_path = list(self._G.nodes)
         else:
             self._create_edges()
-            self._local_path = self._cycle_to_path(
-                current_pos, self._generate_tsp_cycle()
-            )
+
+            cycle = self._generate_tsp_cycle()
+            self._local_path = self._cycle_to_path(current_pos, cycle)
         return list(
             map(
                 lambda coord: (round(coord[0], 3), round(coord[1], 3), self._m_height),
                 self._local_path,
             )
         )
 
-    def _pixel_to_local(self, px_coord: tuple[float, float]):
+    def _pixel_to_local(self, px_coord: tuple[float, float]) -> tuple[float, float]:
         """
         Pixel coordinates are in form (column, row) to match the (x, y) format of vertices.
         """
+        top_left = np.array(self._roi_corners[0])
         horiz_dist = np.multiply(
             px_coord[0] / HEATMAP_DIM,
             np.subtract(self._roi_corners[1], self._roi_corners[0]),
@@ -148,9 +158,9 @@ def _pixel_to_local(self, px_coord: tuple[float, float]):
             px_coord[1] / HEATMAP_DIM,
             np.subtract(self._roi_corners[2], self._roi_corners[0]),
         )
-        return tuple(np.sum([self._roi_corners[0], horiz_dist, vert_dist], axis=0))
+        return tuple(top_left + horiz_dist + vert_dist)
 
-    def _draw_vertices_on(self, img: np.array) -> None:
+    def _draw_vertices_on(self, img: np.ndarray) -> None:
         """
         Row and column where vertices show up may be off by 0.5.
         """
@@ -159,7 +169,7 @@ def _draw_vertices_on(self, img: np.array) -> None:
             v = tuple(int(x) for x in v)
             cv2.line(img, v, v, (0, 255, 0), 20)
 
-    def _draw_tsp_edges_on(self, img: np.array) -> None:
+    def _draw_tsp_edges_on(self, img: np.ndarray) -> None:
         if len(self._local_path) <= 1:
             return
         for i in range(len(self._local_path) - 1):
@@ -173,16 +183,16 @@ def _draw_tsp_edges_on(self, img: np.array) -> None:
         # Number last vertex in path
         cv2.putText(img, f"{i + 1}", v, cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2)
 
-    def draw_path(self, img: np.array) -> np.array:
+    def draw_path(self, img: np.ndarray) -> np.ndarray:
         img_dim = img.shape
         if len(img_dim) < 3 or img_dim[2] < 3:
-            img = Image.fromarray(img)
-            img = np.array(img.convert("RGB"))
+            img = np.array(Image.fromarray(img).convert("RGB"))
+
         self._draw_vertices_on(img)
         self._draw_tsp_edges_on(img)
         return img
 
-    def save_path_image(self, img: np.array):
+    def save_path_image(self, img: np.ndarray):
         time_string = strftime("%Y-%m-%d_%H-%M")
         mapping_logs_path = Path(__file__).absolute().parent.parent.parent / Path(
             f"logs/mapping_path/{time_string}"
@@ -191,3 +201,22 @@ def save_path_image(self, img: np.array):
         cv2.imwrite(
             str(mapping_logs_path / Path("planned_path.png")), self.draw_path(img)
         )
+
+
+if __name__ == "__main__":
+    # Example usage
+    heatmap = np.zeros((HEATMAP_DIM, HEATMAP_DIM), dtype=np.uint8)
+    roi_corners = ((0, 0), (HEATMAP_DIM, 0), (0, HEATMAP_DIM))
+    m_height = 10.0
+    min_unmapped = 0.1
+
+    planner = MappingPathPlanner(heatmap, roi_corners, m_height, min_unmapped)
+    current_position = (500, 500)  # Example current position
+    path = planner.construct_path(current_position)
+
+    import csv
+
+    with open("mapping_path.csv", "w", newline="") as csvfile:
+        writer = csv.writer(csvfile)
+        writer.writerow(["x", "y", "z"])
+        writer.writerows(path)