# Creation Date: 02.03.2022 # Author: Kenan Gömek # This code detects the lane with Simpleblob Detector. # Quit program with 'q' if opencv windows is shows. Else use Ctrl+C. import cv2 as cv import picamera from picamera.array import PiRGBArray from fractions import Fraction import time from datetime import datetime import os import numpy as np import math as M # Define camera settings SENSOR_MODE = 4 # corresponding sensor mode to resolution 1640x1232 # OUTPUT_RESOLUTION = (416, 320) # (1640x1232)/4=(410,308) --> needs to be divisible by 16 --> 416x320 OUTPUT_RESOLUTION = (192, 144) AWB_MODE = 'off' # Auto white balance mode AWB_GAINS = (1.395, 1.15) # White Balance Gains to have colours read correctly: (red, blue). Int, floar or fraction are valid. BRIGHTNESS = 25 # sets the brightness setting of the camera. default is 50. [0-100] #the brighter, the brighter the LEDs and the higher the RGB values and vice versa! CONTRAST = 100 # sets the contrast setting of the camera. The default value is 0. [-100 ... 100] SHUTTER_SPEED = 50 # [µs] ISO = 320 # ISO value EXPOSURE_MODE = 'off' FRAMERATE = 25 # frames per second. 40 fps is max for sensor mode 4 SLEEP_TIME = 2 # Time for sleep-mode for the camera in seconds. My default: 2 s # Parameters pixels_per_mm = 32/24.25 #[px/mm] for 120 mm camera height for resolution: 192x144 # 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 image_heigth = OUTPUT_RESOLUTION[1] # shape [0] image_width = OUTPUT_RESOLUTION[0]# shape[1] # calculate center of image [x_0, y_0] = np.array([image_width/2, image_heigth/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=False # Filter by Inertia params.filterByInertia=False # Filter by Convexity params.filterByConvexity=False # Filter by Circularity params.filterByCircularity=False # params.minDistBetweenBlobs = minDist_px # this has no effect return params def detect_LED_positions_in_grayscale(image_gray, image_bgr, detector): start_processing = time.perf_counter() keypoints = detector.detect(image_gray) # Detect blobs --> LEDs 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 detector: {time_processing} ms') 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(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(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 image-center(x0,y0):\n{detected_LEDs}") return detected_LEDs else: return None # PiCamera def get_frames_from_camera(detector): # Initialise Camera print('Initialise Camera...') with picamera.PiCamera() as camera: with PiRGBArray(camera) as output: # Set camera settings camera.sensor_mode = SENSOR_MODE # force camera into desired sensor mode camera.resolution = OUTPUT_RESOLUTION # frame will be resized from GPU to this resolution. No CPU usage! camera.framerate = FRAMERATE camera.awb_mode = AWB_MODE camera.awb_gains = AWB_GAINS camera.iso = ISO camera.shutter_speed = SHUTTER_SPEED # it was found that, you have to set the right shutter speed at the first initalisation of the current runtime of the program. # The gains (analog, digital) will adjust to this set up. # After the gains are fixed, they will never change! even if you change the shutter speed during the runtime. # To get consistent brightness values, set the right shutter speed at initalisation once. time.sleep(SLEEP_TIME) # wait for iso gains and digital_gain and analog_gain to settle before fixing the gains with exposure_mode = off camera.exposure_mode = EXPOSURE_MODE time.sleep(1) # wait before applying brightness and contrast camera.brightness = BRIGHTNESS camera.contrast = CONTRAST time.sleep(SLEEP_TIME) # Camera warm-up time to apply settings # camera.start_preview() # show camera preview through PiCamera interface # camera.annotate_frame_num=True # Controls whether the current frame number is drawn as an annotation. print('Start caputure...') for frameidx, frame in enumerate(camera.capture_continuous(output, format='bgr', use_video_port=True)): start_processing = time.perf_counter() framenumber = frameidx+1 # frameidx starts with 0, framenumber with 1 image_bgr = frame.array # raw NumPy array without JPEG encoding #cv.imshow("Current Frame", image) # display the image without text output.truncate(0) # clear the stream for next frame # processing lane_detection(image_bgr, detector) # Only uncomment following code if you display the image. No errors if not commented, but higher fps if commented. # if q is pressed, break from loop. pressed_key = cv.waitKey(2) & 0xff if pressed_key == ord('q'): break 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') # ---------------------------------------------------------------------------- # main def main(): # initialise parameters for blob detection once befor loop for performane params_for_blob_detection = define_parameters_for_blob_detection() detector = create_detector(params_for_blob_detection) # start capturing get_frames_from_camera(detector) # start capture cv.destroyAllWindows() print('Program finished') if __name__ == "__main__": main()