kgoemek/SA-Kenan
2
1
Fork 0
Code Releases Activity
526dafd894
SA-Kenan/90_ZielSW/02_With_Processing_Zielsoftware_Simpleblob.py

515 lines
21 KiB
Python
Raw Normal View History

SA Kenan first init
2022-05-09 20:30:26 +02:00
# 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()
Copy Permalink