示例#1
0
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)
示例#2
0
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)