# Creation Date: 01.03.2022 # Author: Kenan Gömek # This program is for the development of the lane detection algorithm on a windows machine # It uses images, which were captured previously # V02 detects the lane by suing the grayscale-image of the rgb-image import cv2 as cv import numpy as np import os import time import platform # input if platform.system() == "Windows": folder_path=r"U:\bwsyncshare\SA\Software\Code\02_SA\04_Spurerkennung\images_input" if platform.system() == "Linux": folder_path=r"/home/pi/Desktop/Studienarbeit/04_Spurerkennung/images_input" # Parameters 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 Blob/LED Detection minDiameter_mm = 5 # [mm] minimum diameter of detected blob/LED 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 show_opencv_window = True # show opencv window draw_opencv = True # draw lane and so on print_additional_info = True 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 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 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. [dx, dy, x_2, y_2] = cv.fitLine(detected_LEDs[:,0:2], cv.DIST_L2, param, reps, aeps) # 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: print(f"Lane: alpha:{alpha[0]}°") # 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_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: print(f"x_LED: {x_LED}, y_LED: {y_LED}") # Abstand (dx, dy) Fußpunkt zu KS_0 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: print(f"dx_LED:{dx_LED} [px] , dy_LED:{dy_LED} [px]") print(f"dx_LED:{dx_LED_mm} [mm] , dy_LED:{dy_LED_mm} [mm]") # Abstand (dx, dy) Fußpunkt 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:{dx_LED_scooty} [px] , dy_LED_scooty:{dy_LED_scooty} [px]") print(f"dx_LED_scooty:{dx_LED_scooty_mm} [mm] , dy_LED_scooty:{dy_LED_scooty_mm} [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 create_detector(params_for_blob_detection): detector = cv.SimpleBlobDetector_create(params_for_blob_detection) # Set up the detector with specified parameters. return detector def define_parameters_for_blob_detection(): """set parameters for simple blob detector""" params = cv.SimpleBlobDetector_Params() # Threshold for Convert the source image to binary images by applying thresholding # with several thresholds from minThreshold (inclusive) to maxThreshold (exclusive) # with distance thresholdStep between neighboring thresholds. # Since the Grayscale image is dark if only one color channel is active, # the Threshold values have to be set like this. # particularly the thresholdStep-Value has to be low params.minThreshold=20 # reminder: this value is set for grayscale image params.maxThreshold=255 params.thresholdStep=1 params.filterByColor=False # do not filter blobs by color # Filter blobs by Area params.filterByArea=True 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 maxArea_px2 = np.pi/4*maxDiameter_px**2 params.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 # Filter by Inertia params.filterByInertia=False params.minInertiaRatio = 0.2 # [0-1] # Filter by Convexity params.filterByConvexity=False params.minConvexity = 0.2 # [0-1] # Filter by Circularity params.filterByCircularity=False params.minCircularity = 0.4 # [0-1] # params.minDistBetweenBlobs = minDist_px # this has no effect return params def detect_LED_positions_in_grayscale(image_gray, image_bgr, detector): keypoints = detector.detect(image_gray) # Detect blobs --> LEDs number_of_detected_leds = len(keypoints) if number_of_detected_leds != 0: # print information of keypoints print(f"detected LEDs: {number_of_detected_leds}") #Pre-allocate matrix for numpy number_of_rows = number_of_detected_leds number_of_columns = 3 position_of_leds = np.zeros((number_of_rows, number_of_columns), dtype=np.uint16) for i, k in enumerate(keypoints): # x_pos = round(k.pt[0],0) # x position # y_pos = round(k.pt[1],0) # y position # print(f"x: {x_pos} y: {y_pos}") # diameter_px = round(k.size,2) # diameter_mm = round(diameter_px*1/pixels_per_mm,2) # print(f"diameter [px]: {diameter_px} diameter [mm]: {diameter_mm}") # diameter # area_px2 = round(np.pi/4*k.size**2,0) # area in px^2 # area_mm2 = round(area_px2*(1/pixels_per_mm)**2,0) # print(f"area [px^2]: {area_px2} area [mm^2]: {area_mm2}") # print('') # calculate parameters to transfer to matrix # x_pos = int(np.ceil(x_pos)) # y_pos = int(np.ceil(y_pos)) # Fill matrix # position_of_leds[i,:] = [x_pos,y_pos, 0] position_of_leds[i,0] = int(np.ceil(k.pt[0])) # x positon position_of_leds[i,1] = int(np.ceil(k.pt[1])) # y position if draw_opencv: # draw the keypoints on the original image # cv.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS ensures the size of the circle corresponds to the size of blob blobs = cv.drawKeypoints(image=image_bgr, keypoints=keypoints, color=(255, 255, 255), \ outImage=np.array([]), flags= cv.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS) if show_opencv_window: cv.imshow("grayscale", image_gray) cv.imshow("Detected", blobs) return position_of_leds else: print(f"No LEDs were detected") return None def detect_position_of_all_LEDs_grayscale(image_gray, image_bgr, detector): position_of_LEDs = detect_LED_positions_in_grayscale(image_gray, image_bgr, detector) if position_of_LEDs is not None: return position_of_LEDs else: return None def get_color_of_leds(matrix_of_LEDs, image_bgr): # is image_r[y_pos, x_pos] = image_bgr[y_pos,x_pos, 2] ? --> yes. No need to split the color channels. offset = 2 # 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 detect_LEDs_with_grayscale(image_bgr, detector): # convert rgb to grayscale image # start_m1 = time.perf_counter() image_gray = convert_rgb_to_grayscale_average(image_bgr) # end_m1 = time.perf_counter() # time_processing = end_m1-start_m1 # time_processing = time_processing*1000 # time_processing = round(time_processing, 2) # print(f'processing time conversion: {time_processing} ms') # get position of leds position_of_LEDs = detect_position_of_all_LEDs_grayscale(image_gray=image_gray, image_bgr=image_bgr, detector=detector) #position_of_LEDs = None if position_of_LEDs is not None: # determine color of leds and add to matrix detected_LEDs = get_color_of_leds(position_of_LEDs, image_bgr) return detected_LEDs else: return None def lane_detection(image_bgr, detector): # Detect LEDs print(f"Detect LEDs and color:") detected_LEDs = detect_LEDs_with_grayscale(image_bgr, detector) if detected_LEDs is not None: # Contruct lane #print(f"_____________________________________") # print("Contruct lane") dx_LED_scooty_mm, dy_LED_scooty_mm, detected_LEDs_KS_LED = \ construct_lane(detected_LEDs, image_bgr) # print result if print_additional_info: print(f"Detected LEDs relative to KS_Sensor (x,y):\n{detected_LEDs}") return detected_LEDs else: return None def main(): filenames_of_images = [f for f in os.listdir(folder_path) if f.endswith('.png')] # initialise parameters for blob detectio once befor loop for performane params_for_blob_detection = define_parameters_for_blob_detection() detector = create_detector(params_for_blob_detection) for i, filename_of_image in enumerate(filenames_of_images): 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, detector) 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()