From a9212b8bd397b091ac80e7b42c74667f8bbe5549 Mon Sep 17 00:00:00 2001 From: Vaibhav Hariani Date: Sun, 15 Sep 2024 12:39:49 -0400 Subject: [PATCH 01/11] added individual follower class definition --- components/lane_follower/src/individual_follower.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 components/lane_follower/src/individual_follower.py diff --git a/components/lane_follower/src/individual_follower.py b/components/lane_follower/src/individual_follower.py new file mode 100644 index 000000000..e69de29bb From f8c5ad74bb21b24935af3202e634e5437d158da9 Mon Sep 17 00:00:00 2001 From: Vaibhav Hariani Date: Sun, 15 Sep 2024 12:50:49 -0400 Subject: [PATCH 02/11] added constants and property for binary warp --- .../lane_follower/src/individual_follower.py | 39 +++++++++++++++++++ 1 file changed, 39 insertions(+) diff --git a/components/lane_follower/src/individual_follower.py b/components/lane_follower/src/individual_follower.py index e69de29bb..dfb4c782a 100644 --- a/components/lane_follower/src/individual_follower.py +++ b/components/lane_follower/src/individual_follower.py @@ -0,0 +1,39 @@ +# Lane Follower for a single camera +# Applies a perspective transform to an image, and isolates lane lines +import numpy as np +import math +import cv2 + + +class Individual_Follower: + NWINDOWS = 9 + MARGIN = 100 + MINIPIX = 40 + + def __init__(self, fit, binary_warped): + self.fit = fit + self._binary_warped = binary_warped + self.histogram = np.sum( + binary_warped[binary_warped.shape[0] // 2 :, :], axis=0 + ) + + @property + def _binary_warped(self, value): + self._binary_warped = value + self.histogram = np.sum( + self.binary_warped[self.binary_warped.shape[0] // 2 :, :], axis=0 + ) + + def Plot_Line(self): + out_img = ( + np.dstack( + (self._binary_warped, self._binary_warped, self._binary_warped) + ) + * 255 + ) + base = np.argmax(self.histogram[:]) + ##lines for pre-commit checks: These should be removed prior to merge + math + cv2 + out_img + base From d0b622321040ceb0cdcc41fd5fe56fc0a3e2f76e Mon Sep 17 00:00:00 2001 From: Vaibhav Hariani Date: Sun, 15 Sep 2024 13:11:59 -0400 Subject: [PATCH 03/11] added window iteration and drawing --- .../lane_follower/src/individual_follower.py | 72 +++++++++++++++---- 1 file changed, 58 insertions(+), 14 deletions(-) diff --git a/components/lane_follower/src/individual_follower.py b/components/lane_follower/src/individual_follower.py index dfb4c782a..d0c4ea1cf 100644 --- a/components/lane_follower/src/individual_follower.py +++ b/components/lane_follower/src/individual_follower.py @@ -5,35 +5,79 @@ import cv2 -class Individual_Follower: +class IndividualFollower: + ##Constants for how we process images + ##These should be parametrized and better defined at a later date: as part of testing NWINDOWS = 9 - MARGIN = 100 + SEARCH_WIDTH = 100 MINIPIX = 40 - def __init__(self, fit, binary_warped): - self.fit = fit - self._binary_warped = binary_warped - self.histogram = np.sum( - binary_warped[binary_warped.shape[0] // 2 :, :], axis=0 - ) + ##Rendering Parameters + BOX_COLOR = (0, 255, 0) + DRAW_THICKNESS = 2 + def __init__(self): + ##self.fit is set in Plot_Line + self.fit = None + ##These two are set below + self._binary_warped = None + self.histogram = None + + ##Need to determine if these should remain properties + ##Or if they should be passed as arguments to Plot_Line @property - def _binary_warped(self, value): + def _binary_warped(self, value: np.ndarray): self._binary_warped = value self.histogram = np.sum( self.binary_warped[self.binary_warped.shape[0] // 2 :, :], axis=0 ) - def Plot_Line(self): + def plot_line(self): + + if self.binary_warped is None: + raise Exception("no binary warp specified") + + ##Image to visualize output out_img = ( np.dstack( (self._binary_warped, self._binary_warped, self._binary_warped) ) * 255 ) - base = np.argmax(self.histogram[:]) + + window_bases = np.argmax(self.histogram[:]) + + window_height = np.int32( + self._binary_warped.shape[0] / IndividualFollower.NWINDOWS + ) + + lane_inds = [] + + nonzero_pixels = self._binary_warped.nonzero() + nonzero_y = np.array(nonzero_pixels[0]) + nonzero_x = np.array(nonzero_pixels[1]) + current = window_bases + + empty_windows = 0 + for window in range(IndividualFollower.NWINDOWS): + window_dims = window * window_height + win_y_upper = self._binary_warped.shape[0] - window_dims + # One window height lower than win_y_higher + win_y_lower = win_y_upper - window_height + win_x_lower = current - IndividualFollower.SEARCH_WIDTH + win_x_upper = current + IndividualFollower.SEARCH_WIDTH + + cv2.rectangle( + out_img, + (win_x_lower, win_y_lower), + (win_x_upper, win_y_upper), + IndividualFollower.BOX_COLOR, + IndividualFollower.DRAW_THICKNESS, + ) + ##lines for pre-commit checks: These should be removed prior to merge + lane_inds + nonzero_x + nonzero_y + empty_windows math - cv2 - out_img - base From 187bd1bb96d7852821045d6ce641b4b1986c0bad Mon Sep 17 00:00:00 2001 From: Vaibhav Hariani Date: Sun, 15 Sep 2024 13:47:32 -0400 Subject: [PATCH 04/11] full refactoring of individual follower --- .../lane_follower/src/individual_follower.py | 139 ++++++++++++++---- 1 file changed, 114 insertions(+), 25 deletions(-) diff --git a/components/lane_follower/src/individual_follower.py b/components/lane_follower/src/individual_follower.py index d0c4ea1cf..54014cb29 100644 --- a/components/lane_follower/src/individual_follower.py +++ b/components/lane_follower/src/individual_follower.py @@ -6,25 +6,28 @@ class IndividualFollower: - ##Constants for how we process images - ##These should be parametrized and better defined at a later date: as part of testing + # Constants for how we process images + # These should be parametrized and better defined at a later date: as part of testing NWINDOWS = 9 SEARCH_WIDTH = 100 - MINIPIX = 40 + MINPIXELS = 40 + LANE_POLY_SIZE = 2 - ##Rendering Parameters + # Rendering Parameters BOX_COLOR = (0, 255, 0) + LANE_COLOR = (255, 0, 0) + DRAW_THICKNESS = 2 def __init__(self): - ##self.fit is set in Plot_Line + # self.fit is set in Plot_Line self.fit = None - ##These two are set below + # These two are set below self._binary_warped = None self.histogram = None - ##Need to determine if these should remain properties - ##Or if they should be passed as arguments to Plot_Line + # Need to determine if these should remain properties + # Or if they should be passed as arguments to Plot_Line @property def _binary_warped(self, value: np.ndarray): self._binary_warped = value @@ -37,7 +40,7 @@ def plot_line(self): if self.binary_warped is None: raise Exception("no binary warp specified") - ##Image to visualize output + # Image to visualize output out_img = ( np.dstack( (self._binary_warped, self._binary_warped, self._binary_warped) @@ -45,8 +48,6 @@ def plot_line(self): * 255 ) - window_bases = np.argmax(self.histogram[:]) - window_height = np.int32( self._binary_warped.shape[0] / IndividualFollower.NWINDOWS ) @@ -56,28 +57,116 @@ def plot_line(self): nonzero_pixels = self._binary_warped.nonzero() nonzero_y = np.array(nonzero_pixels[0]) nonzero_x = np.array(nonzero_pixels[1]) - current = window_bases empty_windows = 0 + lane_base = np.argmax(self.histogram[:]) for window in range(IndividualFollower.NWINDOWS): window_dims = window * window_height win_y_upper = self._binary_warped.shape[0] - window_dims # One window height lower than win_y_higher win_y_lower = win_y_upper - window_height - win_x_lower = current - IndividualFollower.SEARCH_WIDTH - win_x_upper = current + IndividualFollower.SEARCH_WIDTH + win_x_lower = lane_base - IndividualFollower.SEARCH_WIDTH + win_x_upper = lane_base + IndividualFollower.SEARCH_WIDTH + + lower_coords = (win_x_lower, win_y_lower) + upper_coords = (win_x_upper, win_y_lower) + cv2.rectangle( + out_img, + lower_coords, + upper_coords, + IndividualFollower.BOX_COLOR, + IndividualFollower.DRAW_THICKNESS, + ) + + white_pix_inds = ( + (nonzero_y >= win_y_lower) + & (nonzero_y < win_y_upper) + & (nonzero_x >= win_x_lower) + & (nonzero_x >= win_x_upper) + ).nonzero_pixels()[0] + + # This should likely be moved into the if statement: leaving for now + lane_inds.append(white_pix_inds) + if len(white_pix_inds) > IndividualFollower.MINPIXELS: + # np.mean will return a float: We need an exact value + lane_base = np.int32(np.mean(nonzero_x[white_pix_inds])) + else: + empty_windows += 1 + + if len(lane_inds) == 0: + return None, 0, 0, 0 + lane_array = np.concatenate(lane_inds) + lane_x_pos = nonzero_x[lane_array] + lane_y_pos = nonzero_y[lane_array] + + # I don't believe this if statement is necessary, need to test + if lane_x_pos.any() and lane_y_pos.any(): + self._fit = np.polyfit( + lane_y_pos, lane_x_pos, IndividualFollower.LANE_POLY_SIZE + ) - cv2.rectangle( + plotting_coordinates = np.linspace( + 0, + self._binary_warped.shape[0] - 1, + self._binary_warped.shape[1], + ) + + polynomial = np.polyval(self._fit, plotting_coordinates) + out_img[nonzero_y[lane_array], nonzero_x[lane_array]] = ( + IndividualFollower.LANE_COLOR + ) + + ##Generates the search window area + window_img = np.zeros_like(out_img) + ##These lines should be broken up accordingly: They render the search area + line_window1 = np.array( + [ + np.transpose( + np.vstack( + [ + polynomial - IndividualFollower.SEARCH_WIDTH, + plotting_coordinates, + ] + ) + ) + ] + ) + line_window2 = np.array( + [ + np.transpose( + np.vstack( + [ + polynomial + IndividualFollower.SEARCH_WIDTH, + plotting_coordinates, + ] + ) + ) + ] + ) + line_pts = np.hstack((line_window1, line_window2)) + cv2.fillPoly( + window_img, np.int_([line_pts]), color=IndividualFollower.BOX_COLOR + ) + line_pts = np.array( + [np.transpose(np.vstack([polynomial, plotting_coordinates]))], + dtype=np.int32, + ) + cv2.polylines( out_img, - (win_x_lower, win_y_lower), - (win_x_upper, win_y_upper), - IndividualFollower.BOX_COLOR, - IndividualFollower.DRAW_THICKNESS, + line_pts, + isClosed=False, + color=IndividualFollower.LANE_COLOR, + thickness=IndividualFollower.DRAW_THICKNESS, ) - ##lines for pre-commit checks: These should be removed prior to merge - lane_inds - nonzero_x - nonzero_y - empty_windows - math + # Calculating heading error by converting lane polynomial into line + ##TODO: Make this use the furthest found box. This way, we'll use the most accurate heading + y_lower = 0 + y_upper = (IndividualFollower.NWINDOWS + 1) * window_height + slope = np.polyval(self.fit, y_lower) - np.polyval( + self.fit, y_upper + ) / (y_upper - y_lower) + heading = math.atan(slope) + result = cv2.addWeighted(out_img, 1, window_img, 0.3, 0) + # TODO: Determine what result really is, and annotate efficiently + return result, empty_windows, heading, slope From 8c363c853914900e467db0eead3a9f61c1b48350 Mon Sep 17 00:00:00 2001 From: Vaibhav Hariani Date: Sat, 28 Sep 2024 20:40:55 -0400 Subject: [PATCH 05/11] created lf_const.toml, a pyproject.toml, and added type hinting --- .../lane_follower/src/individual_follower.py | 29 +++++++++++-------- components/lane_follower/src/lf_const.toml | 8 +++++ components/lane_follower/src/pyproject.toml | 12 ++++++++ 3 files changed, 37 insertions(+), 12 deletions(-) create mode 100644 components/lane_follower/src/lf_const.toml create mode 100644 components/lane_follower/src/pyproject.toml diff --git a/components/lane_follower/src/individual_follower.py b/components/lane_follower/src/individual_follower.py index 54014cb29..e85abf1bf 100644 --- a/components/lane_follower/src/individual_follower.py +++ b/components/lane_follower/src/individual_follower.py @@ -1,13 +1,17 @@ -# Lane Follower for a single camera -# Applies a perspective transform to an image, and isolates lane lines -import numpy as np import math + import cv2 +import numpy as np + +from einops import rearrange + + +class if_result: + pass class IndividualFollower: - # Constants for how we process images - # These should be parametrized and better defined at a later date: as part of testing + # Get these from .toml or on intiialization NWINDOWS = 9 SEARCH_WIDTH = 100 MINPIXELS = 40 @@ -23,25 +27,25 @@ def __init__(self): # self.fit is set in Plot_Line self.fit = None # These two are set below - self._binary_warped = None - self.histogram = None + self._binary_warped = None | np.ndarray + self._histogram = None | np.ndarray # Need to determine if these should remain properties # Or if they should be passed as arguments to Plot_Line @property def _binary_warped(self, value: np.ndarray): self._binary_warped = value - self.histogram = np.sum( + self._histogram = np.sum( self.binary_warped[self.binary_warped.shape[0] // 2 :, :], axis=0 ) - def plot_line(self): - - if self.binary_warped is None: + def plot_line(self) -> if_result: + if not self._binary_warped: raise Exception("no binary warp specified") # Image to visualize output - out_img = ( + out_img = rearrange() + ( np.dstack( (self._binary_warped, self._binary_warped, self._binary_warped) ) @@ -60,6 +64,7 @@ def plot_line(self): empty_windows = 0 lane_base = np.argmax(self.histogram[:]) + for window in range(IndividualFollower.NWINDOWS): window_dims = window * window_height win_y_upper = self._binary_warped.shape[0] - window_dims diff --git a/components/lane_follower/src/lf_const.toml b/components/lane_follower/src/lf_const.toml new file mode 100644 index 000000000..4ac2f410f --- /dev/null +++ b/components/lane_follower/src/lf_const.toml @@ -0,0 +1,8 @@ +[Individual_Follower] +NWINDOWS = 9 +SEARCH_WIDTH = 100 +MINPIXELS = 40 +LANE_POLY_SIZE = 2 +BOX_COLOR = [0, 255, 0] +LANE_COLOR = [255, 0, 0] +DRAW_THICKNESS = 2 diff --git a/components/lane_follower/src/pyproject.toml b/components/lane_follower/src/pyproject.toml new file mode 100644 index 000000000..e66554a32 --- /dev/null +++ b/components/lane_follower/src/pyproject.toml @@ -0,0 +1,12 @@ +[project] +version = '1.0' +name = "individual-follower" +dependencies = [ + "cv2", + "numpy", +] +authors = [ + {name = "Vaibhav Hariani"}, + {name = "Isaiah Rivera"}, +] +description = "IGVC lane follower module for a single camera" From 550aeecabfa17e0cb265483a871e999fb1430e52 Mon Sep 17 00:00:00 2001 From: Vaibhav Hariani Date: Sat, 28 Sep 2024 21:06:10 -0400 Subject: [PATCH 06/11] Added result class to simplify individual follower. Seriously need to reconsider structure of line_window1 & line_window2: These lines are only for rendering, but seem to be half running in the loop and half out. This doesn't make a lot of sense? I think they can be fully replaced with the resultant np.polyval --- .../lane_follower/src/individual_follower.py | 115 +++++++++++------- components/lane_follower/src/pyproject.toml | 4 - 2 files changed, 74 insertions(+), 45 deletions(-) diff --git a/components/lane_follower/src/individual_follower.py b/components/lane_follower/src/individual_follower.py index e85abf1bf..8fa476b1f 100644 --- a/components/lane_follower/src/individual_follower.py +++ b/components/lane_follower/src/individual_follower.py @@ -3,10 +3,16 @@ import cv2 import numpy as np -from einops import rearrange +from einops import repeat -class if_result: +class ifResult: + def __init__(self): + self.result_img = None + self.empty_windows = 0 + self.heading = 0 + self.slope = 0 + pass @@ -27,8 +33,8 @@ def __init__(self): # self.fit is set in Plot_Line self.fit = None # These two are set below - self._binary_warped = None | np.ndarray - self._histogram = None | np.ndarray + self._binary_warped = None + self._histogram = None # Need to determine if these should remain properties # Or if they should be passed as arguments to Plot_Line @@ -39,22 +45,26 @@ def _binary_warped(self, value: np.ndarray): self.binary_warped[self.binary_warped.shape[0] // 2 :, :], axis=0 ) - def plot_line(self) -> if_result: + def plot_line(self) -> ifResult: if not self._binary_warped: raise Exception("no binary warp specified") # Image to visualize output - out_img = rearrange() - ( - np.dstack( - (self._binary_warped, self._binary_warped, self._binary_warped) - ) - * 255 - ) + out_img = repeat(self._binary_warped, 'h w -> h w c', repeat=3) * 255 + + ##These outputs need to be confirmed compatible + # out_img = ( + # np.dstack( + # self._binary_warped, self._binary_warped, self._binary_warped + # ) + # * 255 + # ) window_height = np.int32( self._binary_warped.shape[0] / IndividualFollower.NWINDOWS ) + ##Create result class: + result = ifResult() lane_inds = [] @@ -99,7 +109,8 @@ def plot_line(self) -> if_result: empty_windows += 1 if len(lane_inds) == 0: - return None, 0, 0, 0 + return result + lane_array = np.concatenate(lane_inds) lane_x_pos = nonzero_x[lane_array] lane_y_pos = nonzero_y[lane_array] @@ -124,34 +135,55 @@ def plot_line(self) -> if_result: ##Generates the search window area window_img = np.zeros_like(out_img) ##These lines should be broken up accordingly: They render the search area - line_window1 = np.array( + + window_1 = np.vstack( [ - np.transpose( - np.vstack( - [ - polynomial - IndividualFollower.SEARCH_WIDTH, - plotting_coordinates, - ] - ) - ) + polynomial - IndividualFollower.SEARCH_WIDTH, + plotting_coordinates, ] - ) - line_window2 = np.array( - [ - np.transpose( - np.vstack( - [ - polynomial + IndividualFollower.SEARCH_WIDTH, - plotting_coordinates, - ] - ) - ) - ] - ) - line_pts = np.hstack((line_window1, line_window2)) + ).T + + window_2 = np.vstack( + [ + polynomial - IndividualFollower.SEARCH_WIDTH, + plotting_coordinates, + ] + ).T + + # These replacements also need to be tested + + # line_window1 = + # np.array( + # [ + # np.transpose( + # np.vstack( + # [ + # polynomial - IndividualFollower.SEARCH_WIDTH, + # plotting_coordinates, + # ] + # ) + # ) + # ] + # ) + # line_window2 = np.array( + # [ + # np.transpose( + # np.vstack( + # [ + # polynomial + IndividualFollower.SEARCH_WIDTH, + # plotting_coordinates, + # ] + # ) + # ) + # ] + # ) + + line_pts = np.hstack((window_1, window_2)) cv2.fillPoly( window_img, np.int_([line_pts]), color=IndividualFollower.BOX_COLOR ) + # This seems to be a lot of numpy fluff that should be replaced with einops + # or removed altogether. line_pts = np.array( [np.transpose(np.vstack([polynomial, plotting_coordinates]))], dtype=np.int32, @@ -168,10 +200,11 @@ def plot_line(self) -> if_result: ##TODO: Make this use the furthest found box. This way, we'll use the most accurate heading y_lower = 0 y_upper = (IndividualFollower.NWINDOWS + 1) * window_height - slope = np.polyval(self.fit, y_lower) - np.polyval( + + result.slope = np.polyval(self.fit, y_lower) - np.polyval( self.fit, y_upper ) / (y_upper - y_lower) - heading = math.atan(slope) - result = cv2.addWeighted(out_img, 1, window_img, 0.3, 0) - # TODO: Determine what result really is, and annotate efficiently - return result, empty_windows, heading, slope + result.result_img = cv2.addWeighted(out_img, 1, window_img, 0.3, 0) + result.heading = math.atan(result.slope) + + return result diff --git a/components/lane_follower/src/pyproject.toml b/components/lane_follower/src/pyproject.toml index e66554a32..15528d718 100644 --- a/components/lane_follower/src/pyproject.toml +++ b/components/lane_follower/src/pyproject.toml @@ -1,10 +1,6 @@ [project] version = '1.0' name = "individual-follower" -dependencies = [ - "cv2", - "numpy", -] authors = [ {name = "Vaibhav Hariani"}, {name = "Isaiah Rivera"}, From c7d7692582702e7ff086f00d7c4d1c497188568d Mon Sep 17 00:00:00 2001 From: Vaibhav Hariani Date: Sat, 28 Sep 2024 21:16:32 -0400 Subject: [PATCH 07/11] Tried removing some of the numpy vomit --- .../lane_follower/src/individual_follower.py | 69 +++---------------- 1 file changed, 8 insertions(+), 61 deletions(-) diff --git a/components/lane_follower/src/individual_follower.py b/components/lane_follower/src/individual_follower.py index 8fa476b1f..3b0950a88 100644 --- a/components/lane_follower/src/individual_follower.py +++ b/components/lane_follower/src/individual_follower.py @@ -114,20 +114,12 @@ def plot_line(self) -> ifResult: lane_array = np.concatenate(lane_inds) lane_x_pos = nonzero_x[lane_array] lane_y_pos = nonzero_y[lane_array] - - # I don't believe this if statement is necessary, need to test + # TODO: test if this statement is necessary if lane_x_pos.any() and lane_y_pos.any(): self._fit = np.polyfit( lane_y_pos, lane_x_pos, IndividualFollower.LANE_POLY_SIZE ) - plotting_coordinates = np.linspace( - 0, - self._binary_warped.shape[0] - 1, - self._binary_warped.shape[1], - ) - - polynomial = np.polyval(self._fit, plotting_coordinates) out_img[nonzero_y[lane_array], nonzero_x[lane_array]] = ( IndividualFollower.LANE_COLOR ) @@ -136,61 +128,16 @@ def plot_line(self) -> ifResult: window_img = np.zeros_like(out_img) ##These lines should be broken up accordingly: They render the search area - window_1 = np.vstack( - [ - polynomial - IndividualFollower.SEARCH_WIDTH, - plotting_coordinates, - ] - ).T - - window_2 = np.vstack( - [ - polynomial - IndividualFollower.SEARCH_WIDTH, - plotting_coordinates, - ] - ).T - - # These replacements also need to be tested - - # line_window1 = - # np.array( - # [ - # np.transpose( - # np.vstack( - # [ - # polynomial - IndividualFollower.SEARCH_WIDTH, - # plotting_coordinates, - # ] - # ) - # ) - # ] - # ) - # line_window2 = np.array( - # [ - # np.transpose( - # np.vstack( - # [ - # polynomial + IndividualFollower.SEARCH_WIDTH, - # plotting_coordinates, - # ] - # ) - # ) - # ] - # ) + # Create linearly spaced points at each height, evaluate polynomial, create coordinates + x_val = np.arange(0, window_height, step=5).T + y_val = np.polyval(self.fit, x_val) + coords = np.concatenate((x_val, y_val), axis=0) + + ##TODO: Test if this approach works - line_pts = np.hstack((window_1, window_2)) - cv2.fillPoly( - window_img, np.int_([line_pts]), color=IndividualFollower.BOX_COLOR - ) - # This seems to be a lot of numpy fluff that should be replaced with einops - # or removed altogether. - line_pts = np.array( - [np.transpose(np.vstack([polynomial, plotting_coordinates]))], - dtype=np.int32, - ) cv2.polylines( out_img, - line_pts, + coords, isClosed=False, color=IndividualFollower.LANE_COLOR, thickness=IndividualFollower.DRAW_THICKNESS, From a4a018f6e4345c11d076e652c8400174c1315c34 Mon Sep 17 00:00:00 2001 From: Vaibhav Hariani Date: Sun, 13 Oct 2024 23:24:28 -0400 Subject: [PATCH 08/11] Added .toml, piping constants into individual folllower from there for better readability --- .../{lf_const.toml => Follower_Consts.toml} | 2 + .../lane_follower/src/individual_follower.py | 45 ++++++++----------- 2 files changed, 20 insertions(+), 27 deletions(-) rename components/lane_follower/src/{lf_const.toml => Follower_Consts.toml} (76%) diff --git a/components/lane_follower/src/lf_const.toml b/components/lane_follower/src/Follower_Consts.toml similarity index 76% rename from components/lane_follower/src/lf_const.toml rename to components/lane_follower/src/Follower_Consts.toml index 4ac2f410f..19ec3d466 100644 --- a/components/lane_follower/src/lf_const.toml +++ b/components/lane_follower/src/Follower_Consts.toml @@ -1,3 +1,5 @@ +# Constants for the individual follower module + [Individual_Follower] NWINDOWS = 9 SEARCH_WIDTH = 100 diff --git a/components/lane_follower/src/individual_follower.py b/components/lane_follower/src/individual_follower.py index 3b0950a88..d0a0cef67 100644 --- a/components/lane_follower/src/individual_follower.py +++ b/components/lane_follower/src/individual_follower.py @@ -2,7 +2,7 @@ import cv2 import numpy as np - +import toml from einops import repeat @@ -17,19 +17,10 @@ def __init__(self): class IndividualFollower: - # Get these from .toml or on intiialization - NWINDOWS = 9 - SEARCH_WIDTH = 100 - MINPIXELS = 40 - LANE_POLY_SIZE = 2 - - # Rendering Parameters - BOX_COLOR = (0, 255, 0) - LANE_COLOR = (255, 0, 0) - DRAW_THICKNESS = 2 + def __init__(self, toml_path='Follower_Consts.toml'): - def __init__(self): + self.consts = toml.load(toml_path)["Individual_Follower"] # self.fit is set in Plot_Line self.fit = None # These two are set below @@ -51,7 +42,6 @@ def plot_line(self) -> ifResult: # Image to visualize output out_img = repeat(self._binary_warped, 'h w -> h w c', repeat=3) * 255 - ##These outputs need to be confirmed compatible # out_img = ( # np.dstack( @@ -60,9 +50,9 @@ def plot_line(self) -> ifResult: # * 255 # ) - window_height = np.int32( - self._binary_warped.shape[0] / IndividualFollower.NWINDOWS - ) + NWINDOWS = self.consts['NWINDOWS'] + + window_height = np.int32(self._binary_warped.shape[0] / NWINDOWS) ##Create result class: result = ifResult() @@ -75,13 +65,14 @@ def plot_line(self) -> ifResult: empty_windows = 0 lane_base = np.argmax(self.histogram[:]) - for window in range(IndividualFollower.NWINDOWS): + SEARCH_WIDTH = self.consts['SEARCH_WIDTH'] + for window in range(NWINDOWS): window_dims = window * window_height win_y_upper = self._binary_warped.shape[0] - window_dims # One window height lower than win_y_higher win_y_lower = win_y_upper - window_height - win_x_lower = lane_base - IndividualFollower.SEARCH_WIDTH - win_x_upper = lane_base + IndividualFollower.SEARCH_WIDTH + win_x_lower = lane_base - SEARCH_WIDTH + win_x_upper = lane_base + SEARCH_WIDTH lower_coords = (win_x_lower, win_y_lower) upper_coords = (win_x_upper, win_y_lower) @@ -89,8 +80,8 @@ def plot_line(self) -> ifResult: out_img, lower_coords, upper_coords, - IndividualFollower.BOX_COLOR, - IndividualFollower.DRAW_THICKNESS, + self.consts["BOX_COLOR"], + self.consts["DRAW_THICKNESS"], ) white_pix_inds = ( @@ -102,7 +93,7 @@ def plot_line(self) -> ifResult: # This should likely be moved into the if statement: leaving for now lane_inds.append(white_pix_inds) - if len(white_pix_inds) > IndividualFollower.MINPIXELS: + if len(white_pix_inds) > self.consts["MINPIXELS"]: # np.mean will return a float: We need an exact value lane_base = np.int32(np.mean(nonzero_x[white_pix_inds])) else: @@ -117,11 +108,11 @@ def plot_line(self) -> ifResult: # TODO: test if this statement is necessary if lane_x_pos.any() and lane_y_pos.any(): self._fit = np.polyfit( - lane_y_pos, lane_x_pos, IndividualFollower.LANE_POLY_SIZE + lane_y_pos, lane_x_pos, self.consts["LANE_POLY_SIZE"] ) out_img[nonzero_y[lane_array], nonzero_x[lane_array]] = ( - IndividualFollower.LANE_COLOR + self.consts["LANE_COLOR"] ) ##Generates the search window area @@ -139,14 +130,14 @@ def plot_line(self) -> ifResult: out_img, coords, isClosed=False, - color=IndividualFollower.LANE_COLOR, - thickness=IndividualFollower.DRAW_THICKNESS, + color=self.consts["LANE_COLOR"], + thickness=self.consts["DRAW_THICKNESS"], ) # Calculating heading error by converting lane polynomial into line ##TODO: Make this use the furthest found box. This way, we'll use the most accurate heading y_lower = 0 - y_upper = (IndividualFollower.NWINDOWS + 1) * window_height + y_upper = (NWINDOWS + 1) * window_height result.slope = np.polyval(self.fit, y_lower) - np.polyval( self.fit, y_upper From 20d1e0bd420f9f51593da5325346b0d08742a0e2 Mon Sep 17 00:00:00 2001 From: Vaibhav Hariani Date: Sun, 13 Oct 2024 23:40:51 -0400 Subject: [PATCH 09/11] Building indermediary function for code readability --- .../lane_follower/src/individual_follower.py | 66 ++++++++++++++++++- 1 file changed, 63 insertions(+), 3 deletions(-) diff --git a/components/lane_follower/src/individual_follower.py b/components/lane_follower/src/individual_follower.py index d0a0cef67..6012c7a4d 100644 --- a/components/lane_follower/src/individual_follower.py +++ b/components/lane_follower/src/individual_follower.py @@ -36,6 +36,67 @@ def _binary_warped(self, value: np.ndarray): self.binary_warped[self.binary_warped.shape[0] // 2 :, :], axis=0 ) + def get_white_pixels(self, window, window_height, out_img=None): + NWINDOWS = self.consts["NWINDOWS"] + SEARCH_WIDTH = self.consts['SEARCH_WIDTH'] + lane_inds = [] + + nonzero_pixels = self._binary_warped.nonzero() + nonzero_y = np.array(nonzero_pixels[0]) + nonzero_x = np.array(nonzero_pixels[1]) + + empty_windows = 0 + lane_base = np.argmax(self.histogram[:]) + + for window in range(NWINDOWS): + window_dims = window * window_height + win_y_upper = self._binary_warped.shape[0] - window_dims + # One window height lower than win_y_higher + win_y_lower = win_y_upper - window_height + win_x_lower = lane_base - SEARCH_WIDTH + win_x_upper = lane_base + SEARCH_WIDTH + + lower_coords = (win_x_lower, win_y_lower) + upper_coords = (win_x_upper, win_y_lower) + cv2.rectangle( + out_img, + lower_coords, + upper_coords, + self.consts["BOX_COLOR"], + self.consts["DRAW_THICKNESS"], + ) + + white_pix_inds = ( + (nonzero_y >= win_y_lower) + & (nonzero_y < win_y_upper) + & (nonzero_x >= win_x_lower) + & (nonzero_x >= win_x_upper) + ).nonzero_pixels()[0] + + # This should likely be moved into the if statement: leaving for now + lane_inds.append(white_pix_inds) + if len(white_pix_inds) > self.consts["MINPIXELS"]: + # np.mean will return a float: We need an exact value + lane_base = np.int32(np.mean(nonzero_x[white_pix_inds])) + else: + empty_windows += 1 + + # if len(lane_inds) == 0: + # return result + + lane_array = np.concatenate(lane_inds) + lane_x_pos = nonzero_x[lane_array] + lane_y_pos = nonzero_y[lane_array] + # TODO: test if this statement is necessary + if lane_x_pos.any() and lane_y_pos.any(): + self._fit = np.polyfit( + lane_y_pos, lane_x_pos, self.consts["LANE_POLY_SIZE"] + ) + + out_img[nonzero_y[lane_array], nonzero_x[lane_array]] = ( + self.consts["LANE_COLOR"] + ) + def plot_line(self) -> ifResult: if not self._binary_warped: raise Exception("no binary warp specified") @@ -115,9 +176,8 @@ def plot_line(self) -> ifResult: self.consts["LANE_COLOR"] ) - ##Generates the search window area - window_img = np.zeros_like(out_img) - ##These lines should be broken up accordingly: They render the search area + ##Generates the search window area + window_img = np.zeros_like(out_img) # Create linearly spaced points at each height, evaluate polynomial, create coordinates x_val = np.arange(0, window_height, step=5).T From e5fd3d1ec3c9a36a1df7e755f0ac0fe47a77dbc3 Mon Sep 17 00:00:00 2001 From: Vaibhav Hariani Date: Mon, 14 Oct 2024 00:02:14 -0400 Subject: [PATCH 10/11] Broke individual follower up into loop & not loop --- .../lane_follower/src/individual_follower.py | 94 ++++--------------- 1 file changed, 18 insertions(+), 76 deletions(-) diff --git a/components/lane_follower/src/individual_follower.py b/components/lane_follower/src/individual_follower.py index 6012c7a4d..69937b864 100644 --- a/components/lane_follower/src/individual_follower.py +++ b/components/lane_follower/src/individual_follower.py @@ -36,9 +36,12 @@ def _binary_warped(self, value: np.ndarray): self.binary_warped[self.binary_warped.shape[0] // 2 :, :], axis=0 ) - def get_white_pixels(self, window, window_height, out_img=None): + def get_white_pixels(self): + # Returns an output image NWINDOWS = self.consts["NWINDOWS"] SEARCH_WIDTH = self.consts['SEARCH_WIDTH'] + + window_height = np.int32(self._binary_warped.shape[0] / NWINDOWS) lane_inds = [] nonzero_pixels = self._binary_warped.nonzero() @@ -48,59 +51,6 @@ def get_white_pixels(self, window, window_height, out_img=None): empty_windows = 0 lane_base = np.argmax(self.histogram[:]) - for window in range(NWINDOWS): - window_dims = window * window_height - win_y_upper = self._binary_warped.shape[0] - window_dims - # One window height lower than win_y_higher - win_y_lower = win_y_upper - window_height - win_x_lower = lane_base - SEARCH_WIDTH - win_x_upper = lane_base + SEARCH_WIDTH - - lower_coords = (win_x_lower, win_y_lower) - upper_coords = (win_x_upper, win_y_lower) - cv2.rectangle( - out_img, - lower_coords, - upper_coords, - self.consts["BOX_COLOR"], - self.consts["DRAW_THICKNESS"], - ) - - white_pix_inds = ( - (nonzero_y >= win_y_lower) - & (nonzero_y < win_y_upper) - & (nonzero_x >= win_x_lower) - & (nonzero_x >= win_x_upper) - ).nonzero_pixels()[0] - - # This should likely be moved into the if statement: leaving for now - lane_inds.append(white_pix_inds) - if len(white_pix_inds) > self.consts["MINPIXELS"]: - # np.mean will return a float: We need an exact value - lane_base = np.int32(np.mean(nonzero_x[white_pix_inds])) - else: - empty_windows += 1 - - # if len(lane_inds) == 0: - # return result - - lane_array = np.concatenate(lane_inds) - lane_x_pos = nonzero_x[lane_array] - lane_y_pos = nonzero_y[lane_array] - # TODO: test if this statement is necessary - if lane_x_pos.any() and lane_y_pos.any(): - self._fit = np.polyfit( - lane_y_pos, lane_x_pos, self.consts["LANE_POLY_SIZE"] - ) - - out_img[nonzero_y[lane_array], nonzero_x[lane_array]] = ( - self.consts["LANE_COLOR"] - ) - - def plot_line(self) -> ifResult: - if not self._binary_warped: - raise Exception("no binary warp specified") - # Image to visualize output out_img = repeat(self._binary_warped, 'h w -> h w c', repeat=3) * 255 ##These outputs need to be confirmed compatible @@ -111,22 +61,6 @@ def plot_line(self) -> ifResult: # * 255 # ) - NWINDOWS = self.consts['NWINDOWS'] - - window_height = np.int32(self._binary_warped.shape[0] / NWINDOWS) - ##Create result class: - result = ifResult() - - lane_inds = [] - - nonzero_pixels = self._binary_warped.nonzero() - nonzero_y = np.array(nonzero_pixels[0]) - nonzero_x = np.array(nonzero_pixels[1]) - - empty_windows = 0 - lane_base = np.argmax(self.histogram[:]) - - SEARCH_WIDTH = self.consts['SEARCH_WIDTH'] for window in range(NWINDOWS): window_dims = window * window_height win_y_upper = self._binary_warped.shape[0] - window_dims @@ -159,10 +93,8 @@ def plot_line(self) -> ifResult: lane_base = np.int32(np.mean(nonzero_x[white_pix_inds])) else: empty_windows += 1 - if len(lane_inds) == 0: - return result - + return None lane_array = np.concatenate(lane_inds) lane_x_pos = nonzero_x[lane_array] lane_y_pos = nonzero_y[lane_array] @@ -175,17 +107,27 @@ def plot_line(self) -> ifResult: out_img[nonzero_y[lane_array], nonzero_x[lane_array]] = ( self.consts["LANE_COLOR"] ) + return out_img + + def plot_line(self) -> ifResult: + if not self._binary_warped: + raise Exception("no binary warp specified") + + result = ifResult() + + NWINDOWS = self.consts["NWINDOWS"] + window_height = np.int32(self._binary_warped.shape[0] / NWINDOWS) + out_img = self.get_white_pixels() + if out_img is None: + return result ##Generates the search window area window_img = np.zeros_like(out_img) - # Create linearly spaced points at each height, evaluate polynomial, create coordinates x_val = np.arange(0, window_height, step=5).T y_val = np.polyval(self.fit, x_val) coords = np.concatenate((x_val, y_val), axis=0) - ##TODO: Test if this approach works - cv2.polylines( out_img, coords, From d0c80ed3c36e014d703dd2a9b44ab4f10c646573 Mon Sep 17 00:00:00 2001 From: Vaibhav Hariani Date: Mon, 14 Oct 2024 00:19:06 -0400 Subject: [PATCH 11/11] Added Lane Follower constants to .toml, beginning effort to move this full module to dev --- .../lane_follower/src/Follower_Consts.toml | 17 ++++++++++++++++ components/lane_follower/src/lane_follower.py | 20 +++++++++++++++++++ 2 files changed, 37 insertions(+) create mode 100644 components/lane_follower/src/lane_follower.py diff --git a/components/lane_follower/src/Follower_Consts.toml b/components/lane_follower/src/Follower_Consts.toml index 19ec3d466..0c60b0047 100644 --- a/components/lane_follower/src/Follower_Consts.toml +++ b/components/lane_follower/src/Follower_Consts.toml @@ -8,3 +8,20 @@ LANE_POLY_SIZE = 2 BOX_COLOR = [0, 255, 0] LANE_COLOR = [255, 0, 0] DRAW_THICKNESS = 2 + +[Lane_Follower] +GUI = true +# TODO: Move these into a conf +KERNEL = 31 +LANE_TOLERANCE = 10 +MISSING_IMAGE_TOLERANCE = 100 +EMPTY_WINDOWS_THRESHOLD = 3 +OVERFLOW = 1000.0 +FORMAT = [640, 480] +TOLERANCE = 100 +PIXELS_TO_METERS = 260.8269125 +lower_hsv = [100, 0, 220] +upper_hsv = [255, 255, 255] +# Alignment points +bottom_coordinates = [[12, 472], [499, 475]] +top_coordinates = [[90, 8], [435, 24]] diff --git a/components/lane_follower/src/lane_follower.py b/components/lane_follower/src/lane_follower.py new file mode 100644 index 000000000..429ca8901 --- /dev/null +++ b/components/lane_follower/src/lane_follower.py @@ -0,0 +1,20 @@ +import toml +import cv2 + +# import numpy as np + + +class LaneFollower: + + def __init__( + self, odom_sub, cam_left, cam_right, toml_path='Follower_Consts.toml' + ): + self.vidcap_left = cv2.VideoCapture(cam_left) + self.vidcap_right = cv2.VideoCapture(cam_right) + self.const = toml.load(toml_path)["Lane_Follower"] + # LOWER = np.array(self.const['lower_hsv']) + # UPPER = np.array(self.const['upper_hsv']) + # PTS1 = np.float32([tl, bl, tr, br]) + # PTS2 = np.float32([[0, 0], [0, 480], [640, 0], [640, 480]]) + # # Matrix to warp the image for birdseye window + # UNWARP = cv2.getPerspectiveTransform(pts1, pts2)