Lane detection sliding windows (#35)

* commit

* implemented lane detection by using a n-sliding window algorithm.

next to the implementation of the sliding window algorithm i also did the following things:
  - moved images to resource folder
  - implemented tests/benchmark for line detection
  - adjusted gamma of the image stitching
  - fixed some formatting

* fixed test

* fixed comments

requirements.txt

@@ -6,3 +6,7 @@ python-can==4.3.1
 pywin32==306
 typing_extensions==4.10.0
 wrapt==1.16.0
+
+typing~=3.7.4.3
+matplotlib~=3.8.3
+scipy=1.12.0

src/common/config.py

@@ -1,11 +1,6 @@
 from common.constants import SpeedMode
 
 speed_mode: SpeedMode = SpeedMode.SLOW
-speed_mode_to_speed = {
-    SpeedMode.SLOW:      25,
-    SpeedMode.MEDIUM:    50,
-    SpeedMode.FAST:      75,
-    SpeedMode.VERY_FAST: 100
-}
+speed_mode_to_speed = {SpeedMode.SLOW: 25, SpeedMode.MEDIUM: 50, SpeedMode.FAST: 75, SpeedMode.VERY_FAST: 100}
 
 speed = speed_mode_to_speed[speed_mode]

src/common/constants.py

@@ -4,18 +4,20 @@
 class CANControlIdentifier(IntEnum):
     """The identifiers for the CAN messages sent to the go-kart."""
 
-    BRAKE    = 0x110
+    BRAKE = 0x110
     STEERING = 0x220
     THROTTLE = 0x330
 
+
 class CANFeedbackIdentifier(IntEnum):
     """The identifiers for the CAN messages received from the go-kart."""
 
-    BRAKE           = 0x710
-    SPEED_SENSOR    = 0x440
-    STEERING_ECU    = 0x720
+    BRAKE = 0x710
+    SPEED_SENSOR = 0x440
+    STEERING_ECU = 0x720
     STEERING_SENSOR = 0x1E5
-    THROTTLE        = 0x730
+    THROTTLE = 0x730
+
 
 class CameraResolution:
     """The camera resolutions that the Logitech StreamCam supports."""
@@ -24,6 +26,7 @@ class CameraResolution:
     HD = (1280, 720)
     VGA = (848, 480)
 
+
 class CameraFramerate:
     """The camera framerates that the Logitech StreamCam supports."""
 
@@ -34,17 +37,19 @@ class CameraFramerate:
     FPS_15 = 15
     FPS_10 = 10
 
+
 class Gear(IntEnum):
     """The gear of the go-kart."""
 
     NEUTRAL = 0
-    DRIVE   = 1
+    DRIVE = 1
     REVERSE = 2
 
+
 class SpeedMode(IntEnum):
     """The mode of the speed controller."""
 
-    SLOW =      0
-    MEDIUM =    1
-    FAST =      2
+    SLOW = 0
+    MEDIUM = 1
+    FAST = 2
     VERY_FAST = 3

src/kart_control/can_controller.py

@@ -28,9 +28,7 @@ def __init__(self, can_bus: can.Bus) -> None:
         self.__listeners = {}
         self.__thread = threading.Thread(target=self.__listen, daemon=True)
 
-    def add_listener(
-        self, message_id: CANFeedbackIdentifier, listener: callable
-    ) -> None:
+    def add_listener(self, message_id: CANFeedbackIdentifier, listener: callable) -> None:
         """Add a listener for a message.
 
         :param message_id: The identifier of the message.

src/kart_control/new_controller/controller.py

@@ -105,37 +105,35 @@ def __start(self) -> None:
                 elif event.ev_type == "Absolute":
                     self._handle_axis_event(event)
 
-    def vibrate(self, duration: int=1000) -> None:
+    def vibrate(self, duration: int = 1000) -> None:
         """Vibrate the controller.
-        
+
         Parameters
         ----------
         :param duration int: the duration to vibrate in miliseconds. default = 1000
-        
+
         """
         try:
             self.gamepad.set_vibration(1, 1, duration)
         except Exception:  # noqa: BLE001
-            print("Failed to vibrate")   # noqa: T201
+            print("Failed to vibrate")  # noqa: T201
 
-    def add_listener(self, event_type: EventType, button_or_axis: ControllerButton | ControllerAxis, 
-                        callback: callable) -> None:
+    def add_listener(
+        self, event_type: EventType, button_or_axis: ControllerButton | ControllerAxis, callback: callable
+    ) -> None:
         """Add a listener to be executed on that event.
-        
+
         Parameters
         ----------
         :param event_type EventType: the type of event to call it on.
         :param button_or_axis ControllerButton | ControllerAxis: the axis or button that the event needs to be for.
         :param callback callable: the callback to call when the event occurs
-        
+
         """
         if event_type not in EventType:
             raise ValueError(f"Invalid event type: {event_type}")
 
-        if (
-            button_or_axis not in ControllerButton
-            and button_or_axis not in ControllerAxis
-        ):
+        if button_or_axis not in ControllerButton and button_or_axis not in ControllerAxis:
             raise ValueError(f"Invalid button or axis: {button_or_axis}")
         key = (event_type, button_or_axis)
         if key not in self._listeners:
@@ -173,7 +171,9 @@ def _handle_axis_event(self, event: EventType) -> None:
         self._axes[axis] = value
         self._check_events(EventType.AXIS_CHANGED, axis, value)
 
-    def _check_events(self, event_type: EventType, data: ControllerButton | ControllerAxis, value: float=None) -> None:
+    def _check_events(
+        self, event_type: EventType, data: ControllerButton | ControllerAxis, value: float = None
+    ) -> None:
         key = (event_type, data)
         if key not in self._listeners:
             return
@@ -186,7 +186,7 @@ def _check_events(self, event_type: EventType, data: ControllerButton | Controll
         for callback in self._listeners[key]:
             callback(event_type, data)
 
-    def _start_long_press_timer(self, button: ControllerButton, timeout: float=1.5) -> None:
+    def _start_long_press_timer(self, button: ControllerButton, timeout: float = 1.5) -> None:
         def timer_callback() -> None:
             if self._buttons[button]:
                 self._check_events(EventType.LONG_PRESS, button)

src/lane_assist/image_manipulation/image_stitch/__init__.py

@@ -10,8 +10,9 @@
 MAX_WIDTH = 1280
 
 MASK_OFFSET = np.array([[1, 1], [1, -1], [-1, -1], [-1, 1]])
-PTS_ORIGINAL = np.float32([[MIN_WIDTH, MIN_HEIGHT], [MIN_WIDTH, MAX_HEIGHT],
-                           [MAX_WIDTH, MAX_HEIGHT], [MAX_WIDTH, MIN_HEIGHT]])
+PTS_ORIGINAL = np.float32(
+    [[MIN_WIDTH, MIN_HEIGHT], [MIN_WIDTH, MAX_HEIGHT], [MAX_WIDTH, MAX_HEIGHT], [MAX_WIDTH, MIN_HEIGHT]]
+)
 
 RATIOS_LEFT = np.float32([[0, 1.0055555], [0.36197916, 2.6185186], [1.6765625, 0.7537037], [1.5010417, 0]])
 RATIOS_RIGHT = np.float32([[0.18125, 0], [0, 0.74907407], [1.28125, 2.55833333], [1.66770833, 0.99722222]])
@@ -47,20 +48,22 @@ def relative_to_absolute(x: float, y: float, width: float, height: float) -> tup
     return x * width, y * height
 
 
-def get_ltbr(x: int, y: int, width: int, height: int) -> tuple[int, int, int, int]:
+def get_ltbr(x: float, y: float, width: int, height: int) -> tuple[int, int, int, int]:
     """Get the left, top, right, and bottom coordinates of the image based on xywh."""
-    return (int(round(x - (width / 2))),
-            int(round(y - (height / 2))),
-            int(round(x + (width / 2))),
-            int(round(y + (height / 2))))
+    return (
+        int(round(x - (width / 2))),
+        int(round(y - (height / 2))),
+        int(round(x + (width / 2))),
+        int(round(y + (height / 2))),
+    )
 
 
 def warp_image(image: np.ndarray, matrix: np.ndarray, width: int, height: int) -> np.ndarray:
     """Warp the image based on the transformation matrix."""
     return cv2.warpPerspective(image, matrix, (width, height), flags=cv2.INTER_LINEAR)
 
 
-def merge_image(base:np.ndarray, overlay:np.ndarray, x1:int, y1:int, x2:int, y2:int) -> np.ndarray:
+def merge_image(base: np.ndarray, overlay: np.ndarray, x1: int, y1: int, x2: int, y2: int) -> np.ndarray:
     """Merge two images."""
     base[y1:y2, x1:x2] = overlay
     return base
@@ -97,11 +100,22 @@ def stitch_images(left: np.ndarray, center: np.ndarray, right: np.ndarray) -> np
     return merge_image(result, center, cx1, cy1, cx2, cy2)
 
 
+def adjust_gamma(image: np.ndarray, gamma: float = 1.0) -> np.ndarray:
+    """Adjust the gamma of the image."""
+    inv_gamma = 1.0 / gamma
+    table = np.array([((i / 255.0) ** inv_gamma) * 255 for i in np.arange(0, 256)]).astype("uint8")
+
+    return cv2.LUT(image, table)
+
+
 if __name__ == "__main__":
     # Load images
-    center_img = cv2.imread("images/crossing/center.jpg")
-    left_img = cv2.imread("images/crossing/left.jpg")
-    right_img = cv2.imread("images/crossing/right.jpg")
+    center_img = cv2.imread("../../../../resources/images/stopline/center.jpg")
+    left_img = cv2.imread("../../../../resources/images/stopline/left.jpg")
+    right_img = cv2.imread("../../../../resources/images/stopline/right.jpg")
+
+    left_img = adjust_gamma(left_img, 0.62)
+    right_img = adjust_gamma(right_img, 0.62)
 
     # Write result
     result_img = stitch_images(left_img, center_img, right_img)

src/lane_assist/image_manipulation/top_down_transfrom/__init__.py

@@ -0,0 +1,42 @@
+import cv2
+import numpy as np
+
+from src.utils.image import cut_image
+
+
+def topdown(image: np.ndarray) -> np.ndarray:
+    """Transform stitched image to top-down view."""
+    if image is None:
+        raise ValueError("Error: Unable to load image")
+
+    pts = np.array([[55, 900], [1841, 253], [2067, 253], [3861, 900]], dtype=np.float32)
+    ipm_pts = np.array([[780, 450], [800, 1100], [600, 1100], [620, 450]], dtype=np.float32)
+    ipm_matrix = cv2.getPerspectiveTransform(pts, ipm_pts)
+    ipm = cv2.warpPerspective(image, ipm_matrix, (image.shape[1], image.shape[0]), flags=cv2.INTER_LINEAR)
+    ipm = cut_image(ipm, 300, 450, 800, 900)
+    ipm = cv2.rotate(ipm, cv2.ROTATE_90_CLOCKWISE)
+    return cv2.rotate(ipm, cv2.ROTATE_90_CLOCKWISE)
+
+
+if __name__ == "__main__":
+    # load stitched image
+    image = cv2.imread("result.jpg")
+
+    # time the topdown function and print the Iterations per second
+    import time
+
+    start = time.time()
+    for _ in range(500):
+        topdown(image)
+    end = time.time()
+    fps = 500 / (end - start)
+
+    # convert to grayscale
+    gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
+
+    # time again
+    start = time.time()
+    for _ in range(500):
+        topdown(gray)
+    end = time.time()
+    fps_gray = 500 / (end - start)

src/lane_assist/line_detection/__init__.py

@@ -0,0 +1,62 @@
+import cv2
+import numpy as np
+
+from lane_assist.image_manipulation.top_down_transfrom import topdown
+from lane_assist.line_detection.line import Line, LineType
+from lane_assist.line_detection.window import Window
+from lane_assist.line_detection.window_search import window_search
+from src.utils.image import list_images
+
+
+def get_lines(image: np.ndarray) -> list[Line]:
+    """Get the lines in the image.
+
+    This function will take an image and return the lines in the image.
+    the image shoulb be stitched and not top down
+    """
+    gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
+    top_down = topdown(gray)
+    white = cv2.inRange(top_down, 200, 255)
+    blurred = cv2.GaussianBlur(white, (15, 15), 0)
+    return window_search(blurred, 50)
+
+
+def main() -> None:
+    """Example usage of the line detection.
+
+    this function is used for testing the line detection algorithm.
+    this is done by taking a few images and drawing the lines on the topdown image.
+    """
+    test_images = [
+        cv2.imread("../../../tests/line_detection/images/corner.jpg"),
+        cv2.imread("../../../tests/line_detection/images/straight.jpg"),
+        cv2.imread("../../../tests/line_detection/images/crossing.jpg"),
+        cv2.imread("../../../tests/line_detection/images/stopline.jpg"),
+    ]
+
+
+    colours = {
+        LineType.SOLID: (255, 0, 0),  # red
+        LineType.DASHED: (0, 255, 0),  # green
+        LineType.STOP: (0, 0, 255),  # blue
+    }
+
+    final_images = []
+    # convert the images, so we can find the lines
+    for img in test_images:
+        lines = get_lines(img)
+        td_img = topdown(img)  # convert too topdown to draw the lines
+
+        # draw the points on the topdown image
+        for line in lines:
+            for point in line.points:
+                colour = colours[line.line_type]
+                cv2.circle(td_img, (point[0], point[1]), 10, colour, -1)
+
+        final_images.append(td_img)
+
+    list_images(final_images, rows=4, cols=2)
+
+
+if __name__ == "__main__":
+    main()