diff --git a/backend/app/classes/SettingsManager.py b/backend/app/classes/SettingsManager.py index df5d5978f..7de5badcd 100644 --- a/backend/app/classes/SettingsManager.py +++ b/backend/app/classes/SettingsManager.py @@ -26,7 +26,10 @@ class SettingsManager(metaclass=Singleton): "area": [0, 100], "ratio": [0, 20], "extent": [0, 100], - "is_binary": "Normal" + "is_binary": "Normal", + "sort_mode": "Largest", + "target_group": 'Single', + "target_intersection": 'Up' } default_general_settings = { "team_number": 1577, diff --git a/backend/app/handlers/VisionHandler.py b/backend/app/handlers/VisionHandler.py index c3dc3b94d..33a1aea6e 100644 --- a/backend/app/handlers/VisionHandler.py +++ b/backend/app/handlers/VisionHandler.py @@ -5,12 +5,11 @@ import numpy from cscore import CameraServer from app.classes.SettingsManager import SettingsManager from ..classes.Singleton import Singleton -import time from multiprocessing import Process import threading import zmq -import base64 - +import math +from enum import Enum, unique class VisionHandler(metaclass=Singleton): @@ -19,8 +18,6 @@ class VisionHandler(metaclass=Singleton): def _hsv_threshold(self, hue: list, saturation: list, value: list, img: numpy.ndarray, is_erode: bool, is_dilate: bool): - # img = cv2.medianBlur(img, 1) - # not sure if we need noise reduction now with erode it hurts the precision if val is to high img = cv2.erode(img, kernel=self.kernel, iterations=is_erode) img = cv2.dilate(img, kernel=self.kernel, iterations=is_dilate) @@ -28,48 +25,184 @@ class VisionHandler(metaclass=Singleton): return cv2.inRange(out, (hue[0], saturation[0], value[0]), (hue[1], saturation[1], value[1])) def find_contours(self, binary_img: numpy.ndarray): - _, contours, _ = cv2.findContours(binary_img, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + _, contours, _ = cv2.findContours(binary_img, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) return contours - def filter_contours(self, input_contours, camera_area, area, ratio, extent): - output = [] - rectangle = [] + 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 - for contour in input_contours: + class SortMode: + def __init__(self, center_x, center_y): + self.center_x = center_x + self.center_y = center_y - rect = cv2.minAreaRect(contour) - # center_point = rect[0] - contour_area = cv2.contourArea(contour) - rect_area = rect[1][0] * rect[1][1] + @classmethod + def moment_x(cls,contour): + M = cv2.moments(contour) + try: + x = float(M['m10'] / M['m00']) + except ZeroDivisionError: + x = 0 + return x - try: - extent_percent = float(contour_area) / rect_area - ratio_percent = float(rect[1][0]) / rect[1][1] - area_percent = rect_area / camera_area - except: - continue + @classmethod + def moment_y(cls, contour): + M = cv2.moments(contour) + try: + y = float(M['m01'] / M['m00']) + except ZeroDivisionError: + y = 0 + return y - if area_percent < area[0] or area_percent > area[1]: - continue - if ratio_percent < ratio[0] or ratio_percent > ratio[1]: - continue - if extent_percent < extent[0] or extent_percent > extent[1]: - continue + @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) - output.append(contour) - rectangle.append(rect) + def Largest(self, input_contours): + return sorted(input_contours, key=lambda x: cv2.contourArea(x)) - return [output, rectangle] + def Smallest(self, input_contours): + return sorted(input_contours, key=lambda x: cv2.contourArea(x), reverse=True) - def draw_image(self, input_image: numpy.ndarray, is_binary: bool, rectangles): + 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 False + 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, contour in i_contours: + finall_contour = contour + for c in range(target_group): + first_contour = i_contours[index + c] + second_contour = i_contours[index + c + 1] + if is_intersecting(first_contour, second_contour, intersection_point): + pass + else: + continue + else: + return i_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])*100 + 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 + + grouped_contours = group_target(filtered_contours, TargetGroup[target_grouping], target_intersection) + sorted_contours = getattr(self.sort_mode, sort_mode)(grouped_contours) + + return sorted_contours + + @unique + class Region(Enum): + UP_MOST = 0 + RIGHT_MOST = 1 + DOWN_MOST = 2 + LEFT_MOST = 3 + CENTER_MOST = 4 + + # def output_contours(): + # # target_region leftmost,rightmost,upmost,downmost,centermost + # # crosshair_calibration function to "put" camera in the middle + # pass + + def draw_image(self, input_image: numpy.ndarray, is_binary: bool, contours): if is_binary: input_image = cv2.cvtColor(input_image, cv2.COLOR_GRAY2RGB) - for rectangle in rectangles[1]: - box = cv2.boxPoints(rectangle) - box = numpy.int0(box) - cv2.drawContours(input_image, [box], 0, (0, 0, 255), 2) - 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) + for contour in contours: + cv2.drawContours(input_image, contour, -1, (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 run(self): @@ -101,8 +234,8 @@ class VisionHandler(metaclass=Singleton): p.start() pipeline = SettingsManager().cams[cam_name]["pipelines"]["pipeline0"] + while True: - # start = time.time( _, image = cv_sink.grabFrame(image) socket.send_json(dict( pipeline=pipeline @@ -110,10 +243,9 @@ class VisionHandler(metaclass=Singleton): socket.send_pyobj(image) p_image = socket.recv_pyobj() cv_publish.putFrame(p_image) - # print(cam_name + " " + str(1 / (end - start))) def camera_process(self, cam_name, port): - + from fractions import Fraction # def change_camera_values(): # camera.setBrightness(0) # camera.setExposureManual(0) @@ -133,15 +265,30 @@ class VisionHandler(metaclass=Singleton): # table.addEntryListenerEx(mode_listener, key="Driver_Mode", # flags=networktables.NetworkTablesInstance.NotifyFlags.UPDATE) # change_camera_values() + diagonalView = math.radians(68.5) #needs to be implemented in client width = SettingsManager().cams[cam_name]["video_mode"]["width"] height = SettingsManager().cams[cam_name]["video_mode"]["height"] + centerX = (width / 2) - .5 + centerY = (height / 2) - .5 cam_area = width * height + + aspect_fraction = Fraction(width,height) + horizontal_ratio = aspect_fraction.numerator + vertical_ratio = aspect_fraction.denominator + + diagonal_aspect = math.hypot(horizontal_ratio, vertical_ratio) + + horizontalView = math.atan(math.tan(diagonalView/2) * (horizontal_ratio / diagonalView)) * 2 + verticalView = math.atan(math.tan(diagonalView/2) * (vertical_ratio / diagonalView)) * 2 + + H_FOCAL_LENGTH = width / (2*math.tan((horizontalView/2))) + V_FOCAL_LENGTH = height / (2*math.tan((verticalView/2))) context = zmq.Context() socket = context.socket(zmq.PAIR) socket.connect('tcp://localhost:%s' % str(port)) - + filter_contours = self.Filter_Contours(center_x=centerX, center_y=centerY) while True: obj = socket.recv_json() image = socket.recv_pyobj() @@ -151,10 +298,14 @@ class VisionHandler(metaclass=Singleton): image, curr_pipeline["erode"], curr_pipeline["dilate"]) # if table.getBoolean("Driver_Mode", False): contours = self.find_contours(hsv_image) - filtered_contours = self.filter_contours(contours, cam_area, curr_pipeline["area"], curr_pipeline["ratio"], - curr_pipeline["extent"]) - res = self.draw_image(input_image=image, is_binary=False, rectangles=filtered_contours) - # cv2.putText(res, str(fps), (10, 200), font, 4, (0, 0, 0), 2, cv2.LINE_AA) + filtered_contours = filter_contours.filter_contours(input_contours=contours, area=curr_pipeline['area'], + ratio=curr_pipeline['ratio'], + extent=curr_pipeline['extent'], + sort_mode=curr_pipeline['sort_mode'], cam_area=cam_area, + target_grouping=curr_pipeline['target_group'], + target_intersection=curr_pipeline['target_intersection']) + + res = self.draw_image(input_image=image, is_binary=False, contours=filtered_contours) socket.send_pyobj(res) diff --git a/backend/settings/cams/USB Camera-B4.09.24.1.json b/backend/settings/cams/USB Camera-B4.09.24.1.json deleted file mode 100644 index b03728f36..000000000 --- a/backend/settings/cams/USB Camera-B4.09.24.1.json +++ /dev/null @@ -1 +0,0 @@ -{"pipelines": {"pipeline0": {"exposure": 25, "brightness": 19, "orientation": "Normal", "resolution": 1, "hue": [0, 10], "saturation": [58, 69], "value": [61, 87], "erode": false, "dilate": false, "area": [0, 100], "ratio": [0, 20], "extent": [0, 100], "is_binary": "Normal"}}, "path": "/dev/v4l/by-path/pci-0000:02:03.0-usb-0:1:1.0-video-index0", "video_mode": {"fps": 150, "width": 320, "height": 240, "pixel_format": "kYUYV"}} \ No newline at end of file diff --git a/chameleon-client/src/App.vue b/chameleon-client/src/App.vue index 630e15164..d01c6d84c 100644 --- a/chameleon-client/src/App.vue +++ b/chameleon-client/src/App.vue @@ -13,6 +13,7 @@ Input Threshold Contours + Output