# Creation Date: 02.03.2022 # Author: Kenan Gömek # This code detects the lane with HoughCircles. # 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) # (1640x1232)/4=(410,308) --> needs to be divisible by 16 --> 416x320 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 # Define Functions # 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 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 = False # Parameters for HoughCircles dp = 1 # Inverse ratio of the accumulator resolution to the image resolution. For example, if dp=1 , the accumulator has the same resolution as the input image. If dp=2 , the accumulator has half as big width and height. minDist_mm = 1 # [mm] minimal distance between two circles minDist_px = int(minDist_mm*pixels_per_mm) # in [px] Minimum distance in px between the centers of the detected circles. If the parameter is too small, multiple neighbor circles may be falsely detected in addition to a true one. If it is too large, some circles may be missed. minRadius_mm = 3 # [mm] minimum radius of a circle minRadius_px = int(minRadius_mm*pixels_per_mm) # [px] Minimum circle radius. maxRadius_mm = 7 # [mm] maximum radius of a circle maxRadius_px = int(maxRadius_mm*pixels_per_mm) # [px] Maximum circle radius. If <= 0, uses the maximum image dimension. If < 0, returns centers without finding the radius. param1 = 150 # 30 First method-specific parameter. In case of HOUGH_GRADIENT , it is the higher threshold of the two passed to the Canny edge detector (the lower one is twice smaller). # If circles/LEDs should be detected at low shutter speeds, than lower this value # Upper threshold for the internal Canny edge detector. # "Gradient value between dark and white" param2 = 5 # 12 Second method-specific parameter. In case of HOUGH_GRADIENT , it is the accumulator threshold for the circle centers at the detection stage. The smaller it is, the more false circles may be detected. Circles, corresponding to the larger accumulator values, will be returned first. # By increasing this threshold value, we can ensure that only the best circles, corresponding to larger accumulator values, are returned. 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 detected_LEDs = cv.HoughCircles(image_gray, cv.HOUGH_GRADIENT, dp=dp, minDist = minDist_px , param1=param1, param2=param2, minRadius=minRadius_px, maxRadius=maxRadius_px) 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 Hough: {time_processing} ms') # check if at least one circle was found in the image if detected_LEDs is not None: detected_LEDs = np.uint16(np.round(detected_LEDs)) # convert the (x, y) coordinates and radius of the circles to integers detected_LEDs = detected_LEDs[0,:] detected_LEDs=np.hstack((detected_LEDs, np.full((detected_LEDs.shape[0],1), 9, dtype=np.uint16))) # matrix with columns: x, y, r number_of_detected_LEDs = detected_LEDs.shape[0] print(f"detected {9} LEDs: {number_of_detected_LEDs}") # paramters for drawing line_thickness = 1 circle_color = (0,255,0) vertex_offset = 2 rectangle_color = (0,128,255) # R G B for (x, y, r, cn) in detected_LEDs: print(f"x:{x} px, y:{y} px, r:{r} px, r:{round(r*1/(pixels_per_mm),2)} mm, D: {round(2*r*1/(pixels_per_mm),2)} mm, color: {9}") cv.circle(image_bgr, (x, y), r, circle_color, thickness=line_thickness) # draw detected circumference of the cirle cv.rectangle(img=image_bgr, pt1=(x-vertex_offset, y-vertex_offset), pt2=(x+vertex_offset, y+vertex_offset), \ color=rectangle_color, thickness=cv.FILLED) cv.imshow(f"HC", image_bgr) return None 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 = 0 # 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 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 = round(end_processing-start_processing, 2) time_processing = time_processing*1000 print(f'processing time: {time_processing} ms') # ---------------------------------------------------------------------------- # main def main(): # 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) # start capturing get_frames_from_camera(detector) # start capture cv.destroyAllWindows() print('Program finished') if __name__ == "__main__": main()