# Creation Date: 02.03.2022 # Author: Kenan Gömek # This code detects the lane. # 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 = (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, float 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 # Offset Camera Sensor in Scooty according to Scooty-KS x_offset_camera_mm = 0 # [mm] y_offset_camera_mm = 0 # [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_height = OUTPUT_RESOLUTION[1] image_width = OUTPUT_RESOLUTION[0] # calculate center of image [x_0, y_0] = np.array([image_width/2, image_height/2], dtype=np.uint16) threshold_color_detection = 60 # values (average) under this will not be considered as active leds in each color channel # see get_color_of_leds()-function # Parameters for Blob/LED Detection minDiameter_mm = 3.75 # [mm] minimum diameter of detected blob/LED. Must be minimum >0 ! # 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. # Parameters for OpenCV # OpenCV windows only for demonstration # They are not needed for functionality 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 minArea_px2 = np.pi/4*minDiameter_px**2 # min Area of a blob in px^2 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 [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: 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_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, 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: # 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 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}") line_color = (255, 255, 255) line_thickness = 1 cv.line(img_bgr, pt_0, pt_1, line_color, line_thickness) # draw lane # draw dx dy line_color = (0, 0, 255) line_thickness = 2 cv.line(img_bgr, (int(x_0), int(y_0)), (int(x_LED), int(y_LED)), line_color, line_thickness) # 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 circle_radius = 5 circle_color = (255, 128, 255) circle_thickness = -1 # filled cv.circle(img_bgr, (int(x_2), int(y_2)), circle_radius, circle_color, circle_thickness) # 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) circle_color = (170, 255, 0) cv.circle(img_bgr, (int(x_LED), int(y_LED)), circle_radius, circle_color, circle_thickness) # 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): M = cv.moments(cnt) 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: 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) 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 relative to image center (x0,y0):\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 # PiCamera def get_frames_from_camera(): # 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) # 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(): # start capturing get_frames_from_camera() # start capture if show_opencv_window: cv.destroyAllWindows() # destroy all open cv windows if __name__ == "__main__": main()