class Vision: """Main vision class. An instance should be created, with test=False (default). As long as the cameras are configured correctly via the GUI interface, everything will work without modification required. This will not work on most machines, so tests of the main process function are the only tests that can be done without a Pi running the FRC vision image. """ entries = None def __init__( self, test_img=[], test_video=[], test_display=False, using_nt=False, ): # self.entries = entries # Memory Allocation # Numpy takes Rows then Cols as dimensions. Height x Width self.hsv = np.zeros(shape=(FRAME_HEIGHT, FRAME_WIDTH, 3), dtype=np.uint8) self.image = self.hsv.copy() self.display = self.hsv.copy() self.mask = np.zeros(shape=(FRAME_HEIGHT, FRAME_WIDTH), dtype=np.uint8) # Camera Configuration self.CameraManager = CameraManager(test_img=test_img, test_video=test_video, test_display=test_display) self.testing = len(test_video) or len(test_img) if not self.testing: self.CameraManager.setCameraProperty( 0, "white_balance_temperature_auto", 0) self.CameraManager.setCameraProperty(0, "exposure_auto", 1) self.CameraManager.setCameraProperty(0, "focus_auto", 0) self.CameraManager.setCameraProperty(0, "exposure_absolute", 1) self.Connection = Connection(using_nt=using_nt, test=self.testing) self.avg_horiz_angle = 0 self.avg_dist = 0 self.prev_dist = 0 self.prev_horiz_angle = 0 self.old_fps_time = 0 def find_loading_bay(self, frame: np.ndarray): cnts, hierarchy = cv2.findContours(self.mask, cv2.RETR_CCOMP, cv2.CHAIN_APPROX_SIMPLE) cnts = np.array(cnts) hierarchy = np.array(hierarchy)[0] outer_rects = {} inner_rects = {} for i, cnt in enumerate(cnts): if hierarchy[i][3] == -1: outer_rects[i] = ( get_corners_from_contour(cnt), hierarchy[i], cv2.contourArea(cnt), ) else: inner_rects[i] = ( get_corners_from_contour(cnt), hierarchy[i], cv2.contourArea(cnt), ) if not (inner_rects and outer_rects): return None good = [] for i in outer_rects: if outer_rects[i][2] > MIN_CONTOUR_AREA: current_inners = [] next_child = outer_rects[i][1][2] while next_child != -1: current_inners.append(inner_rects[next_child]) next_child = inner_rects[next_child][1][0] largest = max(current_inners, key=lambda x: x[2]) if (abs((outer_rects[i][2] / largest[2]) - LOADING_INNER_OUTER_RATIO) < 0.5 and abs((cv2.contourArea(outer_rects[i][0]) / outer_rects[i][2]) - 1) < LOADING_RECT_AREA_RATIO and abs((cv2.contourArea(largest[0]) / largest[2]) - 1) < LOADING_RECT_AREA_RATIO): good.append((outer_rects[i], largest)) self.image = frame.copy() for pair in good: self.image = cv2.drawContours(self.image, pair[0][0].reshape((1, 4, 2)), -1, (255, 0, 0), thickness=2) self.image = cv2.drawContours( self.image, pair[1][0].reshape((1, 4, 2)), -1, (255, 0, 255), thickness=1, ) return (0.0, 0.0) def find_power_port(self, frame: np.ndarray) -> tuple: # cv2.imshow('Mask', frame) # frame = cv2.dilate(frame, None, dst=frame, iterations=1) # frame = cv2.erode(frame, None, dst=frame, iterations=1) # frame = cv2.erode(frame, None, dst=frame, iterations=1) # cv2.imshow('After Ero/Dil', frame) hullList = [] # Convert to RGB to draw contour on - shouldn't recreate every time self.display = cv2.cvtColor(frame, cv2.COLOR_GRAY2BGR, dst=self.display) _, cnts, _ = cv2.findContours(frame, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) if len(cnts) >= 1: acceptable_cnts = [] # Check if the found contour is possibly a target for current_contour in enumerate(cnts): area = cv2.contourArea(current_contour[1]) if PP_MAX_CONTOUR_AREA > area > PP_MIN_CONTOUR_AREA: box = cv2.boundingRect(current_contour[1]) # Convex hull gives the bounding polygon of the contour with no # interior angles greater than 180deg hull = cv2.convexHull(current_contour[1]) hull_area = cv2.contourArea(hull) # If the contour takes up more than X% of the Hull and # width greater than height if (PP_MAX_AREA_RATIO > area / hull_area > PP_MIN_AREA_RATIO and box[2] > box[3]): # print(box) # X,Y,W,H # print("P %.2f, Area %d, Hull, %d" % (area / hull_area, area, hull_area)) acceptable_cnts.append(current_contour[1]) hullList.append(hull) # ***This section of code displays the possible targets*** for i in range(len(acceptable_cnts)): color_G = (0, 255, 0) color_B = (255, 0, 0) cv2.drawContours(self.display, acceptable_cnts, i, color_G) cv2.drawContours(self.display, hullList, i, color_B) if acceptable_cnts: if len(acceptable_cnts) > 1: # Pick the largest found 'power port' power_port_contour = max(acceptable_cnts, key=lambda x: cv2.contourArea(x)) else: power_port_contour = acceptable_cnts[0] power_port_points = get_corners_from_contour( power_port_contour) # x, y, w, h = cv2.boundingRect(power_port_contour) for i in range(4): cv2.circle(self.display, tuple(power_port_points[i][0]), 3, (0, 0, 255)) # cv2.imshow("Display", self.display) # cv2.waitKey() return power_port_points else: return None else: return None def create_annotated_display(self, frame: np.ndarray, points: np.ndarray, printing=False): for i in range(len(points)): cv2.circle(frame, (points[i][0][0], points[i][0][1]), 5, (0, 255, 0)) if printing: print(points) def get_mid(self, contour: np.ndarray) -> tuple: """ Use the cv2 moments to find the centre x of the contour. We just copied it from the opencv reference. The y is just the lowest pixel in the image.""" M = cv2.moments(contour) if M["m00"] != 0: cX = int(M["m10"] / M["m00"]) else: cX = 160 return cX def get_image_values(self, frame: np.ndarray) -> tuple: """Takes a frame, returns a tuple of results, or None.""" self.hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV, dst=self.hsv) self.mask = cv2.inRange(self.hsv, HSV_LOWER_BOUND, HSV_UPPER_BOUND, dst=self.mask) power_port = self.find_power_port(self.mask) self.image = self.mask if power_port is not None: self.prev_dist = self.avg_dist self.prev_horiz_angle = self.avg_horiz_angle self.create_annotated_display(frame, power_port) midX = self.get_mid(power_port) target_top = min(list(power_port[:, :, 1])) target_bottom = max(list(power_port[:, :, 1])) # print("target top: ", target_top, " target bottom: ", target_bottom) horiz_angle = get_horizontal_angle(midX, FRAME_WIDTH, MAX_FOV_WIDTH / 2, True) vert_angles = [ get_vertical_angle_linear(target_bottom, FRAME_HEIGHT, MAX_FOV_HEIGHT / 2, True), get_vertical_angle_linear(target_top, FRAME_HEIGHT, MAX_FOV_HEIGHT / 2, True), ] distances = [ get_distance(vert_angles[0], TARGET_HEIGHT_BOTTOM, CAMERA_HEIGHT, GROUND_ANGLE), get_distance(vert_angles[1], TARGET_HEIGHT_TOP, CAMERA_HEIGHT, GROUND_ANGLE), ] distance = distances[1] vert_angle = vert_angles[1] print("angle: ", math.degrees(vert_angle), " distance: ", distance) self.avg_dist = (distance * (1 - DIST_SMOOTHING_AMOUNT) + self.prev_dist * DIST_SMOOTHING_AMOUNT) self.avg_horiz_angle = ( horiz_angle * (1 - ANGLE_SMOOTHING_AMOUNT) + self.prev_horiz_angle * ANGLE_SMOOTHING_AMOUNT) if self.testing: return (distance, horiz_angle) else: return (self.avg_dist, self.avg_horiz_angle) else: return None def run(self): """Main process function. When ran, takes image, processes image, and sends results to RIO. """ if not self.testing: if self.Connection.using_nt: self.Connection.pong() frame_time, self.frame = self.CameraManager.get_frame(0) if frame_time == 0: print(self.CameraManager.sinks[0].getError(), file=sys.stderr) self.CameraManager.source.notifyError( self.CameraManager.sinks[0].getError()) else: # Flip the image cause originally upside down. self.frame = cv2.rotate(self.frame, cv2.ROTATE_180) results = self.get_image_values(self.frame) if self.Connection.using_nt: self.time = time.monotonic() self.fps = 1 / (self.time - self.old_fps_time) self.old_fps_time = self.time self.Connection.fps_entry.setDouble(self.fps) if results is not None: distance, angle = results self.Connection.send_results( (distance, angle, time.monotonic() )) # distance (meters), angle (radians), timestamp self.CameraManager.send_frame(self.image)
class Vision: """Main vision class. An instance should be created, with test=False (default). As long as the cameras are configured correctly via the GUI interface, everything will work without modification required. This will not work on most machines, so tests of the main process function are the only tests that can be done without a Pi running the FRC vision image. """ entries = None def __init__( self, test_img=None, test_video=None, test_display=False, using_nt=False, ): # self.entries = entries # Memory Allocation self.hsv = np.zeros(shape=(FRAME_WIDTH, FRAME_HEIGHT, 3), dtype=np.uint8) self.image = self.hsv.copy() self.mask = np.zeros(shape=(FRAME_WIDTH, FRAME_HEIGHT), dtype=np.uint8) # Camera Configuration self.CameraManager = CameraManager(test_img=test_img, test_video=test_video, test_display=test_display) self.Connection = Connection(using_nt=using_nt, test=test_video or test_img) self.testing = not (type(test_img) == type(None) or type(test_video) == type(None)) def find_loading_bay(self, frame: np.ndarray): cnts, hierarchy = cv2.findContours(self.mask, cv2.RETR_CCOMP, cv2.CHAIN_APPROX_SIMPLE) cnts = np.array(cnts) hierarchy = np.array(hierarchy)[0] outer_rects = {} inner_rects = {} for i, cnt in enumerate(cnts): if hierarchy[i][3] == -1: outer_rects[i] = ( get_corners_from_contour(cnt), hierarchy[i], cv2.contourArea(cnt), ) else: inner_rects[i] = ( get_corners_from_contour(cnt), hierarchy[i], cv2.contourArea(cnt), ) if not (inner_rects and outer_rects): return None good = [] for i in outer_rects: if outer_rects[i][2] > MIN_CONTOUR_AREA: current_inners = [] next_child = outer_rects[i][1][2] while next_child != -1: current_inners.append(inner_rects[next_child]) next_child = inner_rects[next_child][1][0] largest = max(current_inners, key=lambda x: x[2]) if (abs((outer_rects[i][2] / largest[2]) - INNER_OUTER_RATIO) < 0.5 and abs((cv2.contourArea(outer_rects[i][0]) / outer_rects[i][2]) - 1) < RECT_AREA_RATIO and abs((cv2.contourArea(largest[0]) / largest[2]) - 1) < RECT_AREA_RATIO): good.append((outer_rects[i], largest)) self.image = frame.copy() for pair in good: self.image = cv2.drawContours(self.image, pair[0][0].reshape((1, 4, 2)), -1, (255, 0, 0), thickness=2) self.image = cv2.drawContours( self.image, pair[1][0].reshape((1, 4, 2)), -1, (255, 0, 255), thickness=1, ) return (0.0, 0.0) def find_power_port(self, frame: np.ndarray) -> tuple: _, cnts, _ = cv2.findContours(frame, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) if len(cnts) >= 1: acceptable_cnts = [] for current_contour in enumerate(cnts): area = cv2.contourArea(current_contour[1]) box = cv2.boundingRect(current_contour[1]) hull_area = cv2.contourArea(cv2.convexHull(current_contour[1])) if (area > MIN_CONTOUR_AREA and area / hull_area > 0.2 and box[2] > box[3]): acceptable_cnts.append(current_contour[1]) if acceptable_cnts: power_port_contour = max(acceptable_cnts, key=lambda x: cv2.contourArea(x)) power_port_points = get_corners_from_contour( power_port_contour) # x, y, w, h = cv2.boundingRect(power_port_contour) return power_port_points else: return None else: return None def create_annotated_display(self, frame: np.ndarray, points: np.ndarray, printing=False): for i in range(len(points)): cv2.circle(frame, (points[i][0][0], points[i][0][1]), 5, (0, 255, 0)) if printing == True: print(points) def get_vertical_angle(self, p: int): """Gets angle of point p above the horizontal. Parameter p should have 0 at the bottom of the frame and FRAME_HEIGHT at the top. """ return math.atan2(p - FRAME_HEIGHT, FY) # get_angle and get_distance will be replaced with solve pnp eventually def get_horizontal_angle(self, X: float) -> float: return ((X / FRAME_WIDTH) - 0.5) * MAX_FOV_WIDTH # 33.18 degrees #gets the angle def get_distance(self, Y: float) -> float: target_angle = self.get_vertical_angle(Y) # print(f"Total Angle: {math.degrees(target_angle + GROUND_ANGLE)}") return (TARGET_HEIGHT - CAMERA_HEIGHT) / math.tan(GROUND_ANGLE + target_angle) def get_middles(self, contour: np.ndarray) -> tuple: """ Use the cv2 moments to find the centre x of the contour. We just copied it from the opencv reference. The y is just the lowest pixel in the image.""" M = cv2.moments(contour) if M["m00"] != 0: cX = int(M["m10"] / M["m00"]) else: cX = 160 cY = max(list(contour[:, :, 1])) return cX, cY def get_image_values(self, frame: np.ndarray) -> tuple: """Takes a frame, returns a tuple of results, or None.""" self.hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV, dst=self.hsv) self.mask = cv2.inRange(self.hsv, HSV_LOWER_BOUND, HSV_UPPER_BOUND, dst=self.mask) self.mask = cv2.erode(self.mask, None, dst=self.mask, iterations=1) self.mask = cv2.dilate(self.mask, None, dst=self.mask, iterations=2) self.mask = cv2.erode(self.mask, None, dst=self.mask, iterations=1) power_port = self.find_power_port(self.mask) self.image = self.mask if power_port is not None: self.create_annotated_display(frame, power_port) midX, midY = self.get_middles(power_port) angle = self.get_horizontal_angle(midX) distance = self.get_distance(midY) return (distance, angle) else: return None def run(self): """Main process function. When ran, takes image, processes image, and sends results to RIO. """ if self.Connection.using_nt: self.Connection.pong() frame_time, self.frame = self.CameraManager.get_frame(0) if frame_time == 0: print(self.CameraManager.sinks[0].getError(), file=sys.stderr) self.CameraManager.source.notifyError( self.CameraManager.sinks[0].getError()) else: self.frame = cv2.rotate(self.frame, cv2.ROTATE_180) results = self.get_image_values(self.frame) if results is not None: distance, angle = results self.Connection.send_results( (distance, angle, time.monotonic() )) # distance (meters), angle (radians), timestamp self.CameraManager.send_frame(self.image) def translate( self, value, leftMin, leftMax, rightMin, rightMax ): # https://stackoverflow.com/questions/1969240/mapping-a-range-of-values-to-another # Figure out how 'wide' each range is leftSpan = leftMax - leftMin rightSpan = rightMax - rightMin # Convert the left range into a 0-1 range (float) valueScaled = float(value - leftMin) / float(leftSpan) # Convert the 0-1 range into a value in the right range. return rightMin + (valueScaled * rightSpan)