mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-21 01:01:41 +00:00
222 lines
9.1 KiB
Python
222 lines
9.1 KiB
Python
import cv2
|
|
import numpy
|
|
import math
|
|
from enum import Enum, unique
|
|
|
|
|
|
class VisionHandler():
|
|
def __init__(self):
|
|
self.kernel = numpy.ones((5, 5), numpy.uint8)
|
|
|
|
def _hsv_threshold(self, hue: list, saturation: list, value: list, img: numpy.ndarray, is_erode: bool,
|
|
is_dilate: bool):
|
|
|
|
hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
|
|
thresh = cv2.inRange(hsv, (hue[0], saturation[0], value[0]), (hue[1], saturation[1], value[1]))
|
|
erode_img = cv2.erode(thresh, kernel=self.kernel, iterations=is_erode)
|
|
dilate_img = cv2.dilate(erode_img, kernel=self.kernel, iterations=is_dilate)
|
|
return dilate_img
|
|
|
|
def find_contours(self, binary_img: numpy.ndarray):
|
|
|
|
_,contours, _ = cv2.findContours(binary_img, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_TC89_L1)
|
|
return contours
|
|
|
|
class Filter_Contours:
|
|
def __init__(self,center_x, center_y):
|
|
self.sort_mode = self.SortMode(center_x=center_x, center_y=center_y)
|
|
self.center_y = center_y
|
|
self.center_x = center_x
|
|
|
|
class SortMode:
|
|
def __init__(self, center_x, center_y):
|
|
self.center_x = center_x
|
|
self.center_y = center_y
|
|
|
|
@classmethod
|
|
def moment_x(cls,contour):
|
|
M = cv2.moments(contour)
|
|
try:
|
|
x = float(M['m10'] / M['m00'])
|
|
except ZeroDivisionError:
|
|
x = 0
|
|
return x
|
|
|
|
@classmethod
|
|
def moment_y(cls, contour):
|
|
M = cv2.moments(contour)
|
|
try:
|
|
y = float(M['m01'] / M['m00'])
|
|
except ZeroDivisionError:
|
|
y = 0
|
|
return y
|
|
|
|
@classmethod
|
|
def calc_distance(cls,contour, center_x, center_y):
|
|
M = cv2.moments(contour)
|
|
try:
|
|
x = int(M['m10'] / M['m00'])
|
|
except ZeroDivisionError:
|
|
x = 0
|
|
try:
|
|
y = int(M['m01'] / M['m00'])
|
|
except ZeroDivisionError:
|
|
y = 0
|
|
# this function was suggested by my girlfriend maya jugend that i really love
|
|
return math.sqrt((center_x-x)**2 + (center_y-y)**2)
|
|
|
|
def Largest(self, input_contours):
|
|
return sorted(input_contours, key=lambda x: cv2.contourArea(x), reverse=True)
|
|
|
|
def Smallest(self, input_contours):
|
|
return sorted(input_contours, key=lambda x: cv2.contourArea(x))
|
|
|
|
def Highest(self, input_contours):
|
|
return sorted(input_contours, key=lambda x: self.moment_y(x))
|
|
|
|
def Lowest(self, input_contours):
|
|
return sorted(input_contours, key=lambda x: self.moment_y(x),reverse=True)
|
|
|
|
def Rightmost(self, input_contours):
|
|
return sorted(input_contours, key=lambda x: self.moment_x(x), reverse=True)
|
|
|
|
def Leftmost(self, input_contours):
|
|
return sorted(input_contours, key=lambda x: self.moment_x(x))
|
|
|
|
def Closest(self, input_contours):
|
|
return sorted(input_contours, key=lambda x: self.calc_distance(x, center_x=self.center_x,
|
|
center_y=self.center_y), reverse=True)
|
|
|
|
def filter_contours(self, input_contours, cam_area, area, ratio, extent, sort_mode, target_grouping,
|
|
target_intersection):
|
|
class TargetGroup(Enum):
|
|
Single = 1
|
|
Dual = 2
|
|
Triple = 3
|
|
Quadruple = 4
|
|
Quintuple = 6
|
|
|
|
def group_target(i_contours, target_group, intersection_point):
|
|
|
|
def is_intersecting(contour_a, contour_b, intersection_direction):
|
|
|
|
[vx_a, vy_a, x0_a, y0_a] = cv2.fitLine(contour_a, cv2.DIST_L2, 0, 0.01, 0.01)
|
|
[vx_b, vy_b, x0_b, y0_b] = cv2.fitLine(contour_b, cv2.DIST_L2, 0, 0.01, 0.01)
|
|
# getting line data of both contours
|
|
m_a = vy_a / vx_a
|
|
m_b = vy_b / vx_b
|
|
# calculating slope of both lines
|
|
try:
|
|
intersection_x = ((m_a * x0_a) - y0_a - (m_b * x0_b) + y0_b) / (m_a - m_b)
|
|
except ZeroDivisionError:
|
|
if intersection_direction == 'Parallel':
|
|
return True
|
|
else:
|
|
return False
|
|
intersection_y = (m_a * (intersection_x - x0_a)) + y0_a
|
|
# finding intersection point
|
|
if intersection_direction == 'Up':
|
|
if intersection_y < self.center_y:
|
|
return True
|
|
elif intersection_direction == 'Down':
|
|
if intersection_y > self.center_y:
|
|
return True
|
|
elif intersection_direction == 'Left':
|
|
if intersection_x < self.center_x:
|
|
return True
|
|
elif intersection_direction == 'Right':
|
|
if intersection_x > self.center_x:
|
|
return True
|
|
else:
|
|
return False
|
|
if target_group != TargetGroup.Single:
|
|
f_contour_list = []
|
|
for index, g_contour in enumerate(i_contours):
|
|
final_contour = g_contour
|
|
for c in range(target_group.value - 1):
|
|
try:
|
|
first_contour = i_contours[index + c]
|
|
second_contour = i_contours[index + c + 1]
|
|
except IndexError:
|
|
final_contour = []
|
|
break
|
|
if is_intersecting(first_contour, second_contour, intersection_point):
|
|
final_contour = numpy.concatenate((final_contour, second_contour))
|
|
|
|
else:
|
|
final_contour = []
|
|
break
|
|
if final_contour != []:
|
|
f_contour_list.append(final_contour)
|
|
|
|
return f_contour_list
|
|
else:
|
|
return i_contours
|
|
|
|
'''start of the first filtration of contours'''
|
|
filtered_contours = []
|
|
for contour in input_contours:
|
|
try:
|
|
contour_area = cv2.contourArea(contour)
|
|
target_area = float(contour_area / cam_area)*100
|
|
|
|
if target_area >= area[1] or target_area <= area[0]:
|
|
continue
|
|
|
|
rect = cv2.minAreaRect(contour)
|
|
bounding_rect_area = rect[1][0] * rect[1][1]
|
|
try:
|
|
target_fullness = float(contour_area / bounding_rect_area)*100
|
|
except ZeroDivisionError:
|
|
target_fullness = 0
|
|
|
|
if target_fullness <= extent[0] or target_fullness >= extent[1]:
|
|
continue
|
|
try:
|
|
aspect_ratio = float(rect[1][0]/rect[1][1])
|
|
except ZeroDivisionError:
|
|
aspect_ratio = 0
|
|
if aspect_ratio <= ratio[0] or aspect_ratio >= ratio[1]:
|
|
continue
|
|
|
|
filtered_contours.append(contour)
|
|
except Exception as e:
|
|
print(e)
|
|
continue
|
|
#checking for contour grouping before sorting
|
|
grouped_contours = group_target(filtered_contours, TargetGroup[target_grouping], target_intersection)
|
|
try:
|
|
sorted_contours = getattr(self.sort_mode, sort_mode)(grouped_contours)
|
|
except TypeError:
|
|
sorted_contours = []
|
|
return sorted_contours
|
|
|
|
def output_contour(self, sorted_contours):
|
|
if len(sorted_contours) > 0:
|
|
selected_contour = sorted_contours[0]
|
|
rect = cv2.minAreaRect(selected_contour)
|
|
else:
|
|
return []
|
|
return rect
|
|
|
|
def draw_image(self, input_image, contour):
|
|
if len(input_image.shape)<3:
|
|
input_image = cv2.cvtColor(input_image, cv2.COLOR_GRAY2RGB)
|
|
if contour != []:
|
|
box = cv2.boxPoints(contour)
|
|
box = numpy.int0(box)
|
|
cv2.drawContours(input_image, [box], 0, (0, 0, 255), 3)
|
|
|
|
# center_point = (int(rectangle[0][0]), int(rectangle[0][1]))
|
|
# cv2.circle(input_image, center_point, 0, (0, 255, 0), thickness=3, lineType=8, shift=0)
|
|
return input_image
|
|
|
|
def calculate_pitch(self, pixel_y, center_y, v_focal_length):
|
|
pitch = math.degrees(math.atan((pixel_y - center_y) / v_focal_length))
|
|
pitch *= -1
|
|
return pitch
|
|
|
|
def calculate_yaw(self, pixel_x, center_x, h_focal_length):
|
|
yaw = math.degrees(math.atan((pixel_x - center_x) / h_focal_length))
|
|
return yaw
|