kgoemek/SA-Kenan
2
1
Fork 0
Code Releases Activity
526dafd894
SA-Kenan/90_ZielSW/02_With_Processing_Zielsoftware_findContours.py
Kenan Gömek 526dafd894 SA Kenan first init
2022-05-09 20:30:26 +02:00

522 lines
21 KiB
Python
Raw Blame History

# Creation Date: 02.03.2022
# Author: Kenan Gömek
# This code detects the lane with findContours.
# 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_height = 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_height/2], dtype=np.uint16)
threshold_color_detection = 60
# values under this will not be considered as active leds in each color channel
# see get_color_of_leds()
# Parameters for Blob/LED Detection
minDiameter_mm = 3.75 # [mm] minimum diameter of detected blob/LED. Must be minimum >0 !
# 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
# 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
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
# maxDiameter_px = maxDiameter_mm*pixels_per_mm # [px] maximum diameter of detected blob/LED
minArea_px2 = np.pi/4*minDiameter_px**2 # min Area of a blob in px^2
# maxArea_px2 = np.pi/4*maxDiameter_px**2
# minArea = minArea_px2 # min Area of a blob in px^2
# params.maxArea = maxArea_px2 # max Area of a blob in px^2.
# reasons for not filtering maxArea: motion blur + rolling shutter --> larger Area
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}")
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 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)
# position_of_LEDs = None
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
cv.destroyAllWindows()
print('Program finished')
if __name__ == "__main__":
main()
View Git Blame Copy Permalink