523 lines
21 KiB
Python
523 lines
21 KiB
Python
|
# 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()
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|