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

515 lines
22 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 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()
Copy Permalink