# Creation Date: 01.03.2022 # Author: Kenan Gömek # This program is for the development of the lane detection algorithm on a windows and Linux machine # It uses images, which were captured previously # V02 detects the lane by using the grayscale-image of the rgb-image from turtle import position import cv2 as cv import numpy as np import os import time import platform # input if platform.system() == "Windows": folder_path=r"images_input" if platform.system() == "Linux": folder_path=r"/home/pi/Desktop/images_input" # Parameters pixels_per_mm = 32/24.25 #[px/mm] for 120 mm camera height for resolution: 416x320 # pixels_per_mm = 71/24.25 #[px/mm] for 120 mm camera height for resolution: 416x320 # pixels_per_mm = 107/24.25 #[px/mm] for 120 mm camera height for resolution: 640x480 # Offset Camera Sensor in Scooty according to Scooty-KS x_offset_camera_mm = 100 # [mm] y_offset_camera_mm = 50 # [mm] x_offset_camera_px = x_offset_camera_mm*pixels_per_mm # [px] y_offset_camera_px = y_offset_camera_mm*pixels_per_mm # [px] # image parameters: width > height image_height = 320 # shape [0] image_width = 416 # shape[1] # calculate center of image [x_0, y_0] = np.array([image_width/2, image_height/2], dtype=np.uint16) threshold_color_detection = 60 # values under this will not be considered as active leds in each color channel # Parameters for LED Detection minDiameter_mm = 3.75 # [mm] minimum diameter of detected blob/LED. Must be minimum >0 ! # maxDiameter_mm = 9 # [mm] maximum diameter of detected blob/LED # Define color numbers to identify the color channels in the matrix with all detected LEDs. No string, because numpy array should stay uint16 color_number_off = 0 color_number_red = 1 color_number_green = 2 color_number_blue = 3 color_number_yellow = 4 color_number_magenta = 5 color_number_cyan = 6 color_number_white = 7 # Parameters for grayscale to binary conversion binary_threshold = 15 # determined by testing and application # the higher threshold is, the smaller the diameter of the led, because more high values are extracted binary_maxval = 255 # values ofer threshold will be set to maxval # Parameters for line fitting for lane construction param = 0 # not used for DIST_L2 reps = 0.001 # Sufficient accuracy for the radius (distance between the coordinate origin and the line). aeps = 0.001 # Sufficient accuracy for the angle. show_opencv_window = True # show opencv window draw_opencv = True # draw lane and so on print_additional_info = True # calculations before start # Filter blobs by Area --> not implemented anymore, because no need, because detection is good and less time for calculation needed # more than good trade off! minDiameter_px = minDiameter_mm*pixels_per_mm # [px] minimum diameter of detected blob/LED # maxDiameter_px = maxDiameter_mm*pixels_per_mm # [px] maximum diameter of detected blob/LED minArea_px2 = np.pi/4*minDiameter_px**2 # min Area of a blob in px^2 # maxArea_px2 = np.pi/4*maxDiameter_px**2 # minArea = minArea_px2 # min Area of a blob in px^2 # params.maxArea = maxArea_px2 # max Area of a blob in px^2. # reasons for not filtering maxArea: motion blur + rolling shutter --> larger Area def points_trafo(detected_LEDs, alpha_rad, dx, dy): """Tranfsform points of LED to lane in KS-LED""" detected_LEDs_trafo = detected_LEDs.copy() # copy, becuase else it is only a pointer detected_LEDs_trafo = detected_LEDs_trafo.astype(np.int16) # avoid integer overflow x_pnts = detected_LEDs_trafo[:,0] y_pnts = detected_LEDs_trafo[:,1] # Translation x1 = x_pnts-dx-x_0 x_trafo = x1 y1 = y_pnts-dy-y_0 y_trafo = y1 # Rotation. Winkel Sensor im UZS, also negativ zu mathematischer definiton --> passive Drehung # cos(-x)=cos(x) # sin(-x)=-sin(x) --> -sin(-x) = - -sin(x) = sin(x) # wegen besserer verständlichkeit nicht verinfachung angewendet x_trafo = np.cos(-alpha_rad)*x1-np.sin(-alpha_rad)*y1 detected_LEDs_trafo[:,0] = x_trafo y_trafo = np.sin(-alpha_rad)*x1+np.cos(-alpha_rad)*y1 detected_LEDs_trafo[:,1] = y_trafo #sort points along lane: x_2, y_2 -axis (KS_LED) detected_LEDs_trafo = detected_LEDs_trafo[detected_LEDs_trafo[:, 0].argsort(kind='quicksort')] return detected_LEDs_trafo def construct_lane(detected_LEDs, img_bgr): """construct the lane""" # This function is partially commented in german, because higher math is used # clearer what is trying to be achieved # get points # xy_pnts = detected_LEDs[:,0:2] # x_pnts = detected_LEDs[:,0] # y_pnts = detected_LEDs[:,1] # approach 2: # fit line through centers of LEDs in KS_0 # DIST_L": the simplest and the fastest least-squares method: the simple euclidean distance [dx, dy, x_2, y_2] = cv.fitLine(detected_LEDs[:,0:2], cv.DIST_L2, param, reps, aeps) # x_2, y_2: point on the line --> mean of leds --> point in the middle of the leds # x2, y2: same as: mean_of_leds = np.mean([x_pnts, y_pnts], 1) alpha_rad = np.arctan2(dy, dx) # calculate angle of line alpha = np.arctan2(dy, dx)*180/np.pi # calculate angle of line # print(f"Lane: dx: {dx}, dy:{dy}, x2:{x_2}, y2:{y_2}, alpha:{alpha}°") if print_additional_info: alpha_print = alpha[0] alpha_print = float("{0:.2f}".format(alpha_print)) print(f"Alpha: {alpha_print}°") # get smallest distance to point an line # Berechnung nach: Repetitorium Höhere Mathematik, Wirth # Gerade: x = a+ t*b # Punkt : OP = p # d = abs(b x (p-a))/(abs(b)) # info: np.array()[:,0] --> gets only array with 1 dimensions with desired values p = np.array([x_0, y_0]) a = np.array([x_2, y_2])[:,0] b = np.array([np.cos(alpha_rad), np.sin(alpha_rad)])[:,0] # Richtungsvektor c = p-a # Betrag von Vektor: np.linalg.norm(vec) cross= np.cross(b, c) d = np.linalg.norm(cross)/np.linalg.norm(b) # distance [px] #print(f"d: {round(d,2)}") # Fußpunkt x_FP=(X_LED, Y_LED) t_0_dot = np.dot(c, b) t_0_norm = (np.linalg.norm(b)**2) t_0 = t_0_dot/t_0_norm [x_LED, y_LED] = (a+t_0*b) if print_additional_info: # convert float32, round and prepare for printing string x_LED_print = float("{0:.2f}".format(x_LED)) y_LED_print = float("{0:.2f}".format(y_LED)) print(f"x_LED: {x_LED_print} [px], y_LED: {y_LED_print} [px]") # Abstand (dx_LED, dy_LED) Fußpunkt x_FP zu KS_0 (P) dx_LED = x_LED - x_0 dx_LED_mm = dx_LED*(1/pixels_per_mm) dy_LED = y_LED - y_0 dy_LED_mm = dy_LED*(1/pixels_per_mm) if print_additional_info: # convert float32, round and prepare for printing string dx_LED_print = float("{0:.2f}".format(dx_LED)) dy_LED_print = float("{0:.2f}".format(dy_LED)) dx_LED_mm_print = float("{0:.2f}".format(dx_LED_mm)) dy_LED_mm_print = float("{0:.2f}".format(dy_LED_mm)) print(f"dx_LED: {dx_LED_print} [px] , dy_LED: {dy_LED_print} [px]") print(f"dx_LED: {dx_LED_mm_print} [mm] , dy_LED: {dy_LED_mm_print} [mm]") # Abstand (dx, dy) Fußpunkt x_FP von Bildmitte zu KS_Scooty # Diese Werte zurückgeben dx_LED_scooty = x_LED - x_0 + x_offset_camera_px dx_LED_scooty_mm = dx_LED_scooty*(1/pixels_per_mm) dy_LED_scooty = y_LED - y_0 + y_offset_camera_px dy_LED_scooty_mm = dy_LED_scooty*(1/pixels_per_mm) if print_additional_info: print(f"dx_LED_scooty: {round(dx_LED_scooty,2)} [px] , dy_LED_scooty: {round(dy_LED_scooty,2)} [px]") print(f"dx_LED_scooty: {round(dx_LED_scooty_mm,2)} [mm] , dy_LED_scooty: {round(dy_LED_scooty_mm,2)} [mm]") # Punkte Trafo, um sortierte position der LEDs entlang Spur zu erhalten # Bei normal detected kann bei vertikaler LED zb Fehler entstehen und dann muster: 211323233 -> daher mit dieser sortierten weitermachen detected_LEDs_KS_LED = points_trafo(detected_LEDs, alpha_rad, dx_LED, dy_LED) if print_additional_info: print(f"Detected LEDs in KS_LED:(x2, y2):\n {detected_LEDs_KS_LED}") #----------------------------------- # draw useful lines and points # draw lane line if draw_opencv: pt_0 = (a+b*np.array([-300, -300])).astype(np.int32) pt_1 = (a+b*np.array([300, 300])).astype(np.int32) #print(f"pt_0: {pt_0}, pt_1: {pt_1}") cv.line(img_bgr, pt_0, pt_1, (255,255,255),1) # draw lane # draw dx dy cv.line(img_bgr, (int(x_0), int(y_0)), (int(x_LED), int(y_LED)), (0,0,255), 2) # shortest distance from KS_0 to KS_LED --> Lot # cv.line(img_bgr, (int(x_0), int(y_0)), (int(x_LED), int(y_0)), (0,0,255), 2) # only dx # cv.line(img_bgr, (int(x_LED), int(y_0)), (int(x_LED), int(y_LED)), (0,0,255), 2) # only dy #draw additional points cv.circle(img_bgr, (int(x_2), int(y_2)), 5,(255,128,255),-1) #pink. Center of points #cv.putText(img_bgr, '(x2, y2)',(int(x_2)+5, int(y_2)-5), cv.FONT_HERSHEY_SIMPLEX, 2, (255,255,255), cv.LINE_AA) cv.circle(img_bgr, (int(x_LED), int(y_LED)), 5,(170,255,0),-1) # lime green. Fußpunkt if show_opencv_window: cv.imshow("Lane", img_bgr) return dx_LED_scooty_mm, dy_LED_scooty_mm, detected_LEDs_KS_LED def convert_rgb_to_grayscale_average(image_bgr): """This function converts the RGB image into an grayscale image. Algorithm: Average: Y = (R+G+B)/3""" # convert dtype to prevent integer overflow while addition image_bgr = image_bgr.astype(np.uint16, copy=False) image_gray = (image_bgr[:,:,0]+image_bgr[:,:,1]+image_bgr[:,:,2])/3 # add values / do conversion image_gray = image_gray.astype(np.uint8, copy=False) # convert back to uint8 return image_gray def get_color_of_leds(matrix_of_LEDs, image_bgr): """Determine color of LEDs at positions and add to matrix""" # is image_r[y_pos, x_pos] = image_bgr[y_pos,x_pos, 2] ? --> yes. No need to split the color channels. offset = 1 # half of length from rectangle which is going to be used to determine the color around the middle point of the blob/led # offset=0 --> only the value from the middle point of the blob/led # offset=1 --> 9 values, offset=2-->25 values for led in matrix_of_LEDs: x_pos = led[0] # uint16 y_pos = led[1] # uint16 # get values of color channels in region around middle point of blob/led: # +1 at stop index, because it is not inclusive region_around_blue_led = image_bgr[y_pos-offset:y_pos+offset+1, x_pos-offset:x_pos+offset+1, 0] # uint8 region_around_green_led = image_bgr[y_pos-offset:y_pos+offset+1, x_pos-offset:x_pos+offset+1, 1] # uint8 region_around_red_led = image_bgr[y_pos-offset:y_pos+offset+1, x_pos-offset:x_pos+offset+1, 2] # uint8 # average of the values # convert dtype to prevent integer overflow while addition region_around_red_led = region_around_red_led.astype(np.uint16, copy=False) region_around_green_led = region_around_green_led.astype(np.uint16, copy=False) region_around_blue_led = region_around_blue_led.astype(np.uint16, copy=False) # sum all elements in matrix and divide with number of elements number_of_elements= region_around_blue_led.size value_of_red_led = region_around_red_led.sum()/number_of_elements # float64, if not integer result value_of_green_led = region_around_green_led.sum()/number_of_elements # float64, if not integer result value_of_blue_led = region_around_blue_led.sum()/number_of_elements # float64, if not integer result # determine which leds are active: # if value > threshold --> led is active status_blue_led = False; status_green_led = False; status_red_led = False if value_of_blue_led > threshold_color_detection: status_blue_led = True if value_of_green_led > threshold_color_detection: status_green_led = True if value_of_red_led > threshold_color_detection: status_red_led = True # determine color by checking the cases: # case 1: red if status_blue_led==False and status_green_led==False and status_red_led==True: color = color_number_red # case 2: green elif status_blue_led==False and status_green_led==True and status_red_led==False: color = color_number_green # case 3: blue elif status_blue_led==True and status_green_led==False and status_red_led==False: color = color_number_blue # case 4: yellow = red + green elif status_blue_led==False and status_green_led==True and status_red_led==True: color = color_number_yellow # case 5: magenta = red + blue elif status_blue_led==True and status_green_led==False and status_red_led==True: color = color_number_magenta # case 6: cyan = green + blue elif status_blue_led==True and status_green_led==True and status_red_led==False: color = color_number_cyan # case 7: white = red + green + blue elif status_blue_led==True and status_green_led==True and status_red_led==True: color = color_number_white # case 8: led not active # this case can not occur, because no inactive led can be detected from the implemented blob-algorithm in detect_LED_positions_in_grayscale else: color = color_number_off # fill matrix with color led[2] = color # uint16 return matrix_of_LEDs def get_position_of_LEDs_contours(image_gray, image_bgr): # create binary image ret, image_binary = cv.threshold(image_gray, binary_threshold, binary_maxval, cv.THRESH_BINARY) # find contours contour_retrieval_algorithm = cv.RETR_EXTERNAL # retrieves only the extreme outer contours contours, hierarchy = cv.findContours(image_binary, contour_retrieval_algorithm, cv.CHAIN_APPROX_SIMPLE) # analyse contours number_of_detected_contours = len(contours) if number_of_detected_contours != 0: # centroid of contours # Pre-allocate matrix for numpy number_of_rows = 0 number_of_columns = 3 position_of_leds = np.zeros((number_of_rows, number_of_columns), dtype=np.uint16) #empty: [] if draw_opencv: image_bgr_contours = image_bgr.copy() # create copy of image # copy is needed to draw on, because else the color of # the circle is detected als LED-Color number_of_detected_LEDs = 0 for i, cnt in enumerate(contours): area = cv.contourArea(cnt) # diameter = 2*np.sqrt(area/np.pi) # diameter_mm = diameter*(1/pixels_per_mm) # print(f"area: {area} [px^2], diameter: {diameter} [px], diamter: {diameter_mm} [mm]") # Filter contours by area. minimum Area needs to be at least >0 ! if area > minArea_px2: M = cv.moments(cnt) number_of_detected_LEDs += 1 # prevent zero division if M['m00']==0: cx = 0 cy = 0 else: cx = int(M['m10']/M['m00']) cy = int(M['m01']/M['m00']) #print(cx, cy) # add positions to matrix x_pos = int(cx) # x positon y_pos = int(cy) # y position position_of_leds = np.vstack((position_of_leds, \ np.array([x_pos, y_pos, color_number_off], dtype=np.uint16))) # vstack: row wise # draw centroids if draw_opencv: radius = 2 color = (255,255,255) thickness = -1 # filled cv.circle(image_bgr_contours,(cx,cy), radius, color, thickness) if number_of_detected_LEDs != 0: if print_additional_info: print(f"detected LEDs: {number_of_detected_LEDs}") if draw_opencv: # draw contours contours_to_pass = -1 # pass all contours color_of_contour = (255,255,255) line_thickness = 1 cv.drawContours(image_bgr_contours, contours, contours_to_pass, color_of_contour, line_thickness) if show_opencv_window: cv.imshow("binary", image_binary) cv.imshow("Contours", image_bgr_contours) return position_of_leds else: if print_additional_info: print(f"No LEDs were detected") return None else: if print_additional_info: print(f"No contours were detected") return None def detect_position_of_LEDs(image_bgr): # convert rgb to grayscale image_gray = convert_rgb_to_grayscale_average(image_bgr) if show_opencv_window: cv.imshow("grayscale", image_gray) # get position of leds position_of_LEDs = get_position_of_LEDs_contours(image_gray, image_bgr) # position_of_LEDs = None return position_of_LEDs def lane_detection(image_bgr): # Detect LEDs if print_additional_info: print(f"Detect LEDs and color:") position_of_LEDs = detect_position_of_LEDs(image_bgr) # detected_LEDs = None # only that following code is not being executet for development # Get color of leds if position_of_LEDs is not None: detected_LEDs = get_color_of_leds(position_of_LEDs, image_bgr) # print result if print_additional_info: print(f"Detected LEDs in KS_Sensor (x,y):\n{detected_LEDs}") else: detected_LEDs = None # Contruct lane if detected_LEDs is not None: if print_additional_info: print("\nContruct lane with consideration of camera offset:") dx_LED_scooty_mm, dy_LED_scooty_mm, detected_LEDs_KS_LED = \ construct_lane(detected_LEDs, image_bgr) return detected_LEDs else: return None def main(): filenames_of_images = [f for f in os.listdir(folder_path) if f.endswith('.png')] for i, filename_of_image in enumerate(filenames_of_images): if print_additional_info: print(f"image {i+1}/{len(filenames_of_images)}:{filename_of_image}") full_path_of_image = os.path.join(folder_path, filename_of_image) image_bgr = cv.imread(full_path_of_image, cv.IMREAD_COLOR) # load original image start_processing = time.perf_counter() detected_LEDs = lane_detection(image_bgr) end_processing = time.perf_counter() time_processing = end_processing-start_processing time_processing = time_processing*1000 time_processing = round(time_processing, 2) print(f'processing time: {time_processing} ms') #print(f"_____________________________________") # show images: # cv.imshow("Blue channel", image_b) # cv.imshow("Green channel", image_g) # cv.imshow("Red channel", image_r) if show_opencv_window: pressed_key = cv.waitKey(0) & 0xff # display and wait if a key is pressed and then continue if pressed_key == ord('q'): exit() cv.destroyAllWindows() if __name__ == "__main__": main()