wip: many fixes many improvements

configs/config.defaults.yaml

@@ -26,9 +26,9 @@ preprocessing:
 
 line_detection:
   pixels_in_window: 3
-  window_margin_growth: 0.05  # Percentage, relative to the window width.
+  window_margin_growth: 0.3  # Percentage, relative to the window width.
   max_angle_difference: 30  # degrees
-  max_angle_junction: 70  # degrees
+  max_angle_junction: 60  # degrees
   min_window_shift: 0.3 # Percentage, relative to the window height.
 
   window_sizing:
@@ -37,11 +37,11 @@ line_detection:
     min_width: 1.0  # meters
 
   thresholds:
-    zebra_crossing: 1.0  # meters
+    zebra_crossing: 0.6  # meters
 
   filtering:
     active: true
-    rel_height: 0.9  # percentage
+    rel_height: 0.95  # percentage
     min_distance: 1.0  # meters
     margin: 0.2  # meters
 

src/calibration/data.py

@@ -51,8 +51,7 @@ def transform(self, images: list[np.ndarray]) -> np.ndarray:
         if self.topdown_matrix is None:
             raise ValueError("Calibrator has not been calibrated yet.")
 
-        stitched = np.zeros(self.stitched_shape[::-1], dtype=np.uint8)
-
+        stitched = np.full(self.stitched_shape[::-1], 255, dtype=np.uint8)
         futures = []
         for i, image in enumerate(images):
             if i == self.ref_idx:
@@ -70,7 +69,9 @@ def transform(self, images: list[np.ndarray]) -> np.ndarray:
             stitched,
             self.topdown_matrix,
             self.output_shape,
-            flags=cv2.INTER_NEAREST
+            flags=cv2.INTER_NEAREST,
+            borderMode=cv2.BORDER_CONSTANT,
+            borderValue=(255, 255, 255)
         )
 
     def _stitch_image(self, stitched: np.ndarray, image: np.ndarray, idx: int) -> np.ndarray:

src/lane_assist/lane_assist.py

@@ -56,10 +56,6 @@ class LaneAssist:
     __stop_line_assist: StopLineAssist
     __calibration: CalibrationData
 
-    # NEW: tracking of current lane, target lane will replace requested lane
-    __current_lane: int
-    __target_lane: int
-
     def __init__(
             self,
             image_generation: Callable[[], Generator[np.ndarray, None, None]],

src/lane_assist/line_detection/window.py

@@ -45,6 +45,11 @@ def __init__(self, x: int, y: int, shape: tuple[int, int], max_width: int) -> No
         self.__max_width = max_width
         self.__original_shape = shape
 
+    @property
+    def first_point(self) -> tuple[int, int]:
+        """The first point in the window."""
+        return self.__points[0]
+
     @property
     def last_point(self) -> tuple[int, int]:
         """The last point in the window."""
@@ -93,8 +98,7 @@ def move(self, x: int, y: int, found_points: bool = True) -> None:
                     self.directions = np.roll(self.directions, 1, axis=0)
                     self.directions[0] = [x - self.x, y - self.y]
 
-            self.shape = self.__original_shape
-            self.not_found = 0
+            self.__reset_not_found()
         else:
             margin = 1 + config["line_detection"]["window_margin_growth"]
             new_width = min(self.__max_width, self.shape[1] * margin)
@@ -106,6 +110,17 @@ def move(self, x: int, y: int, found_points: bool = True) -> None:
         self.x = x
         self.y = y
 
+    def move_back(self) -> None:
+        """Move the window back."""
+        if self.point_count == 0:
+            return
+
+        self.__points.pop()
+        self.x, self.y = self.last_point
+        self.directions[0] = self.directions[1]
+
+        self.__reset_not_found()
+
     def __is_crowded(self, x: int, y: int) -> bool:
         """Check if the new position is too close to the last point.
 
@@ -117,3 +132,8 @@ def __is_crowded(self, x: int, y: int) -> bool:
         min_distance = self.shape[0] * config["line_detection"]["min_window_shift"]
 
         return distance < min_distance
+
+    def __reset_not_found(self) -> None:
+        """Reset the not found counter."""
+        self.not_found = 0
+        self.shape = self.__original_shape

src/lane_assist/line_detection/window_search.py

@@ -19,6 +19,7 @@ def process_window(image: np.ndarray, window: Window, stop_line: bool) -> Line |
     """
     image_center = image.shape[1] // 2
     attempts_left = 3
+    attempts_reset_after = 10
 
     while not __window_at_bounds(image, window):
         top, bottom, left, right = window.get_borders(image.shape)
@@ -32,8 +33,10 @@ def process_window(image: np.ndarray, window: Window, stop_line: bool) -> Line |
             continue
 
         # Calculate the new position of the window
-        target = window.shape[1] if right < image_center else 0
-        offset = center_of_masses(chunk, target, config["line_detection"]["pixels_in_window"])
+        ref_point = __get_ref_point(window)
+        target_point = __get_target_point(window, ref_point, image_center)
+
+        offset = center_of_masses(chunk, target_point, config["line_detection"]["pixels_in_window"])
         if offset is None:
             __move_no_points(window)
             continue
@@ -52,13 +55,18 @@ def process_window(image: np.ndarray, window: Window, stop_line: bool) -> Line |
 
             is_junction = angle_diff > config["line_detection"]["max_angle_junction"]
             if window.not_found == 0 and is_junction and attempts_left > 0:
+                x_diff, y_diff = window.directions[1:].mean(axis=0)
                 attempts_left -= 1
-                x_diff, y_diff = window.directions.mean(axis=0)
 
+                window.move_back()
                 window.move(int(window.x + x_diff), int(window.y + y_diff), False)
                 continue
 
-        attempts_left = 3
+        if attempts_reset_after == 0:
+            attempts_left = 3
+            attempts_reset_after = 10
+
+        attempts_reset_after -= 1
         window.move(new_pos[0], new_pos[1])
 
     # Check if we have enough points to make a line
@@ -104,6 +112,33 @@ def __get_angle(window: Window, new_pos: tuple[int, int]) -> float:
     return abs(prev_direction - curr_direction)
 
 
+def __get_ref_point(window: Window) -> int:
+    """Get the reference point for the window.
+
+    :param window: The window to get the reference point for.
+    :return: The reference point.
+    """
+    ref_point = window.x
+    if window.point_count > 0:
+        ref_point = window.first_point[0]
+
+    return ref_point + window.margin
+
+
+def __get_target_point(window: Window, ref_point: int, image_center: int) -> int:
+    """Get the target point for the window.
+
+    :param window: The window to get the target point for.
+    :param ref_point: The reference point.
+    :param image_center: The center of the image.
+    :return: The target point.
+    """
+    if ref_point < image_center:
+        return window.shape[1]
+
+    return 0
+
+
 def __move_no_points(window: Window) -> None:
     """Move the window if there are no points in it.
 
@@ -114,6 +149,9 @@ def __move_no_points(window: Window) -> None:
 
     if window.point_count >= len(window.directions):
         x_shift, y_shift = window.directions.mean(axis=0)
+    elif window.point_count > 1:
+        x_shift, y_shift = window.directions.sum(axis=0)
+        y_shift = max(-window.shape[0], y_shift)
 
     window.move(window.x + x_shift, window.y + y_shift, False)
 

src/lane_assist/preprocessing/image_filters.py

@@ -77,7 +77,10 @@ def morphex_filter(image: np.ndarray, calibration: CalibrationData) -> np.ndarra
 
     histogram_peaks = basic_filter_ranges(image, hpx, width, margin)
 
-    full_mask = cv2.morphologyEx(image, cv2.MORPH_OPEN, cv2.getStructuringElement(cv2.MORPH_RECT, (7, 7)))
+    full_mask = np.full((image.shape[0] + 10, image.shape[1]), 255, dtype=np.uint8)
+    full_mask[:-10] = image
+
+    full_mask = cv2.morphologyEx(full_mask, cv2.MORPH_OPEN, cv2.getStructuringElement(cv2.MORPH_RECT, (7, 7)))
     full_mask = cv2.dilate(full_mask, cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (13, 13)))
 
     full_mask[full_mask > 100] = 255

src/simulation/main.py

@@ -62,7 +62,12 @@ def get_sim_image_generator() -> Generator[np.ndarray, None, None]:
             )
 
             top_view = cv2.warpPerspective(
-                front_view, calibration.topdown_matrix, calibration.output_shape, flags=cv2.INTER_NEAREST
+                front_view,
+                calibration.topdown_matrix,
+                calibration.output_shape,
+                flags=cv2.INTER_NEAREST,
+                borderMode=cv2.BORDER_CONSTANT,
+                borderValue=(255, 255, 255)
             )
 
             if config["telemetry"]["enabled"]: