示例#1
0
文件: camera.py 项目: tarora2/seawolf
    def get_frame(self):
        '''Gets a frame from the camera.'''
        self.frame_count += 1

        if not self.capture and not self.image and not self.dc1394_capture:
            self.open_capture()

        if self.image:
            return cv.CloneImage(self.image)

        if self.dc1394_capture:
            frame = self.dc1394_capture.get_frame()
        else:
            frame = cv.QueryFrame(self.capture)
        # If the capture device doesn't work, OpenCV might not complain, but
        # just returns None when grabbing a frame.  Here we check for errors,
        # and see if it was actualy an image, not a video that we're opening
        if not frame:

            # See if file is an image, not video
            if isinstance(self.identifier, basestring):
                try:
                    self.image = cv.LoadImage(self.identifier)
                except IOError:
                    if self.frame_count > 1:
                        raise self.CaptureError(
                            "The video has run out of frames.")
                    else:
                        raise self.CaptureError(
                            "Either a read error occured "
                            "with the file, or the file is not a valid video "
                            "or image file.")
                if self.image:
                    return cv.CloneImage(self.image)
                else:
                    raise self.CaptureError(
                        "This shouldn't happen.  Please "
                        " report a bug!  Include this traceback!!")
            else:
                raise self.CaptureError("Could not capture frame from "
                                        'identifier "%s"' % self.identifier)

            # TODO: Weird GStreamer error occured when a folder is incorrectly
            #      specified as a video file, or when the user doesn't have
            #      permissions for the file.

        #TODO: doesn't work. svr.debug doesn't accept debug request?
        if self.display:
            #cv.ShowImage(self.get_window_name(), frame)
            svr.debug(self.get_window_name(), frame)

        if self.record_path:
            filename = os.path.join(self.record_path,
                                    "%s.jpg" % self.frame_count)
            cv.SaveImage(filename, frame)

        return frame
示例#2
0
    def process_frame(self, frame):
        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        gray = cv2.equalizeHist(gray)
        self.candidates = self.cascade.detectMultiScale(
            gray, scaleFactor=1.3, minNeighbors=4, minSize=(30, 30), flags = cv2.CASCADE_SCALE_IMAGE)

        self.candiates = cv2.detect(gray, self.cascade)
        vis = frame.copy()
        self.draw_rects(vis, self.candidates, (0, 255, 0))

        svr.debug('facedetect', vis)
示例#3
0
    def get_frame(self):
        '''Gets a frame from the camera.'''
        self.frame_count += 1

        if not self.capture and not self.image and not self.dc1394_capture:
            self.open_capture()

        if self.image:
            return cv.CloneImage(self.image)

        if self.dc1394_capture:
            frame = self.dc1394_capture.get_frame()
        else:
            frame = cv.QueryFrame(self.capture)
        # If the capture device doesn't work, OpenCV might not complain, but
        # just returns None when grabbing a frame.  Here we check for errors,
        # and see if it was actualy an image, not a video that we're opening
        if not frame:

            # See if file is an image, not video
            if isinstance(self.identifier, basestring):
                try:
                    self.image = cv.LoadImage(self.identifier)
                except IOError:
                    if self.frame_count > 1:
                        raise self.CaptureError("The video has run out of frames.")
                    else:
                        raise self.CaptureError("Either a read error occured "
                                                "with the file, or the file is not a valid video "
                                                "or image file.")
                if self.image:
                    return cv.CloneImage(self.image)
                else:
                    raise self.CaptureError("This shouldn't happen.  Please "
                                            " report a bug!  Include this traceback!!")
            else:
                raise self.CaptureError("Could not capture frame from "
                                        'identifier "%s"' % self.identifier)

            # TODO: Weird GStreamer error occured when a folder is incorrectly
            #      specified as a video file, or when the user doesn't have
            #      permissions for the file.

        if self.display:
            #cv.ShowImage(self.get_window_name(), frame)
            svr.debug(self.get_window_name(), frame)

        if self.record_path:
            filename = os.path.join(self.record_path, "%s.jpg" % self.frame_count)
            cv.SaveImage(filename, frame)

        return frame
示例#4
0
    def print_frame(self, name, frame, on_svr=False):
        """prints out the given frame locally, or offers to
        stream the image via svr."""

        name = "print{}: {}".format(self.step, name)

        # print using svr
        if on_svr:
            svr.debug(name, frame)

        # print using openCV
        else:
            self.debug_stream(name, frame)

        # increment step counter
        self.step += 1
示例#5
0
 def show(self):
    
   
   #adding positive noise to the frame
   noise =  np.random.normal(loc = 0, scale = 4, size = self.frame.shape)
   noise = np.array(np.array([50, 50, 20]) * np.ones(self.frameSize) + noise,self.frame.dtype)
   self.frame = cv2.add(self.frame, noise)
 
   #cv2.imshow(self.name, self.frame)
   
   #cv2.imwrite("./PICS/" + self.name + str(self.count) + ".png", self.frame)
   self.count+= 1
   #converting image to type desired by svr
   container = cv2.cv.fromarray(np.copy(self.frame))
   cv_image = cv2.cv.GetImage(container)
   svr.debug(self.name, cv_image)
示例#6
0
    def print_frame(self, name, frame, on_svr=False):
        """prints out the given frame locally, or offers to
        stream the image via svr."""

        name = "print{}: {}".format(self.step, name)

        # print using svr
        if on_svr:
            svr.debug(name, frame)

        # print using openCV
        else:   
            self.debug_stream(name, frame)

        # increment step counter
        self.step += 1
示例#7
0
    def process_frame(self, frame, debug=True):

        # Scale image to reduce processing
        #scale_in_place(frame, (frame.width*0.7, frame.height*0.7))
        if debug:
            debug_frame = cv.CloneImage(frame)
        else:
            debug_frame = False

        # Searching State
        # Search for buoys, then move to tracking when they are found
        if self.state == "searching":
            trackers = self.initial_search(frame, debug_frame)
            if trackers:
                self.trackers = trackers
                self.state = "tracking"

                try:
                    svr.debug_close("Saturation")
                    svr.debug_close("Red")
                    svr.debug_close("AdaptiveThreshold")
                except:
                    pass

        # Tracking State
        num_buoys_found = 0
        if self.state == "tracking":
            num_buoys_found, locations = self.buoy_track(
                frame, self.trackers, debug_frame)
            if num_buoys_found > 0:
                self.buoy_locations = locations

        if debug:
            svr.debug("Buoy2011", debug_frame)

        # Convert to output format
        self.output.buoys = []
        for location in self.buoy_locations:
            buoy = Container()
            buoy.theta = location[0] * (34 / (frame.width / 2))
            buoy.phi = location[1] * (30 / (frame.height / 2)
                                      )  # Complete guess on vertical fov
            buoy.id = 1
            self.output.buoys.append(buoy)

        self.return_output()
示例#8
0
文件: buoy.py 项目: tarora2/seawolf
    def process_frame(self, frame):
        buoy_locations = []

        # Scale image to reduce processing
        '''
        scale = 0.7
        frame_scaled = cv.CreateImage((int(frame.width*scale), int(frame.height*scale)), 8, 3)
        cv.Resize(frame, frame_scaled)
        cv.SetImageROI(frame, (0, 0, int(frame.width*scale), int(frame.height*scale)))
        '''

        # Create debug_frame
        if self.debug:
            debug_frame = cv.CloneImage(frame)
        else:
            debug_frame = False

        # Look for buoys if there aren't any trackers yet
        if not self.trackers:
            self.trackers = self.initial_search(frame, debug_frame)
            if self.trackers:

                buoy_locations = map(lambda x: adjust_location(x.object_center, frame.width, frame.height), self.trackers)
                if debug_frame:
                    svr.debug("Buoy", debug_frame)
                return

        # Tracking
        else:
            num_buoys_found, buoy_locations = self.buoy_track(frame, self.trackers, debug_frame)

        if debug_frame:
            svr.debug("Buoy", debug_frame)

        # Convert to output format
        self.output.buoys = []
        for location in buoy_locations:
            buoy = Container()
            buoy.theta = location[0]
            buoy.phi = location[1]
            buoy.id = 1
            self.output.buoys.append(buoy)

        if self.output.buoys:
            self.return_output()
        return self.output
示例#9
0
    def process_frame(self, frame):
        buoy_locations = []

        # Create debug_frame
        if self.debug:
            debug_frame = cv.CloneImage(frame)
        else:
            debug_frame = False

        # Look for buoys if there aren't any trackers yet
        if not self.trackers:
            self.trackers = self.initial_search(frame, debug_frame)
            if self.trackers:

                buoy_locations = map(lambda x: adjust_location(x.object_center, frame.width, frame.height), self.trackers)
                if debug_frame:
                    svr.debug("Buoy", debug_frame)
                return

        # Tracking
        else:
            num_buoys_found, buoy_locations = self.buoy_track(frame, self.trackers, debug_frame)

            # Debug info
            if debug_frame:
                for tracker in self.trackers:
                    print "Drawing Circle!!!!", tracker.object_center
                    cv.Circle(debug_frame, (int(tracker.object_center[0]), int(tracker.object_center[1])), tracker.size[0], (0, 0, 255), 2)

        if debug_frame:
            svr.debug("Buoy", debug_frame)

        # Convert to output format
        self.output.buoys = []
        for location in buoy_locations:
            buoy = Container()
            buoy.theta = location[0]
            buoy.phi = location[1]
            buoy.id = 1
            self.output.buoys.append(buoy)

        if self.output.buoys:
            self.return_output()
        return self.output
示例#10
0
    def process_frame(self, frame, debug=True):

        # Scale image to reduce processing
        #scale_in_place(frame, (frame.width*0.7, frame.height*0.7))
        if debug:
            debug_frame = cv.CloneImage(frame)
        else:
            debug_frame = False

        # Searching State
        # Search for buoys, then move to tracking when they are found
        if self.state == "searching":
            trackers = self.initial_search(frame, debug_frame)
            if trackers:
                self.trackers = trackers
                self.state = "tracking"

                try:
                    svr.debug_close("Saturation")
                    svr.debug_close("Red")
                    svr.debug_close("AdaptiveThreshold")
                except:
                    pass

        # Tracking State
        num_buoys_found = 0
        if self.state == "tracking":
            num_buoys_found, locations = self.buoy_track(frame, self.trackers, debug_frame)
            if num_buoys_found > 0:
                self.buoy_locations = locations

        if debug:
            svr.debug("Buoy2011", debug_frame)

        # Convert to output format
        self.output.buoys = []
        for location in self.buoy_locations:
            buoy = Container()
            buoy.theta = location[0] * (34 / (frame.width / 2))
            buoy.phi = location[1] * (30 / (frame.height / 2))  # Complete guess on vertical fov
            buoy.id = 1
            self.output.buoys.append(buoy)

        self.return_output()
示例#11
0
    def process_frame(self, frame):
        '''Process the frame and search for path markers.

        Keyword Arguments:
        frame -- image in cv format to process
        '''
        binary_img = self.preprocess(frame)

        grad = self.get_gradient(binary_img)
        hough_p = self.hough_p(grad)

        # Do not continue if hough_p is None type, it will cause errors.
        if hough_p is None:
            self.output = []
            svr.debug("Paths", frame)
            return

        groups = self.group_lines(hough_p)
        self.output = groups
        self.do_debug(frame, groups)
        return
示例#12
0
 def do_debug(self, frame, groups):
     '''Create some feedback for people to look at for debugging.  Input is
     really what every we happen to need or want to test.
     
     First this function finds the location of the center of the frame.  Then
     it uses sin and cos to produce endpoints for lines showing each angle in
     'groups'.
     
     Keyword Arguments:
     frame -- original image
     '''
     width, height = cv.GetSize(frame)
     center_x = width // 2
     center_y = height // 2
     radius = (center_x ** 2 + center_y ** 2) ** .5
     for theta in groups:
         x_off = np.int32(radius * math.cos(theta))
         y_off = np.int32(radius * math.sin(theta))
         cv.Line(frame, (center_x - x_off, center_x - y_off),\
             (center_x + x_off, center_y + y_off), (255, 255, 255))
         
     svr.debug("Paths", frame)
示例#13
0
    def process_frame(self, frame):

        #----------- BLOCK 1: FIND NEW BUOYS ---------- #

        # find new buoys
        new_buoys = self.find_buoys(frame)

        # send found buoys back to manager
        buoy_data = WorkerData()
        buoy_data.new_buoys = new_buoys
        self.send_message(buoy_data)

        #---------- BLOCK 2: DEBUG ----------- #
        if self.debug:

            # wait for debug information from parent
            debug_info = self.wait_for_parent(None)

            # display confirmed buoys
            for confirmed in debug_info.confirmed:

                if debug_info.camera == LEFT:
                    x = confirmed.lx
                    y = confirmed.ly

                if debug_info.camera == RIGHT:
                    x = confirmed.rx
                    y = confirmed.ry

                w = confirmed.width

                x = int(x)
                y = int(y)

                # draw rectangles on frame
                cv.Rectangle(frame, (x, y), (x + w, y + w), confirmed.debug_color, thickness=6)

            # show debug frame
            svr.debug("Buoy" + self.camera_name, frame)
示例#14
0
    def find_blobs(self, frame, debug_image):
        '''Find blobs in an image.

        Hopefully this gets blobs that correspond with
        buoys, but any intelligent checking is done outside of this function.

        '''

        # Get Channels
        hsv = cv.CreateImage(cv.GetSize(frame), 8, 3)
        cv.CvtColor(frame, hsv, cv.CV_BGR2HSV)
        saturation = libvision.misc.get_channel(hsv, 1)
        red = libvision.misc.get_channel(frame, 2)

        # Adaptive Threshold
        cv.AdaptiveThreshold(
            saturation,
            saturation,
            255,
            cv.CV_ADAPTIVE_THRESH_MEAN_C,
            cv.CV_THRESH_BINARY_INV,
            self.saturation_adaptive_thresh_blocksize -
            self.saturation_adaptive_thresh_blocksize % 2 + 1,
            self.saturation_adaptive_thresh,
        )
        cv.AdaptiveThreshold(
            red,
            red,
            255,
            cv.CV_ADAPTIVE_THRESH_MEAN_C,
            cv.CV_THRESH_BINARY,
            self.red_adaptive_thresh_blocksize -
            self.red_adaptive_thresh_blocksize % 2 + 1,
            -1 * self.red_adaptive_thresh,
        )

        kernel = cv.CreateStructuringElementEx(9, 9, 4, 4, cv.CV_SHAPE_ELLIPSE)
        cv.Erode(saturation, saturation, kernel, 1)
        cv.Dilate(saturation, saturation, kernel, 1)
        cv.Erode(red, red, kernel, 1)
        cv.Dilate(red, red, kernel, 1)

        buoys_filter = cv.CreateImage(cv.GetSize(frame), 8, 1)
        cv.And(saturation, red, buoys_filter)

        if debug_image:
            svr.debug("Saturation", saturation)
            svr.debug("Red", red)
            svr.debug("AdaptiveThreshold", buoys_filter)

        # Get blobs
        labeled_image = cv.CreateImage(cv.GetSize(buoys_filter), 8, 1)
        blobs = libvision.blob.find_blobs(buoys_filter, labeled_image,
                                          MIN_BLOB_SIZE, 10)

        return blobs, labeled_image
示例#15
0
    def find_blobs(self, frame, debug_image):
        '''Find blobs in an image.

        Hopefully this gets blobs that correspond with
        buoys, but any intelligent checking is done outside of this function.

        '''

        # Get Channels
        hsv = cv.CreateImage(cv.GetSize(frame), 8, 3)
        cv.CvtColor(frame, hsv, cv.CV_BGR2HSV)
        saturation = libvision.misc.get_channel(hsv, 1)
        red = libvision.misc.get_channel(frame, 2)

        # Adaptive Threshold
        cv.AdaptiveThreshold(saturation, saturation,
                             255,
                             cv.CV_ADAPTIVE_THRESH_MEAN_C,
                             cv.CV_THRESH_BINARY_INV,
                             self.saturation_adaptive_thresh_blocksize - self.saturation_adaptive_thresh_blocksize % 2 + 1,
                             self.saturation_adaptive_thresh,
                             )
        cv.AdaptiveThreshold(red, red,
                             255,
                             cv.CV_ADAPTIVE_THRESH_MEAN_C,
                             cv.CV_THRESH_BINARY,
                             self.red_adaptive_thresh_blocksize - self.red_adaptive_thresh_blocksize % 2 + 1,
                             -1 * self.red_adaptive_thresh,
                             )

        kernel = cv.CreateStructuringElementEx(9, 9, 4, 4, cv.CV_SHAPE_ELLIPSE)
        cv.Erode(saturation, saturation, kernel, 1)
        cv.Dilate(saturation, saturation, kernel, 1)
        cv.Erode(red, red, kernel, 1)
        cv.Dilate(red, red, kernel, 1)

        buoys_filter = cv.CreateImage(cv.GetSize(frame), 8, 1)
        cv.And(saturation, red, buoys_filter)

        if debug_image:
            svr.debug("Saturation", saturation)
            svr.debug("Red", red)
            svr.debug("AdaptiveThreshold", buoys_filter)

        # Get blobs
        labeled_image = cv.CreateImage(cv.GetSize(buoys_filter), 8, 1)
        blobs = libvision.blob.find_blobs(buoys_filter, labeled_image, MIN_BLOB_SIZE, 10)

        return blobs, labeled_image
示例#16
0
    def process_frame(self, frame):
        found_path = False
        cv.Smooth(frame, frame, cv.CV_MEDIAN, 7, 7)

        # use RGB color finder
        binary = libvision.cmodules.target_color_rgb.find_target_color_rgb(
            frame, self.R, self.G, self.B, self.min_blob_size, self.dev_thresh,
            self.precision / 1000.0)

        if self.debug:
            color_filtered = cv.CloneImage(binary)
            svr.debug("color_filtered", color_filtered)

        # Get Edges
        cv.Canny(binary, binary, 30, 40)

        # Hough Transform
        line_storage = cv.CreateMemStorage()
        lines = cv.HoughLines2(binary,
                               line_storage,
                               cv.CV_HOUGH_STANDARD,
                               rho=1,
                               theta=math.pi / 180,
                               threshold=self.hough_threshold,
                               param1=0,
                               param2=0)
        lines = lines[:self.lines_to_consider]  # Limit number of lines

        # If there are at least 2 lines and they are close to parallel...
        # There's a path!
        #print lines
        if len(lines) >= 2:

            # Find: min, max, average
            theta_max = lines[0][1]
            theta_min = lines[0][1]
            total_theta = 0
            for rho, theta in lines:
                total_theta += theta
                if theta_max < theta:
                    theta_max = theta
                if theta_min > theta:
                    theta_min = theta

            theta_range = theta_max - theta_min
            # Near vertical angles will wrap around from pi to 0.  If the range
            # crosses this vertical line, the range will be way too large.  To
            # correct for this, we always take the smallest angle between the
            # min and max.
            if theta_range > math.pi / 2:
                theta_range = math.pi - theta_range

            if theta_range < self.theta_threshold:
                found_path = True

                angles = map(lambda line: line[1], lines)
                self.theta = circular_average(angles, math.pi)

        print found_path
        if found_path:
            self.seen_in_a_row += 1
        else:
            self.seen_in_a_row = 0

        # stores whether or not we are confident about the path's presence
        object_present = False
        print self.seen_in_a_row  ###

        if self.seen_in_a_row >= self.seen_in_a_row_threshold:
            object_present = True
            self.image_coordinate_center = self.find_centroid(binary)
            # Move the origin to the center of the image
            self.center = (self.image_coordinate_center[0] - frame.width / 2,
                           self.image_coordinate_center[1] * -1 +
                           frame.height / 2)

        if self.debug:

            # Show color filtered
            color_filtered_rgb = cv.CreateImage(cv.GetSize(frame), 8, 3)
            cv.CvtColor(color_filtered, color_filtered_rgb, cv.CV_GRAY2RGB)
            cv.SubS(color_filtered_rgb, (255, 0, 0), color_filtered_rgb)
            cv.Sub(frame, color_filtered_rgb, frame)

            # Show edges
            binary_rgb = cv.CreateImage(cv.GetSize(frame), 8, 3)
            cv.CvtColor(binary, binary_rgb, cv.CV_GRAY2RGB)
            cv.Add(frame, binary_rgb, frame)  # Add white to edge pixels
            cv.SubS(binary_rgb, (0, 0, 255), binary_rgb)
            cv.Sub(frame, binary_rgb, frame)  # Remove all but Red

            # Show lines
            if self.seen_in_a_row >= self.seen_in_a_row_threshold:
                rounded_center = (
                    int(round(self.image_coordinate_center[0])),
                    int(round(self.image_coordinate_center[1])),
                )
                cv.Circle(frame, rounded_center, 5, (0, 255, 0))
                libvision.misc.draw_lines(frame,
                                          [(frame.width / 2, self.theta)])
            else:
                libvision.misc.draw_lines(frame, lines)

            #cv.ShowImage("Path", frame)
            svr.debug("Path", frame)

        # populate self.output with infos
        self.output.found = object_present
        self.output.theta = self.theta

        if self.center:
            # scale center coordinates of path based on frame size
            self.output.x = self.center[0] / (frame.width / 2)
            self.output.y = self.center[1] / (frame.height / 2)
            libvision.misc.draw_linesC(frame,
                                       [(frame.width / 2, self.output.theta)],
                                       [255, 0, 255])
            print "Output Returned!!! ", self.output.theta
        else:
            self.output.x = None
            self.output.y = None
            print "No output..."

        if self.output.found and self.center:
            print self.output

        self.return_output()
示例#17
0
    def process_frame(self, frame):

        # Resize image to 320x240
        #copy = cv.CreateImage(cv.GetSize(frame), 8, 3)
        #cv.Copy(frame, copy)
        #cv.SetImageROI(frame, (0, 0, 320, 240))
        #cv.Resize(copy, frame, cv.CV_INTER_NN)

        found_hedge = False

        test_frame = cv.CreateImage(cv.GetSize(frame), 8, 3)

        cv.Copy(frame, test_frame)

        cv.Smooth(frame, frame, cv.CV_MEDIAN, 7, 7)

        # Set binary image to have value channel
        hsv = cv.CreateImage(cv.GetSize(frame), 8, 3)
        binary = cv.CreateImage(cv.GetSize(frame), 8, 1)
        cv.CvtColor(frame, hsv, cv.CV_BGR2HSV)
        cv.SetImageCOI(hsv, 2)
        cv.Copy(hsv, binary)
        cv.SetImageCOI(hsv, 0)

        cv.AdaptiveThreshold(
            binary,
            binary,
            255,
            cv.CV_ADAPTIVE_THRESH_MEAN_C,
            cv.CV_THRESH_BINARY_INV,
            self.adaptive_thresh_blocksize,
            self.adaptive_thresh,
        )

        # Morphology

        kernel = cv.CreateStructuringElementEx(3, 3, 1, 1, cv.CV_SHAPE_ELLIPSE)
        #cv.Erode(binary, binary, kernel, 1)
        cv.Dilate(binary, binary, kernel, 4)

        if self.debug:
            color_filtered = cv.CloneImage(binary)

        # Get Edges
        #cv.Canny(binary, binary, 30, 40)

        # Hough Transform
        '''
        line_storage = cv.CreateMemStorage()
        raw_lines = cv.HoughLines2(binary, line_storage, cv.CV_HOUGH_STANDARD,
            rho=1,
            theta=math.pi/180,
            threshold=self.hough_threshold,
            param1=0,
            param2=0
        )
        '''
        # Hough Transform
        line_storage = cv.CreateMemStorage()
        raw_lines = cv.HoughLines2(binary,
                                   line_storage,
                                   cv.CV_HOUGH_PROBABILISTIC,
                                   rho=1,
                                   theta=math.pi / 180,
                                   threshold=self.hough_threshold,
                                   param1=self.min_length,
                                   param2=self.max_gap)

        self.hor_lines = []

        for line in raw_lines:
            slope = line_slope(line[0], line[1])
            if slope is None:
                continue
            if math.fabs(line_slope(line[0], line[1])) < self.hor_threshold:
                self.hor_lines.append(line)

        max_length = 0

        for line in self.hor_lines:
            print line
            if math.fabs(line_distance(line[0], line[1])) > max_length:
                max_length = math.fabs(line_distance(line[0], line[1]))
                crossbar_seg = line
        '''
        # Get vertical lines
        vertical_lines = []
        for line in raw_lines:
            if line[1] < self.vertical_threshold or \
                line[1] > math.pi-self.vertical_threshold:

                vertical_lines.append( (abs(line[0]), line[1]) )

        # Group vertical lines
        vertical_line_groups = []  # A list of line groups which are each a line list
        for line in vertical_lines:
            group_found = False
            for line_group in vertical_line_groups:

                if line_group_accept_test(line_group, line, self.max_range):
                    line_group.append(line)
                    group_found = True

            if not group_found:
                vertical_line_groups.append([line])

        # Average line groups into lines
        vertical_lines = []
        for line_group in vertical_line_groups:
            rhos = map(lambda line: line[0], line_group)
            angles = map(lambda line: line[1], line_group)
            line = (sum(rhos)/len(rhos), circular_average(angles, math.pi))
            vertical_lines.append(line)

        # Get horizontal lines
        horizontal_lines = []
        for line in raw_lines:
            dist_from_horizontal = (math.pi/2 + line[1]) % math.pi
            if dist_from_horizontal < self.horizontal_threshold or \
                dist_from_horizontal > math.pi-self.horizontal_threshold:

                horizontal_lines.append( (abs(line[0]), line[1]) )

        # Group horizontal lines
        horizontal_line_groups = []  # A list of line groups which are each a line list
        for line in horizontal_lines:
            group_found = False
            for line_group in horizontal_line_groups:

                if line_group_accept_test(line_group, line, self.max_range):
                    line_group.append(line)
                    group_found = True

            if not group_found:
                horizontal_line_groups.append([line])

        if len(horizontal_line_groups) is 1:
            self.seen_crossbar = True
            rhos = map(lambda line: line[0], horizontal_line_groups[0])
            angles = map(lambda line: line[1], horizontal_line_groups[0])
            line = (sum(rhos)/len(rhos), circular_average(angles, math.pi))
            horizontal_lines = [line]
        else:
            self.seen_crossbar = False
            horizontal_lines = []

        self.left_pole = None
        self.right_pole = None
        if len(vertical_lines) is 2:
            roi = cv.GetImageROI(frame)
            width = roi[2]
            height = roi[3]
            self.left_pole = round(min(vertical_lines[0][0], vertical_lines[1][0]), 2) - width/2
            self.right_pole = round(max(vertical_lines[0][0], vertical_lines[1][0]), 2) - width/2
        #TODO: If one pole is seen, is it left or right pole?

        # Calculate planar distance r (assuming we are moving perpendicular to
        # the hedge)
        if self.left_pole and self.right_pole:
            theta = abs(self.left_pole - self.right_pole)
            self.r = 3 / tan(radians(theta/2))
        else:
            self.r = None

        if self.r and self.seen_crossbar:
            bar_phi = (-1*horizontal_lines[0][0] + frame.height/2) / (frame.height/2) * 32
            self.crossbar_depth = self.r * atan(radians(bar_phi))
        else:
            self.crossbar_depth = None
        '''
        self.left_pole = None
        self.right_pole = None
        self.seen_crossbar = False
        self.crossbar_depth = None

        if self.debug and max_length != 0:
            cv.CvtColor(color_filtered, frame, cv.CV_GRAY2RGB)

            #libvision.misc.draw_lines(frame, vertical_lines)
            #libvision.misc.draw_lines(frame, horizontal_lines)
            # for line in raw_lines:
            #    cv.Line(frame,line[0],line[1], (255,255,0), 10, cv.CV_AA, 0)
            #    cv.Circle(frame, line[1], 15, (255,0,0), 2,8,0)
            # print len(raw_lines)
            #cv.ShowImage("Hedge", cv.CloneImage(frame))
            if (crossbar_seg[0][0] - frame.width / 2) * 37 / (
                    frame.width / 2) < (crossbar_seg[1][0] - frame.width /
                                        2) * 37 / (frame.width / 2):
                self.left_pole = round((crossbar_seg[0][0] - frame.width / 2) *
                                       37 / (frame.width / 2))
                self.right_pole = round(
                    (crossbar_seg[1][0] - frame.width / 2) * 37 /
                    (frame.width / 2))
            else:
                self.left_pole = round((crossbar_seg[1][0] - frame.width / 2) *
                                       37 / (frame.width / 2))
                self.right_pole = round(
                    (crossbar_seg[0][0] - frame.width / 2) * 37 /
                    (frame.width / 2))
            self.crossbar_depth = round(
                -1 * (crossbar_seg[1][1] - frame.height / 2) * 36 /
                (frame.height / 2))

            if math.fabs(self.left_pole) <= 37 and math.fabs(
                    self.left_pole) >= self.frame_boundary_thresh:
                self.left_pole = None
            if math.fabs(self.right_pole) <= 37 and math.fabs(
                    self.right_pole) >= self.frame_boundary_thresh:
                self.right_pole = None

            self.seen_crossbar = True

            if self.left_pole and self.right_pole:

                self.returning = (self.left_pole + self.right_pole) / 2
                print "Returning ", self.returning

                if self.last_seen < 0:
                    self.last_center = None
                    self.last_seen = 0
                if self.last_center is None:
                    self.last_center = self.returning
                    self.seen_count = 1
                elif math.fabs(self.last_center -
                               self.returning) < self.center_trans_thresh:
                    self.seen_count += 1
                    self.last_seen += 2
                else:
                    self.last_seen -= 1

                if self.seen_count < self.seen_count_thresh:
                    self.left_pole = None
                    self.right_pole = None
                else:
                    print "FOUND CENTER AND RETURNED IT"
                    self.found = True
            else:
                self.returning = 0
                if self.last_seen < 0:
                    self.last_center = None
                    self.last_seen = 0
                self.last_seen -= 1
                self.left_pole = None
                self.right_pole = None

            cv.Line(frame, crossbar_seg[0], crossbar_seg[1], (255, 255, 0), 10,
                    cv.CV_AA, 0)
            if self.left_pole and crossbar_seg[0][0] < crossbar_seg[1][0]:

                cv.Line(frame, crossbar_seg[0],
                        (crossbar_seg[0][0], crossbar_seg[0][0] - 500),
                        (255, 0, 0), 10, cv.CV_AA, 0)
            elif self.left_pole:
                cv.Line(frame, crossbar_seg[1],
                        (crossbar_seg[1][0], crossbar_seg[1][1] - 500),
                        (255, 0, 0), 10, cv.CV_AA, 0)

            if self.right_pole and crossbar_seg[0][0] > crossbar_seg[1][0]:

                cv.Line(frame, crossbar_seg[0],
                        (crossbar_seg[0][0], crossbar_seg[0][0] - 500),
                        (255, 0, 0), 10, cv.CV_AA, 0)
            elif self.right_pole:
                cv.Line(frame, crossbar_seg[1],
                        (crossbar_seg[1][0], crossbar_seg[1][1] - 500),
                        (255, 0, 0), 10, cv.CV_AA, 0)

            # populate self.output with infos
            self.output.seen_crossbar = self.seen_crossbar
            self.output.left_pole = self.left_pole
            self.output.right_pole = self.right_pole
            #self.output.r = self.r
            self.output.crossbar_depth = self.crossbar_depth

            self.return_output()
            print self
        else:
            cv.CvtColor(color_filtered, frame, cv.CV_GRAY2RGB)

        svr.debug("Hedge", cv.CloneImage(frame))
        svr.debug("Hedge2", test_frame)
示例#18
0
    def process_frame(self, frame):
        ################
        #setup CV ######
        ################
        print "processing frame"
        (w, h) = cv.GetSize(frame)

        #generate hue selection frames
        ones = np.ones((h, w, 1), dtype='uint8')
        a = ones * (180 - self.target_hue)
        b = ones * (180 - self.target_hue + 20)
        a_array = cv.fromarray(a)
        b_array = cv.fromarray(b)

        #create locations for the test frame and binary frame
        frametest = cv.CreateImage(cv.GetSize(frame), 8, 3)
        binarytest = cv.CreateImage(cv.GetSize(frame), 8, 1)

        #use the red channel for the binary frame (just for debugging purposes)
        cv.Copy(frame, frametest)
        cv.SetImageCOI(frametest, 3)
        cv.Copy(frametest, binarytest)

        #reset the COI for test frame to RGB.
        cv.SetImageCOI(frametest, 0)

        # Resize image to 320x240
        #copy = cv.CreateImage(cv.GetSize(frame), 8, 3)
        #cv.Copy(frame, copy)
        #cv.SetImageROI(frame, (0, 0, 320, 240))
        #cv.Resize(copy, frame, cv.CV_INTER_NN)
        found_gate = False

        #create a new frame for comparison purposes
        unchanged_frame = cv.CreateImage(cv.GetSize(frame), 8, 3)
        cv.Copy(frame, unchanged_frame)

        #apply noise filter #1
        cv.Smooth(frame, frame, cv.CV_MEDIAN, 7, 7)

        # Set binary image to have saturation channel
        hsv = cv.CreateImage(cv.GetSize(frame), 8, 3)
        binary = cv.CreateImage(cv.GetSize(frame), 8, 1)
        cv.CvtColor(frame, hsv, cv.CV_BGR2HSV)
        cv.SetImageCOI(hsv, 1)
        cv.Copy(hsv, binary)

        #spin the color wheel (psuedo-code for later if necessary)
        # truncate spectrum marked as end
        # shift all values up based on truncating value (mask out 0 regions)
        # take truncated bits, and flip them (180->0, 179->1...)
        # dnow that truncated bits are flipped, add them back in to final image

        #Reset hsv COI
        cv.SetImageCOI(hsv, 0)

        #correct for wraparound on red spectrum
        cv.InRange(binary, a_array, b_array, binarytest)  #generate mask
        cv.Add(binary, cv.fromarray(ones * 180), binary,
               mask=binarytest)  #use mask to selectively add values

        #run adaptive threshold for edge detection
        cv.AdaptiveThreshold(
            binary,
            binary,
            255,
            cv.CV_ADAPTIVE_THRESH_MEAN_C,
            cv.CV_THRESH_BINARY_INV,
            self.adaptive_thresh_blocksize,
            self.adaptive_thresh,
        )

        # Morphology
        kernel = cv.CreateStructuringElementEx(5, 5, 3, 3, cv.CV_SHAPE_ELLIPSE)
        cv.Erode(binary, binary, kernel, 1)
        cv.Dilate(binary, binary, kernel, 1)
        if self.debug:
            color_filtered = cv.CloneImage(binary)

        # Get Edges
        cv.Canny(binary, binary, 30, 40)

        # Hough Transform
        line_storage = cv.CreateMemStorage()
        raw_lines = cv.HoughLines2(binary,
                                   line_storage,
                                   cv.CV_HOUGH_STANDARD,
                                   rho=1,
                                   theta=math.pi / 180,
                                   threshold=self.hough_threshold,
                                   param1=0,
                                   param2=0)

        # Get vertical lines
        vertical_lines = []
        i = 0
        for line in raw_lines:
            if line[1] < self.vertical_threshold or \
                line[1] > (math.pi-self.vertical_threshold):

                #absolute value does better grouping currently
                vertical_lines.append((abs(line[0]), line[1]))
            i += 1

        # print message to user for performance purposes
        logging.debug("{} possibilities reduced to {} lines".format(
            i, len(vertical_lines)))

        # Group vertical lines
        vertical_line_groups = [
        ]  #A list of line groups which are each a line list
        i = 0
        for line in vertical_lines:
            group_found = False
            for line_group in vertical_line_groups:
                i += 1
                if line_group_accept_test(line_group, line, self.max_range):
                    line_group.append(line)
                    group_found = True

            if not group_found:
                vertical_line_groups.append([line])

        #quick debugging statement
        logging.debug("{} internal iterations for {} groups".format(
            i, len(vertical_line_groups)))

        # Average line groups into lines
        vertical_lines = []
        for line_group in vertical_line_groups:
            rhos = map(lambda line: line[0], line_group)  #get rho of each line
            angles = map(lambda line: line[1], line_group)
            line = (sum(rhos) / len(rhos), circular_average(angles, math.pi))
            vertical_lines.append(line)

        self.left_pole = None
        self.right_pole = None
        self.returning = 0
        self.found = False

        if len(vertical_lines) is 2:
            roi = cv.GetImageROI(frame)
            width = roi[2]
            height = roi[3]
            self.left_pole = round(
                min(vertical_lines[0][0], vertical_lines[1][0]), 2) - width / 2
            self.right_pole = round(
                max(vertical_lines[0][0], vertical_lines[1][0]), 2) - width / 2

            self.returning = (self.left_pole + self.right_pole) / 2
            logging.info("Returning {}".format(self.returning))

            #If this is first iteration, count this as seeing the gate
            if self.last_seen < 0:
                self.last_center = None
                self.last_seen = 0

            #increment a counter if result is good.
            if self.last_center is None:
                self.last_center = self.returning
                self.seen_count = 1
            elif math.fabs(self.last_center -
                           self.returning) < self.center_trans_thresh:
                self.seen_count += 1
                self.last_seen += 2
            else:
                self.last_seen -= 1

            #if not convinced, forget left/right pole. Else, proclaim success.
            if self.seen_count < self.seen_count_thresh:
                self.left_pole = None
                self.right_pole = None
            else:
                print "FOUND CENTER AND RETURNED IT"
                self.found = True

        else:
            self.returning = 0

            if self.last_seen < 0:
                self.last_center = None
                self.last_seen = 0

            self.last_seen -= 1
            self.left_pole = None
            self.right_POLE = None

        #extra debugging stuff
        if self.debug:
            cv.CvtColor(color_filtered, frame, cv.CV_GRAY2RGB)
            libvision.misc.draw_lines(frame, vertical_lines)

            if self.found:
                cv.Circle(frame, (int(frame.width / 2 + self.returning),
                                  int(frame.height / 2)), 15, (0, 255, 0), 2,
                          8, 0)
                font = cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, 1, 1, 1, 3)
                cv.PutText(frame, "Gate Sent to Mission Control", (100, 400),
                           font, (255, 255, 0))
                #print frame.width

        #cv.ShowImage("Gate", cv.CloneImage(frame))
        svr.debug("Gate", cv.CloneImage(frame))
        svr.debug("Unchanged", cv.CloneImage(unchanged_frame))

        self.return_output()
示例#19
0
文件: gate.py 项目: tarora2/seawolf
    def process_frame(self, frame):
        (w, h) = cv.GetSize(frame)

        #generate hue selection frames

        #create locations for the a pair of test frames
        frametest = cv.CreateImage(cv.GetSize(frame), 8, 3)
        binarytest = cv.CreateImage(cv.GetSize(frame), 8, 1)

        #use the red channel for the binary frame (just for debugging purposes)
        cv.Copy(frame, frametest)
        cv.SetImageCOI(frametest, 3)
        cv.Copy(frametest, binarytest)
        cv.SetImageCOI(frametest, 0)  #reset COI
        #svr.debug("R?",binarytest)

        # Resize image to 320x240
        #copy = cv.CreateImage(cv.GetSize(frame), 8, 3)
        #cv.Copy(frame, copy)
        #cv.SetImageROI(frame, (0, 0, 320, 240))
        #cv.Resize(copy, frame, cv.CV_INTER_NN)

        found_gate = False

        #create a new frame just for comparison purposes
        unchanged_frame = cv.CreateImage(cv.GetSize(frame), 8, 3)
        cv.Copy(frame, unchanged_frame)

        #apply a course noise filter
        cv.Smooth(frame, frame, cv.CV_MEDIAN, 7, 7)

        # Set binary image to have saturation channel
        hsv = cv.CreateImage(cv.GetSize(frame), 8, 3)
        binary = cv.CreateImage(cv.GetSize(frame), 8, 1)
        cv.CvtColor(frame, hsv, cv.CV_BGR2HSV)
        cv.SetImageCOI(hsv, 1)
        cv.Copy(hsv, binary)
        cv.SetImageCOI(hsv, 0)  #reset COI

        #shift hue of image such that orange->red are at top of spectrum
        '''
        binary = libvision.misc.cv_to_cv2(binary)
        binary = libvision.misc.shift_hueCV2(binary, self.target_shift)
        binary = libvision.misc.cv2_to_cv(binary)
	'''

        #correct for wraparound on red spectrum
        #cv.InRange(binary,a_array,b_array,binarytest) #generate mask
        #cv.Add(binary,cv.fromarray(ones*180),binary,mask=binarytest) #use mask to selectively add values
        svr.debug("R2?", binary)
        svr.debug("R2?", binary)

        #run adaptive threshold for edge detection and more noise filtering
        cv.AdaptiveThreshold(
            binary,
            binary,
            255,
            cv.CV_ADAPTIVE_THRESH_MEAN_C,
            cv.CV_THRESH_BINARY_INV,
            self.adaptive_thresh_blocksize,
            self.adaptive_thresh,
        )

        # Morphology
        kernel = cv.CreateStructuringElementEx(5, 5, 3, 3, cv.CV_SHAPE_ELLIPSE)
        cv.Erode(binary, binary, kernel, 1)
        cv.Dilate(binary, binary, kernel, 1)
        if self.debug:
            color_filtered = cv.CloneImage(binary)

        # Get Edges
        cv.Canny(binary, binary, 30, 40)

        # Hough Transform
        line_storage = cv.CreateMemStorage()
        raw_lines = cv.HoughLines2(binary,
                                   line_storage,
                                   cv.CV_HOUGH_STANDARD,
                                   rho=1,
                                   theta=math.pi / 180,
                                   threshold=self.hough_threshold,
                                   param1=0,
                                   param2=0)

        # Get vertical lines
        vertical_lines = []
        for line in raw_lines:
            if line[1] < self.vertical_threshold or \
               line[1] > math.pi-self.vertical_threshold:

                #absolute value does better grouping currently
                vertical_lines.append((abs(line[0]), line[1]))

        #print message to user for performance purposes
        logging.debug("{} possibilities reduced to {} lines".format(
            len(raw_lines), len(vertical_lines)))

        # Group vertical lines
        vertical_line_groups = [
        ]  # A list of line groups which are each a line list
        i = 0
        for line in vertical_lines:
            group_found = False
            for line_group in vertical_line_groups:
                i += 1
                if line_group_accept_test(line_group, line, self.max_range):
                    line_group.append(line)
                    group_found = True

            if not group_found:
                vertical_line_groups.append([line])

        #quick debugging statement
        logging.debug("{} internal iterations for {} groups".format(
            i, len(vertical_line_groups)))

        # Average line groups into lines
        vertical_lines = []
        for line_group in vertical_line_groups:
            rhos = map(lambda line: line[0], line_group)
            angles = map(lambda line: line[1], line_group)
            line = (sum(rhos) / len(rhos), circular_average(angles, math.pi))
            vertical_lines.append(line)

        ####################################################
        #vvvv Horizontal line code isn't used for anything

        # Get horizontal lines
        horizontal_lines = []
        for line in raw_lines:
            dist_from_horizontal = (math.pi / 2 + line[1]) % math.pi
            if dist_from_horizontal < self.horizontal_threshold or \
               dist_from_horizontal > math.pi-self.horizontal_threshold:

                horizontal_lines.append((abs(line[0]), line[1]))

        # Group horizontal lines
        horizontal_line_groups = [
        ]  # A list of line groups which are each a line list
        for line in horizontal_lines:
            group_found = False
            for line_group in horizontal_line_groups:

                if line_group_accept_test(line_group, line, self.max_range):
                    line_group.append(line)
                    group_found = True

            if not group_found:
                horizontal_line_groups.append([line])

        if len(horizontal_line_groups) is 1:
            self.seen_crossbar = True
            if self.debug:
                rhos = map(lambda line: line[0], horizontal_line_groups[0])
                angles = map(lambda line: line[1], horizontal_line_groups[0])
                line = (sum(rhos) / len(rhos),
                        circular_average(angles, math.pi))
                horizontal_lines = [line]
        else:
            self.seen_crossbar = False
            horizontal_lines = []

        #^^^ Horizontal line code isn't used for anything
        ###################################################

        self.left_pole = None
        self.right_pole = None
        #print vertical_lines
        self.returning = 0
        self.found = False

        if len(vertical_lines) is 2:
            roi = cv.GetImageROI(frame)
            width = roi[2]
            height = roi[3]
            self.left_pole = round(
                min(vertical_lines[0][0], vertical_lines[1][0]), 2) - width / 2
            self.right_pole = round(
                max(vertical_lines[0][0], vertical_lines[1][0]), 2) - width / 2

            self.returning = (self.left_pole + self.right_pole) / 2
            logging.info("Returning {} as gate center delta.".format(
                self.returning))

            #initialize first iteration with 2 known poles
            if self.last_seen < 0:
                self.last_center = None
                self.last_seen = 0

            #increment a counter if result is good.
            if self.last_center is None:
                self.last_center = self.returning
                self.seen_count = 1
            elif math.fabs(self.last_center -
                           self.returning) < self.center_trans_thresh:
                self.seen_count += 1
                self.last_seen += 2
            else:
                self.last_seen -= 1

            #if not conviced, forget left/right pole. Else proclaim success.
            if self.seen_count < self.seen_count_thresh:
                self.left_pole = None
                self.right_pole = None
            else:
                print "FOUND CENTER AND RETURNED IT"
                self.found = True
        else:
            self.returning = 0
            if self.last_seen < 0:
                self.last_center = None
                self.last_seen = 0
            self.last_seen -= 1
            self.left_pole = None
            self.right_pole = None

        #TODO: If one pole is seen, is it left or right pole?

        if self.debug:
            cv.CvtColor(color_filtered, frame, cv.CV_GRAY2RGB)
            libvision.misc.draw_lines(frame, vertical_lines)
            libvision.misc.draw_lines(frame, horizontal_lines)

            if self.found:
                cv.Circle(frame, (int(frame.width / 2 + self.returning),
                                  int(frame.height / 2)), 15, (0, 255, 0), 2,
                          8, 0)
                font = cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, 1, 1, 1, 3)
                cv.PutText(frame, "Gate Sent to Mission Control", (100, 400),
                           font, (255, 255, 0))
                #print frame.width

            #cv.ShowImage("Gate", cv.CloneImage(frame))
            svr.debug("Gate", cv.CloneImage(frame))
            svr.debug("Unchanged", cv.CloneImage(unchanged_frame))

        #populate self.output with infos
        self.output.seen_crossbar = self.seen_crossbar
        self.output.left_pole = self.left_pole
        self.output.right_pole = self.right_pole

        self.return_output()
示例#20
0
文件: test.py 项目: tarora2/seawolf
    def process_frame(self, frame, debug=True):
        """ process this frame, then place output in self.output """

        if debug:
            # display frame
            svr.debug("Frame", frame)

            # create a new image, the size of frame
            gray = cv.CreateImage(cv.GetSize(frame), 8, 1)
            edge = cv.CreateImage(cv.GetSize(frame), 8, 1)
            lines = cv.CloneImage(frame)

            # copy BW frame into binary
            cv.CvtColor(frame, gray, cv.CV_BGR2GRAY)

            # Get Edges
            cv.Canny(gray, edge, 60, 80)

            # Create a Binary Image
            binary = cv.CreateImage(cv.GetSize(frame), 8, 1)

            # Run Adaptive Threshold
            cv.AdaptiveThreshold(gray, binary,
                                 255,
                                 cv.CV_ADAPTIVE_THRESH_MEAN_C,
                                 cv.CV_THRESH_BINARY_INV,
                                 19,
                                 4,
                                 )

            # display adaptive threshold
            svr.debug("Thresh", binary)

            # Hough Transform
            line_storage = cv.CreateMemStorage()
            raw_lines = cv.HoughLines2(edge, line_storage, cv.CV_HOUGH_STANDARD,
                                       rho=1,
                                       theta=math.pi / 180,
                                       threshold=50,
                                       param1=0,
                                       param2=0
                                       )

            # process line data
            found_lines = []
            for line in raw_lines[:10]:
                found_lines.append((abs(line[0]), line[1]))
            print found_lines

            # display our transformed image
            svr.debug("Edges", edge)

            # draw found lines
            libvision.misc.draw_lines(lines, found_lines)

            # display image with lines
            svr.debug("Lines", lines)

            cv.WaitKey(10)

        self.output = "test data"
示例#21
0
    def process_frame(self, frame):
        self.numpy_frame = libvision.cv_to_cv2(frame)
        self.debug_frame = self.numpy_frame.copy()
        self.test_frame = self.numpy_frame.copy()

        self.numpy_frame = cv2.medianBlur(self.numpy_frame, 7)
        self.numpy_frame = cv2.cvtColor(self.numpy_frame, cv2.COLOR_BGR2HSV)

        (rf1, rf2, rf3) = cv2.split(self.numpy_frame)

        Rbinary = rf3
        Gbinary = rf1

        # Adaptive Threshold
        Rbinary = cv2.adaptiveThreshold(Rbinary, 255,
                                        cv2.ADAPTIVE_THRESH_MEAN_C,
                                        cv2.THRESH_BINARY_INV,
                                        self.adaptive_thresh_blocksize,
                                        self.adaptive_thresh)

        Gbinary = cv2.adaptiveThreshold(Gbinary, 255,
                                        cv2.ADAPTIVE_THRESH_MEAN_C,
                                        cv2.THRESH_BINARY_INV,
                                        self.Gadaptive_thresh_blocksize,
                                        self.Gadaptive_thresh)

        # Morphology
        kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (3, 3))

        Rbinary = cv2.erode(Rbinary, kernel)
        Rbinary = cv2.dilate(Rbinary, kernel)
        Gbinary = cv2.erode(Gbinary, kernel)
        Gbinary = cv2.dilate(Gbinary, kernel)

        Rframe = cv2.cvtColor(Rbinary, cv2.COLOR_GRAY2RGB)
        Gframe = cv2.cvtColor(Gbinary, cv2.COLOR_GRAY2RGB)

        # Hough Transform
        raw_linesG = cv2.HoughLines(Gbinary,
                                    rho=1,
                                    theta=math.pi / 180,
                                    threshold=self.hough_thresholdG)

        # Get vertical lines
        vertical_linesG = []
        for line in raw_linesG[0]:
            rho = line[0]
            theta = line[1]
            if theta < self.vertical_thresholdG or \
                    theta > math.pi - self.vertical_thresholdG:

                vertical_linesG.append((abs(rho), theta))

        # Group vertical lines
        vertical_line_groupsG = []  # A list of line groups which are each a line list
        for line in vertical_linesG:
            group_found = False
            for line_group in vertical_line_groupsG:

                if line_group_accept_test(line_group, line, self.max_range):
                    line_group.append(line)
                    group_found = True

            if not group_found:
                vertical_line_groupsG.append([line])

        # Average line groups into lines
        vertical_linesG = []
        for line_group in vertical_line_groupsG:
            rhos = map(lambda line: line[0], line_group)
            angles = map(lambda line: line[1], line_group)
            line = (sum(rhos) / len(rhos), circular_average(angles, math.pi))
            vertical_linesG.append(line)

        # Get horizontal lines
        horizontal_lines = []
        for line in raw_linesG[0]:
            rho = line[0]
            theta = line[1]
            dist_from_horizontal = (math.pi / 2 + line[1]) % math.pi
            if dist_from_horizontal < self.horizontal_threshold or \
                    dist_from_horizontal > math.pi - self.horizontal_threshold:

                horizontal_lines.append((abs(line[0]), line[1]))

        # Group horizontal lines
        horizontal_line_groups = []  # A list of line groups which are each a line list
        print "Horizontal lines: ",
        for line in horizontal_lines:
            group_found = False
            for line_group in horizontal_line_groups:

                if line_group_accept_test(line_group, line, self.max_range):
                    line_group.append(line)
                    group_found = True

            if not group_found:
                horizontal_line_groups.append([line])

        if len(horizontal_line_groups) is 1:
            self.seen_crossbar = True
            rhos = map(lambda line: line[0], horizontal_line_groups[0])
            angles = map(lambda line: line[1], horizontal_line_groups[0])
            line = (sum(rhos) / len(rhos), circular_average(angles, math.pi))
            horizontal_lines = [line]
        else:
            self.seen_crossbar = False
            horizontal_lines = []

        self.left_pole = None
        self.right_pole = None

        Rframe = libvision.cv2_to_cv(Rframe)
        Gframe = libvision.cv2_to_cv(self.debug_frame)
        Rbinary = libvision.cv2_to_cv(Rbinary)
        self.debug_frame = libvision.cv2_to_cv(self.debug_frame)
        self.test_frame = libvision.cv2_to_cv(self.test_frame)
        Gbinary = libvision.cv2_to_cv(Gbinary)

        if len(vertical_linesG) is 2:
            roi = cv.GetImageROI(frame)
            width = roi[2]
            height = roi[3]
            self.left_pole = round(min(vertical_linesG[0][0], vertical_linesG[1][0]), 2) - width / 2
            self.right_pole = round(max(vertical_linesG[0][0], vertical_linesG[1][0]), 2) - width / 2
        # TODO: If one pole is seen, is it left or right pole?

        # Calculate planar distance r (assuming we are moving perpendicular to
        # the hedge)
        if self.left_pole and self.right_pole:
            theta = abs(self.left_pole - self.right_pole)
            self.r = 3 / math.tan(math.radians(theta / 2))
        else:
            self.r = None

        if self.r and self.seen_crossbar:
            bar_phi = (-1 * horizontal_lines[0][0] + Gframe.height / 2) / (Gframe.height / 2) * 32
            self.crossbar_depth = self.r * math.atan(math.radians(bar_phi))
        else:
            self.crossbar_depth = None

        # Line Finding on Red pvc
        # Hough Transform
        line_storage = cv.CreateMemStorage()
        raw_linesR = cv.HoughLines2(Rbinary, line_storage, cv.CV_HOUGH_STANDARD,
                                    rho=1,
                                    theta=math.pi / 180,
                                    threshold=self.hough_thresholdR,
                                    param1=0,
                                    param2=0
                                    )

        # Get vertical lines
        vertical_linesR = []
        for line in raw_linesR:
            if line[1] < self.vertical_thresholdR or \
               line[1] > math.pi - self.vertical_thresholdR:

                vertical_linesR.append((abs(line[0]), line[1]))

        # Group vertical lines
        vertical_line_groupsR = []  # A list of line groups which are each a line list
        for line in vertical_linesR:
            group_found = False
            for line_group in vertical_line_groupsR:

                if line_group_accept_test(line_group, line, self.max_range):
                    line_group.append(line)
                    group_found = True

            if not group_found:
                vertical_line_groupsR.append([line])

        # Average line groups into lines
        vertical_linesR = []
        for line_group in vertical_line_groupsR:
            rhos = map(lambda line: line[0], line_group)
            angles = map(lambda line: line[1], line_group)
            line = (sum(rhos) / len(rhos), circular_average(angles, math.pi))
            vertical_linesR.append(line)
        '''
        for red_line in vertical_linesR:
            print "Red Line:", red_line[0],", ",red_line[1]
        for green_line in vertical_linesG:
            print "Green Line:", green_line[0],", ",green_line[1]
        '''
        for red_line in vertical_linesR:
            for green_line in vertical_linesG[:]:
                if math.fabs(green_line[0] - red_line[0]) < self.GR_Threshold0 and \
                   math.fabs(green_line[1] - red_line[1]) < self.GR_Threshold1:
                    vertical_linesG.remove(green_line)

        for red_line in vertical_linesR:
            print "New Red Line:", red_line[0], ", ", red_line[1]
        for green_line in vertical_linesG:
            print "New Green Line:", green_line[0], ", ", green_line[1]

        if len(vertical_linesR) is 0:
            print "No Red Found"

        self.left_pole = None
        self.right_pole = None
        if len(vertical_linesR) is 2:
            roi = cv.GetImageROI(frame)
            width = roi[2]
            height = roi[3]
            self.left_pole = round(min(vertical_linesR[0][0], vertical_linesR[1][0]), 2) - width / 2
            self.right_pole = round(max(vertical_linesR[0][0], vertical_linesR[1][0]), 2) - width / 2
        # TODO: If one pole is seen, is it left or right pole?

        # Calculate planar distance r (assuming we are moving perpendicular to
        # the hedge)
        if self.left_pole and self.right_pole:
            theta = abs(self.left_pole - self.right_pole)
            self.r = 3 / math.tan(math.radians(theta / 2))
        else:
            self.r = None

        for i in range(len(vertical_linesR[:])):
            if vertical_linesR[i][1] > math.pi / 2:
                vertical_linesR[i] = (vertical_linesR[i][0], -(math.pi - vertical_linesR[i][1]))
                print "Line changed to ", vertical_linesR[i]
        for line in vertical_linesR:
            print line
            if line[1] > math.pi / 2:
                line = (line[0], math.pi - line[1])
                print "Line changed to ", line

        libvision.misc.draw_lines(Gframe, vertical_linesG)
        libvision.misc.draw_lines(Gframe, horizontal_lines)
        libvision.misc.draw_lines(Rframe, vertical_linesR)

        # there was a merge error, these 3 lines conflicted b/c your copy out of date

        for line in vertical_linesR:
            roi = cv.GetImageROI(frame)
            width = roi[2]
            height = roi[3]
            x = line[0] * math.cos(line[1])
            y = line[0] * math.sin(line[1])
            cv.Circle(Rframe, (int(x), int(y)), 5, (0, 255, 0), -1, 8, 0)
            if x > width or y > width or x < 0 or y < 0:
                print "Lost point  ", x

        svr.debug("Original", self.test_frame)
        svr.debug("Red", Rframe)
        svr.debug("Green", Gframe)
示例#22
0
    def process_frame(self, frame):
        self.debug_frame = cv.CreateImage(cv.GetSize(frame), 8, 3)
        og_frame = cv.CreateImage(cv.GetSize(frame), 8, 3)
        cv.Copy(frame, self.debug_frame)
        cv.Copy(self.debug_frame, og_frame)

        cv.Smooth(frame, frame, cv.CV_MEDIAN, 7, 7)

        # Set binary image to have saturation channel
        hsv = cv.CreateImage(cv.GetSize(frame), 8, 3)
        binary = cv.CreateImage(cv.GetSize(frame), 8, 1)
        cv.CvtColor(frame, hsv, cv.CV_BGR2HSV)
        cv.SetImageCOI(hsv, 1)  #3 before competition #2 at competition
        cv.Copy(hsv, binary)
        cv.SetImageCOI(hsv, 0)

        cv.AdaptiveThreshold(binary, binary,
                             255,
                             cv.CV_ADAPTIVE_THRESH_MEAN_C,
                             cv.CV_THRESH_BINARY_INV,
                             self.adaptive_thresh_blocksize,
                             self.adaptive_thresh,
        )

        # Morphology
        kernel = cv.CreateStructuringElementEx(5, 5, 3, 3, cv.CV_SHAPE_ELLIPSE)
        cv.Erode(binary, binary, kernel, 1)
        cv.Dilate(binary, binary, kernel, 1)

        # Get Edges
        #cv.Canny(binary, binary, 30, 40)

        cv.CvtColor(binary, self.debug_frame, cv.CV_GRAY2RGB)

        # Hough Transform
        line_storage = cv.CreateMemStorage()
        raw_lines = cv.HoughLines2(binary, line_storage, cv.CV_HOUGH_PROBABILISTIC,
                                   rho=1,
                                   theta=math.pi / 180,
                                   threshold=self.hough_threshold,
                                   param1=self.min_length,
                                   param2=self.max_gap
        )

        lines = []

        for line in raw_lines:
            lines.append(line)

        #Grouping lines depending on endpoint similarities

        for line1 in lines[:]:
            for line2 in lines[:]:
                if line1 in lines and line2 in lines and line1 != line2:
                    if math.fabs(line1[0][0] - line2[0][0]) < self.max_corner_range and \
                       math.fabs(line1[0][1] - line2[0][1]) < self.max_corner_range and \
                       math.fabs(line1[1][0] - line2[1][0]) < self.max_corner_range and \
                       math.fabs(line1[1][1] - line2[1][1]) < self.max_corner_range:
                        if line_distance(line1[0], line1[1]) > line_distance(line2[0], line2[1]):
                            lines.remove(line2)
                        else:
                            lines.remove(line1)
                    elif math.fabs(line1[0][0] - line2[1][0]) < self.max_corner_range and \
                         math.fabs(line1[0][1] - line2[1][1]) < self.max_corner_range and \
                         math.fabs(line1[1][0] - line2[0][0]) < self.max_corner_range and \
                         math.fabs(line1[1][1] - line2[0][1]) < self.max_corner_range:
                        if line_distance(line1[0], line1[1]) > line_distance(line2[0], line2[1]):
                            lines.remove(line2)
                        else:
                            lines.remove(line1)

        self.hough_corners = []
        for line in lines:
            self.hough_corners.append(line[0])
            self.hough_corners.append(line[1])

        for corner1 in self.hough_corners[:]:
            for corner2 in self.hough_corners[:]:
                if corner1 is not corner2 and corner1 in self.hough_corners and corner2 in self.hough_corners:
                    if math.fabs(corner1[0] - corner2[0]) < self.max_corner_range4 and \
                       math.fabs(corner1[1] - corner2[1]) < self.max_corner_range4:
                        corner1 = [(corner1[0] + corner2[0]) / 2, (corner1[1] + corner2[1]) / 2]
                        self.hough_corners.remove(corner2)

        for line1 in lines:
            #cv.Line(self.debug_frame,line1[0],line1[1], (0,0,255), 10, cv.CV_AA, 0)
            for line2 in lines:
                if line1 is not line2:
                    self.find_corners(line1, line2)

        for corner1 in self.corners:
            for corner2 in self.corners:
                if math.fabs(corner1[1][0] - corner2[1][0]) < self.max_corner_range2 and \
                   math.fabs(corner1[1][1] - corner2[1][1]) < self.max_corner_range2 and \
                   math.fabs(corner1[2][0] - corner2[2][0]) < self.max_corner_range2 and \
                   math.fabs(corner1[2][1] - corner2[2][1]) < self.max_corner_range2 and \
                   math.fabs(corner1[0][0] - corner2[0][0]) > self.max_corner_range2 and \
                   math.fabs(corner1[0][1] - corner2[0][1]) > self.max_corner_range2:
                    pt1 = (int(corner1[0][0]), int(corner1[0][1]))
                    pt4 = (int(corner2[0][0]), int(corner2[0][1]))
                    pt3 = (int(corner1[1][0]), int(corner1[1][1]))
                    pt2 = (int(corner1[2][0]), int(corner1[2][1]))
                    #line_color = (0,255,0)s
                    #cv.Line(self.debug_frame,pt1,pt2, line_color, 10, cv.CV_AA, 0)                  
                    #cv.Line(self.debug_frame,pt1,pt3, line_color, 10, cv.CV_AA, 0)
                    #cv.Line(self.debug_frame,pt4,pt2, line_color, 10, cv.CV_AA, 0)                  
                    #cv.Line(self.debug_frame,pt4,pt3, line_color, 10, cv.CV_AA, 0)
                    new_bin = Bin(pt1, pt2, pt3, pt4)
                    new_bin.id = self.bin_id
                    self.bin_id += 1
                    if math.fabs(line_distance(pt1, pt2) - line_distance(pt3, pt4)) < self.parallel_sides_length_thresh and \
                       math.fabs(line_distance(pt1, pt3) - line_distance(pt2, pt4)) < self.parallel_sides_length_thresh:
                        self.Bins.append(new_bin)
                        print "new_bin"

                elif (math.fabs(corner1[1][0] - corner2[2][0]) < self.max_corner_range2 and
                      math.fabs(corner1[1][1] - corner2[2][1]) < self.max_corner_range2 and
                      math.fabs(corner1[2][0] - corner2[1][0]) < self.max_corner_range2 and
                      math.fabs(corner1[2][1] - corner2[1][1]) < self.max_corner_range2 and
                      math.fabs(corner1[0][0] - corner2[0][0]) > self.max_corner_range2 and
                      math.fabs(corner1[0][1] - corner2[0][1]) > self.max_corner_range2):
                    continue

        self.corners = []
        self.final_corners = self.sort_corners() #Results are not used. Experimental corners which have been seen twice, should be only the corners we want, but there were problems
        self.sort_bins()
        self.update_bins()
        self.group_bins()
        self.draw_bins()

        for corner in self.hough_corners:
            line_color = [255, 0, 0]
            cv.Circle(self.debug_frame, corner, 15, (255, 0, 0), 2, 8, 0)

        for line in lines:
            line_color = [255, 0, 0]
            cv.Line(self.debug_frame, line[0], line[1], line_color, 5, cv.CV_AA, 0)
            #cv.Circle(self.debug_frame, line[0], 15, (255,0,0), 2,8,0)
            #cv.Circle(self.debug_frame, line[1], 15, (255,0,0), 2,8,0)

        #Output bins
        self.output.bins = self.Bins
        anglesum = 0
        for bins in self.output.bins:
            bins.theta = (bins.center[0] - frame.width / 2) * 37 / (frame.width / 2)
            bins.phi = -1 * (bins.center[1] - frame.height / 2) * 36 / (frame.height / 2)
            anglesum += bins.angle
            # bins.orientation = bins.angle
        if len(self.output.bins) > 0:
            self.output.orientation = anglesum / len(self.output.bins)
        else:
            self.output.orientation = None
        self.return_output()

        svr.debug("Bins", self.debug_frame)
        svr.debug("Original", og_frame)

        #BEGIN SHAPE PROCESSING

        #constants
        img_width = 128
        img_height = 256

        number_x = 23
        number_y = 111
        number_w = 82
        number_h = 90

        bin_thresh_blocksize = 11
        bin_thresh = 1.9

        red_significance_threshold = 0.4

        #load templates - run once, accessible to number processor

        number_templates = [
            (10, cv.LoadImage("number_templates/10.png")),
            (16, cv.LoadImage("number_templates/16.png")),
            (37, cv.LoadImage("number_templates/37.png")),
            (98, cv.LoadImage("number_templates/98.png")),
        ]

        #Begin Bin Contents Processing

        for bin in self.Bins:
            #Take the bin's corners, and get an image containing an img_width x img_height rectangle of it
            transf = cv.CreateMat(3, 3, cv.CV_32FC1)
            cv.GetPerspectiveTransform(
                [bin.corner1, bin.corner2, bin.corner3, bin.corner4],
                [(0, 0), (0, img_height), (img_width, 0), (img_width, img_height)],
                transf
            )
            bin_image = cv.CreateImage([img_width, img_height], 8, 3)
            cv.WarpPerspective(frame, bin_image, transf)

            #AdaptaveThreshold to get black and white image highlighting the number (still works better than my yellow-vs-red threshold attempt
            hsv = cv.CreateImage(cv.GetSize(bin_image), 8, 3)
            bin_thresh_image = cv.CreateImage(cv.GetSize(bin_image), 8, 1)
            cv.CvtColor(bin_image, hsv, cv.CV_BGR2HSV)
            cv.SetImageCOI(hsv, 3)
            cv.Copy(hsv, bin_thresh_image)
            cv.SetImageCOI(hsv, 0)
            cv.AdaptiveThreshold(bin_thresh_image, bin_thresh_image,
                                 255,
                                 cv.CV_ADAPTIVE_THRESH_MEAN_C,
                                 cv.CV_THRESH_BINARY_INV,
                                 bin_thresh_blocksize,
                                 bin_thresh,
            )
            kernel = cv.CreateStructuringElementEx(5, 5, 3, 3, cv.CV_SHAPE_ELLIPSE)
            cv.Erode(bin_thresh_image, bin_thresh_image, kernel, 1)
            cv.Dilate(bin_thresh_image, bin_thresh_image, kernel, 1)

            #Here, we loop through all four different templates, and figure out which one we think is most likely.
            #The comparison function counts corresponding pixels that are non-zero in each image, and then corresponding pixels that are different in each image. The ratio of diff_count/both_count is our "unconfidence" ratio. The lower it is, the more confident we are.
            #There are two nearly identical pieces of code within this loop. One checks the bin right-side-up, and the other one checks it flipped 180.
            last_thought_number = -1
            last_unconfidence_ratio = number_w * number_h + 2
            for i in range(0, len(number_templates)):
                both_count = 0
                diff_count = 0
                this_number_image = number_templates[i][1]
                for y in range(0, number_h):
                    for x in range(0, number_w):
                        if (bin_thresh_image[y + number_y, x + number_x] != 0) and (this_number_image[y, x][0] != 0):
                            both_count += 1
                        elif (bin_thresh_image[y + number_y, x + number_x] != 0) or (this_number_image[y, x][0] != 0):
                            diff_count += 1
                if both_count == 0:
                    unconfidence_ratio = number_w * number_h + 1  # max unconfidence
                else:
                    unconfidence_ratio = 1.0 * diff_count / both_count
                if unconfidence_ratio < last_unconfidence_ratio:
                    last_thought_number = number_templates[i][0]
                    last_unconfidence_ratio = unconfidence_ratio
                both_count = 0
                diff_count = 0
                for y in range(0, number_h):
                    for x in range(0, number_w):
                        if (bin_thresh_image[img_height - number_y - 1 - y, img_width - number_x - 1 - x] != 0) and (
                                this_number_image[y, x][0] != 0):
                            both_count += 1
                        elif (bin_thresh_image[img_height - number_y - 1 - y, img_width - number_x - 1 - x] != 0) or (
                                this_number_image[y, x][0] != 0):
                            diff_count += 1
                if both_count == 0:
                    unconfidence_ratio = number_w * number_h + 1  # max unconfidence
                else:
                    unconfidence_ratio = 1.0 * diff_count / both_count
                if unconfidence_ratio < last_unconfidence_ratio:
                    last_thought_number = number_templates[i][0]
                    last_unconfidence_ratio = unconfidence_ratio

            print str(last_thought_number) + " | " + str(last_unconfidence_ratio)

            try: #check if it's defined
                bin.number_unconfidence_ratio
            except:
                bin.number_unconfidence_ratio = last_unconfidence_ratio
                bin.number = last_thought_number
                print "Set Speed Limit Number"
            else:
                if last_unconfidence_ratio < bin.number_unconfidence_ratio:
                    bin.number_unconfidence_ratio = last_unconfidence_ratio
                    if bin.number == last_thought_number:
                        print "More Confident on Same Number: Updated"
                    else:
                        print "More Confident on Different Number: Updated"
                        bin.icon = last_thought_number
示例#23
0
文件: pizza.py 项目: tarora2/seawolf
    def process_frame(self, frame):
        self.debug_frame = cv.CreateImage(cv.GetSize(frame), 8, 3)
        og_frame = cv.CreateImage(cv.GetSize(frame), 8, 3)
        cv.Copy(frame, self.debug_frame)
        cv.Copy(self.debug_frame, og_frame)

        cv.Smooth(frame, frame, cv.CV_MEDIAN, 7, 7)

        # Set binary image to have saturation channel
        hsv = cv.CreateImage(cv.GetSize(frame), 8, 3)
        binary = cv.CreateImage(cv.GetSize(frame), 8, 1)
        cv.CvtColor(frame, hsv, cv.CV_BGR2HSV)
        cv.SetImageCOI(hsv, 1)
        cv.Copy(hsv, binary)
        cv.SetImageCOI(hsv, 0)

        cv.AdaptiveThreshold(
            binary,
            binary,
            255,
            cv.CV_ADAPTIVE_THRESH_MEAN_C,
            cv.CV_THRESH_BINARY_INV,
            self.adaptive_thresh_blocksize,
            self.adaptive_thresh,
        )

        # Morphology
        kernel = cv.CreateStructuringElementEx(5, 5, 3, 3, cv.CV_SHAPE_ELLIPSE)
        cv.Erode(binary, binary, kernel, 1)
        cv.Dilate(binary, binary, kernel, 1)

        # Get Edges
        #cv.Canny(binary, binary, 30, 40)

        cv.CvtColor(binary, self.debug_frame, cv.CV_GRAY2RGB)

        # Hough Transform
        line_storage = cv.CreateMemStorage()
        raw_lines = cv.HoughLines2(binary,
                                   line_storage,
                                   cv.CV_HOUGH_PROBABILISTIC,
                                   rho=1,
                                   theta=math.pi / 180,
                                   threshold=self.hough_threshold,
                                   param1=self.min_length,
                                   param2=self.max_gap)

        lines = []
        corners = []

        for line in raw_lines:
            lines.append(line)

        # Grouping lines depending on endpoint similarities

        for line1 in lines[:]:
            for line2 in lines[:]:
                if line1 in lines and line2 in lines and line1 != line2:
                    if math.fabs(line1[0][0] - line2[0][0]) < self.max_corner_range and \
                       math.fabs(line1[0][1] - line2[0][1]) < self.max_corner_range and \
                       math.fabs(line1[1][0] - line2[1][0]) < self.max_corner_range and \
                       math.fabs(line1[1][1] - line2[1][1]) < self.max_corner_range:
                        if line_distance(line1[0], line1[1]) > line_distance(
                                line2[0], line2[1]):
                            lines.remove(line2)
                        else:
                            lines.remove(line1)
                    elif math.fabs(line1[0][0] - line2[1][0]) < self.max_corner_range and \
                            math.fabs(line1[0][1] - line2[1][1]) < self.max_corner_range and \
                            math.fabs(line1[1][0] - line2[0][0]) < self.max_corner_range and \
                            math.fabs(line1[1][1] - line2[0][1]) < self.max_corner_range:
                        if line_distance(line1[0], line1[1]) > line_distance(
                                line2[0], line2[1]):
                            lines.remove(line2)
                        else:
                            lines.remove(line1)

        for line in lines:
            corners.append(line[0])
            corners.append(line[1])

        for corner1 in corners:
            for corner2 in corners:
                for corner3 in corners:
                    for corner4 in corners:
                        # Checks that corners are not the same and are in the proper orientation
                        if corner4[0] != corner3[0] and corner4[0] != corner2[0] and corner4[0] != corner1[0] and \
                           corner3[0] != corner2[0] and corner3[0] != corner1[0] and corner2[0] != corner1[0] and \
                           corner4[1] != corner3[1] and corner4[1] != corner2[1] and corner4[1] != corner1[1] and \
                           corner3[1] != corner2[1] and corner3[1] != corner1[1] and corner2[1] != corner1[1] and \
                           corner2[0] >= corner3[0] and corner1[1] >= corner4[1] and corner2[0] >= corner1[0]:
                            # Checks that the side ratios are correct
                            if math.fabs(line_distance(corner1, corner3) - line_distance(corner2, corner4)) < self.size_threshold and \
                               math.fabs(line_distance(corner1, corner2) - line_distance(corner3, corner4)) < self.size_threshold and \
                               math.fabs(line_distance(corner1, corner3) / line_distance(corner1, corner2)) < self.ratio_threshold and \
                               math.fabs(line_distance(corner1, corner2) / line_distance(corner1, corner3)) < self.ratio_threshold:
                                #^^^ CHANGED OR TO AND --> DID MUCH BETTER. CONSIDER CHANGING ON BINSCORNER

                                # Checks that angles are roughly 90 degrees
                                angle_cnr_2 = math.fabs(
                                    angle_between_lines(
                                        line_slope(corner1, corner2),
                                        line_slope(corner2, corner4)))
                                if self.angle_min < angle_cnr_2 < self.angle_max:
                                    angle_cnr_3 = math.fabs(
                                        angle_between_lines(
                                            line_slope(corner1, corner3),
                                            line_slope(corner3, corner4)))
                                    if self.angle_min2 < angle_cnr_3 < self.angle_max2:
                                        new_box = Pizza(
                                            corner1, corner2, corner3, corner4)
                                        self.match_Boxes(new_box)

        for Box in self.Boxes[:]:
            Box.lastseen -= 1
            if Box.lastseen < 0:
                self.Boxes.remove(Box)

        self.draw_pizza()

        for line in lines:
            cv.Line(self.debug_frame, line[0], line[1], (255, 255, 0), 10,
                    cv.CV_AA, 0)
            cv.Circle(self.debug_frame, line[0], 15, (255, 0, 0), 2, 8, 0)
            cv.Circle(self.debug_frame, line[1], 15, (255, 0, 0), 2, 8, 0)

        self.output.pizza = self.Boxes
        anglesum = 0
        for Box in self.Boxes:
            Box.theta = (Box.center[0] - frame.width / 2) * 37 / (frame.width /
                                                                  2)
            Box.phi = -1 * (Box.center[1] -
                            frame.height / 2) * 36 / (frame.height / 2)
            anglesum += Box.angle
        if len(self.output.pizza) > 0:
            self.output.orientation = anglesum / len(self.output.pizza)
        else:
            self.output.orientation = None
        self.return_output()

        svr.debug("Pizza", self.debug_frame)
        svr.debug("Original", og_frame)
示例#24
0
    def process_frame(self, frame):

        self.output.found = False

        cv.Smooth(frame, frame, cv.CV_MEDIAN, 7, 7)

        # Use RGB color finder
        binary = libvision.cmodules.target_color_rgb.find_target_color_rgb(
            frame, 250, 125, 0, 1500, 500, .3)
        color_filtered = cv.CloneImage(binary)

        blob_map = cv.CloneImage(binary)
        blobs = libvision.blob.find_blobs(binary,
                                          blob_map,
                                          min_blob_size=50,
                                          max_blobs=10)

        if not blobs:
            return

        binary = cv.CloneImage(blob_map)
        mapping = [0] * 256
        for blob in blobs:
            mapping[blob.id] = 255
        libvision.greymap.greymap(blob_map, binary, mapping)

        # Get Edges
        cv.Canny(binary, binary, 30, 40)

        # Hough Transform
        line_storage = cv.CreateMemStorage()
        lines = cv.HoughLines2(binary,
                               line_storage,
                               cv.CV_HOUGH_STANDARD,
                               rho=1,
                               theta=math.pi / 180,
                               threshold=self.hough_threshold,
                               param1=0,
                               param2=0)
        print "hough transform found", len(lines), " lines"
        lines = lines[:self.lines_to_consider]  # Limit number of lines

        # if not lines:
        #    return

        paths = self.path_manager.process(lines, blobs)

        if paths and not self.path:
            # If path[1] is clockwise of paths[0]
            distance = circular_distance(paths[0].angle, paths[1].angle)

            if distance > 0:
                self.path = paths[self.which_path]
            else:
                self.path = paths[1 - self.which_path]

        if paths and self.path in paths and self.path.blobs:

            temp_map = cv.CloneImage(blob_map)

            mapping = [0] * 256
            for blob in self.path.blobs:
                mapping[blob.id] = 255
            libvision.greymap.greymap(blob_map, temp_map, mapping)
            center = self.find_centroid(temp_map)

            svr.debug("map", temp_map)

            self.path.center = (center[0] - (frame.width / 2),
                                -center[1] + (frame.height / 2))

        random = 0
        if random == 0:
            # Show color filtered
            color_filtered_rgb = cv.CreateImage(cv.GetSize(frame), 8, 3)
            cv.CvtColor(color_filtered, color_filtered_rgb, cv.CV_GRAY2RGB)
            cv.SubS(color_filtered_rgb, (255, 0, 0), color_filtered_rgb)
            cv.Sub(frame, color_filtered_rgb, frame)

            # Show edges
            binary_rgb = cv.CreateImage(cv.GetSize(frame), 8, 3)
            cv.CvtColor(binary, binary_rgb, cv.CV_GRAY2RGB)
            cv.Add(frame, binary_rgb, frame)  # Add white to edge pixels
            cv.SubS(binary_rgb, (0, 0, 255), binary_rgb)
            cv.Sub(frame, binary_rgb, frame)  # Remove all but Red
            test_lines = []
            new_path = None

            for line in lines[:]:
                if self.candidates == []:
                    new_path = Path(line[0], line[1])
                    new_path.id = self.path_id
                    self.path_id += 1
                    new_path.last_seen += 1
                    self.candidates.append(new_path)
                    print "got a candidate"
            for candidate in self.candidates:
                if len(self.confirmed) == 0:
                    self.confirmed.append(candidate)

            for line in lines[:]:
                for candidate in self.candidates:
                    if math.fabs(line[0] - candidate.loc) < self.distance_threshold and \
                       math.fabs(line[1] - candidate.angle) < self.angle_threshold:
                        candidate.loc = (candidate.loc + line[0]) / 2
                        candidate.angle = (candidate.angle + line[1]) / 2
                        if candidate.last_seen < self.max_lastseen:
                            candidate.last_seen += 1
                        # print line1

                        if line in lines:
                            lines.remove(line)
                    else:
                        new_path = Path(line[0], line[1])
                        new_path.id = self.path_id
                        self.path_id += 1
                        new_path.last_seen += 1
                        new_path.seencount += 5
                        self.candidates.append(new_path)

            for candidate in self.candidates[:]:
                candidate.last_seen -= 1
                if candidate.seencount > self.min_seencount:
                    self.confirmed.append(candidate)
                    self.candidates.remove(candidate)
                if candidate.last_seen == -1:
                    self.candidates.remove(candidate)

            for confirmed in self.confirmed:
                for line in lines[:]:
                    if math.fabs(line[0] - confirmed.loc) < self.distance_trans and \
                       math.fabs(line[1] - confirmed.angle) < self.angle_trans:
                        confirmed.loc = line[0]
                        confirmed.angle = line[1]
                        if confirmed.last_seen < self.max_lastseen:
                            confirmed.last_seen += 2

                        if line in lines:
                            self.lines.remove(line)
                            print "line removed"

            for confirmed in self.confirmed:
                for candidate in self.candidates[:]:
                    if math.fabs(candidate.loc - confirmed.loc) < self.distance_trans and \
                       math.fabs(candidate.angle - confirmed.angle) < self.angle_trans:
                        confirmed.loc = candidate.loc
                        confirmed.angle = candidate.angle
                        if confirmed.last_seen < self.max_lastseen:
                            confirmed.last_seen += 2

                        print "lines"
                        if candidate in self.candidates:
                            self.candidates.remove(candidate)
                            print "line removed"

            for confirmed1 in self.confirmed[:]:
                for confirmed2 in self.confirmed[:]:
                    if math.fabs(confirmed1.loc - confirmed2.loc) < self.distance_threshold and \
                       math.fabs(confirmed1.angle - confirmed2.angle) < self.angle_threshold:
                        if confirmed1.id > confirmed2.id and confirmed1 in self.confirmed:
                            confirmed2.loc == (confirmed2.loc +
                                               confirmed1.loc) / 2
                            confirmed2.angle == (confirmed2.angle +
                                                 confirmed1.angle) / 2
                            self.confirmed.remove(confirmed1)
                            if confirmed2.last_seen < self.max_lastseen:
                                confirmed2.last_seen += 2
                        if confirmed2.id > confirmed1.id and confirmed2 in self.confirmed:
                            confirmed2.loc == (confirmed2.loc +
                                               confirmed1.loc) / 2
                            confirmed2.angle == (confirmed2.angle +
                                                 confirmed1.angle) / 2
                            self.confirmed.remove(confirmed2)
                            if confirmed1.last_seen < self.max_lastseen:
                                confirmed1.last_seen += 2

            for confirmed in self.confirmed[:]:
                confirmed.last_seen -= 1
                if confirmed.last_seen < -10:
                    self.confirmed.remove(confirmed)

            final_lines = []
            for confirmed in self.confirmed:
                final_line = [confirmed.loc, confirmed.angle]
                final_lines.append(final_line)
                print confirmed.id
            candidate_ids = []
            for candidate in self.candidates:
                new_id = candidate.id
                candidate_ids.append(new_id)
            print candidate_ids
            print len(self.candidates)

            libvision.misc.draw_lines(frame, final_lines)
            #libvision.misc.draw_lines2(frame, lines)
            print "Number of Paths:", len(self.confirmed)
            print "Number of Candidates:", len(self.candidates)
            # type -s after the command to run vision for this to work and not produce errors.
            # if len(self.confirmed)>1:
            #    raw_input()

            self.output.paths = []
            center_x = 0
            center_y = 0
            self.output.paths = self.confirmed

            for path in self.output.paths:
                path.theta = path.angle
                center_x = frame.width / 2
                path.x = center_x
                center_y = (-math.cos(path.angle) /
                            (math.sin(path.angle) + .001)) * center_x + (
                                path.loc / ((math.sin(path.angle) + .001)))
                path.y = center_y
                if center_y > frame.height or center_y < 0 or \
                   center_y < self.min_center_distance or \
                   frame.height - center_y < self.min_center_distance:
                    center_y2 = frame.height / 2
                    center_x2 = (center_y2 -
                                 (path.loc /
                                  (math.sin(path.angle) + .0001))) / (
                                      -math.cos(path.angle) /
                                      (math.sin(path.angle) + .0001))

                    if center_x2 > frame.width or center_x2 < 0:
                        path.center = [center_x, center_y]
                    else:
                        path.center = [center_x2, center_y2]
                else:
                    path.center = [center_x, center_y]

                cv.Circle(frame, (int(path.center[0]), int(path.center[1])),
                          15, (255, 255, 255), 2, 8, 0)

            self.return_output()
            svr.debug("Path", frame)
示例#25
0
    def process_frame(self, frame):
        # This is equivalent to the old routine, but it isn't actually necessary
        #height, width, depth = libvision.cv_to_cv2(frame).shape
        #self.debug_frame = np.zeros((height, width, 3), np.uint8)

        # Debug numpy is CV2
        self.debug_frame = libvision.cv_to_cv2(frame)

        # CV2 Transforms
        self.numpy_frame = self.debug_frame.copy()
        self.numpy_frame = cv2.medianBlur(self.numpy_frame, 5)
        self.numpy_frame = cv2.cvtColor(self.numpy_frame, cv2.COLOR_BGR2HSV)

        (self.frame1, self.frame2, self.frame3) = cv2.split(self.numpy_frame)
        # Change the frame number to determine what channel to focus on
        self.numpy_frame = self.frame2

        # Thresholding
        self.numpy_frame = cv2.adaptiveThreshold(self.numpy_frame,
                                                 255,
                                                 cv2.ADAPTIVE_THRESH_MEAN_C,
                                                 cv2.THRESH_BINARY_INV,
                                                 self.adaptive_thresh_blocksize,
                                                 self.adaptive_thresh)

        kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (3, 3))
        kernel2 = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (4, 4))
    # kernel = np.ones((2,2), np.uint8)
        self.numpy_frame = cv2.erode(self.numpy_frame, kernel)
        self.numpy_frame = cv2.dilate(self.numpy_frame, kernel2)

        self.adaptive_frame = self.numpy_frame.copy()

    # Find contours
        contours, hierarchy = cv2.findContours(self.numpy_frame,
                                               cv2.RETR_EXTERNAL,
                                               cv2.CHAIN_APPROX_SIMPLE)
        self.raw_buoys = []

        if len(contours) > 0:
            cnt = contours[0]
            cv2.drawContours(
                self.numpy_frame, contours, -1, (255, 255, 255), 3)

            for h, cnt in enumerate(contours):
                approx = cv2.approxPolyDP(
                    cnt, 0.01 * cv2.arcLength(cnt, True), True)

                center, radius = cv2.minEnclosingCircle(cnt)
                x, y = center

                if len(approx) > 12:
                    if (radius > 30):
                        new_buoy = Buoy(int(x), int(y), int(radius), "unknown")
                        new_buoy.id = self.recent_id
                        self.recent_id += 1
                        self.raw_buoys.append(new_buoy)
                        cv2.drawContours(
                            self.numpy_frame, [cnt], 0, (0, 0, 255), -1)
                        self.raw_buoys.append(new_buoy)

        for buoy1 in self.raw_buoys[:]:
            for buoy2 in self.raw_buoys[:]:
                if buoy1 is buoy2:
                    continue
                if buoy1 in self.raw_buoys and buoy2 in self.raw_buoys and \
                   math.fabs(buoy1.centerx - buoy2.centerx) > self.mid_sep and \
                   math.fabs(buoy1.centery - buoy2.centery) > self.mid_sep:
                    if buoy1.radius < buoy2.radius:
                        self.raw_buoys.remove(buoy1)
                    elif buoy2.radius < buoy1.radius:
                        self.raw_buoys.remove(buoy2)

        for buoy in self.raw_buoys:
            self.match_buoys(buoy)

        self.sort_buoys()
        self.draw_buoys()

        self.return_output()

        self.debug_to_cv = libvision.cv2_to_cv(self.debug_frame)
        self.numpy_to_cv = libvision.cv2_to_cv(self.numpy_frame)
        self.adaptive_to_cv = libvision.cv2_to_cv(self.adaptive_frame)

        svr.debug("processed", self.numpy_to_cv)
        svr.debug("adaptive", self.adaptive_to_cv)
        svr.debug("debug", self.debug_to_cv)

        # Convert to output format
        self.output.buoys = []
        if self.raw_buoys is not None and len(self.raw_buoys) > 0:
    	    for buoy in self.raw_buoys:
		x = buoy.centerx
                y = buoy.centery
                buoy = Container()
                buoy.theta = x
                buoy.phi = y
                buoy.id = 1
                self.output.buoys.append(buoy)

        if self.output.buoys:
            self.return_output()
        return self.output
示例#26
0
    def process_frame(self, frame):

        # Resize image to 320x240
        #copy = cv.CreateImage(cv.GetSize(frame), 8, 3)
        #cv.Copy(frame, copy)
        #cv.SetImageROI(frame, (0, 0, 320, 240))
        #cv.Resize(copy, frame, cv.CV_INTER_NN)

        found_hedge = False

        cv.Smooth(frame, frame, cv.CV_MEDIAN, 7, 7)

        # Set binary image to have value channel
        hsv = cv.CreateImage(cv.GetSize(frame), 8, 3)
        binary = cv.CreateImage(cv.GetSize(frame), 8, 1)
        cv.CvtColor(frame, hsv, cv.CV_BGR2HSV)
        cv.SetImageCOI(hsv, 1)
        cv.Copy(hsv, binary)
        cv.SetImageCOI(hsv, 0)

        cv.AdaptiveThreshold(
            binary,
            binary,
            255,
            cv.CV_ADAPTIVE_THRESH_MEAN_C,
            cv.CV_THRESH_BINARY_INV,
            self.adaptive_thresh_blocksize,
            self.adaptive_thresh,
        )

        # Morphology
        '''
        kernel = cv.CreateStructuringElementEx(3, 3, 1, 1, cv.CV_SHAPE_ELLIPSE)
        cv.Erode(binary, binary, kernel, 1)
        cv.Dilate(binary, binary, kernel, 1)
        '''
        if self.debug:
            color_filtered = cv.CloneImage(binary)

        # Get Edges
        #cv.Canny(binary, binary, 30, 40)

        # Hough Transform
        line_storage = cv.CreateMemStorage()
        raw_lines = cv.HoughLines2(binary,
                                   line_storage,
                                   cv.CV_HOUGH_STANDARD,
                                   rho=1,
                                   theta=math.pi / 180,
                                   threshold=self.hough_threshold,
                                   param1=0,
                                   param2=0)

        # Get vertical lines
        vertical_lines = []
        for line in raw_lines:
            if line[1] < self.vertical_threshold or \
                    line[1] > math.pi - self.vertical_threshold:

                vertical_lines.append((abs(line[0]), line[1]))

        # Group vertical lines
        vertical_line_groups = [
        ]  # A list of line groups which are each a line list
        for line in vertical_lines:
            group_found = False
            for line_group in vertical_line_groups:

                if line_group_accept_test(line_group, line, self.max_range):
                    line_group.append(line)
                    group_found = True

            if not group_found:
                vertical_line_groups.append([line])

        # Average line groups into lines
        vertical_lines = []
        for line_group in vertical_line_groups:
            rhos = map(lambda line: line[0], line_group)
            angles = map(lambda line: line[1], line_group)
            line = (sum(rhos) / len(rhos), circular_average(angles, math.pi))
            vertical_lines.append(line)

        # Get horizontal lines
        horizontal_lines = []
        for line in raw_lines:
            dist_from_horizontal = (math.pi / 2 + line[1]) % math.pi
            if dist_from_horizontal < self.horizontal_threshold or \
                    dist_from_horizontal > math.pi - self.horizontal_threshold:

                horizontal_lines.append((abs(line[0]), line[1]))

        # Group horizontal lines
        horizontal_line_groups = [
        ]  # A list of line groups which are each a line list
        for line in horizontal_lines:
            group_found = False
            for line_group in horizontal_line_groups:

                if line_group_accept_test(line_group, line, self.max_range):
                    line_group.append(line)
                    group_found = True

            if not group_found:
                horizontal_line_groups.append([line])

        if len(horizontal_line_groups) is 1:
            self.seen_crossbar = True
            rhos = map(lambda line: line[0], horizontal_line_groups[0])
            angles = map(lambda line: line[1], horizontal_line_groups[0])
            line = (sum(rhos) / len(rhos), circular_average(angles, math.pi))
            horizontal_lines = [line]
        else:
            self.seen_crossbar = False
            horizontal_lines = []

        self.left_pole = None
        self.right_pole = None
        if len(vertical_lines) is 2:
            roi = cv.GetImageROI(frame)
            width = roi[2]
            height = roi[3]
            self.left_pole = round(
                min(vertical_lines[0][0], vertical_lines[1][0]), 2) - width / 2
            self.right_pole = round(
                max(vertical_lines[0][0], vertical_lines[1][0]), 2) - width / 2
        # TODO: If one pole is seen, is it left or right pole?

        # Calculate planar distance r (assuming we are moving perpendicular to
        # the hedge)
        if self.left_pole and self.right_pole:
            theta = abs(self.left_pole - self.right_pole)
            self.r = 3 / tan(radians(theta / 2))
        else:
            self.r = None

        if self.r and self.seen_crossbar:
            bar_phi = (-1 * horizontal_lines[0][0] +
                       frame.height / 2) / (frame.height / 2) * 32
            self.crossbar_depth = self.r * atan(radians(bar_phi))
        else:
            self.crossbar_depth = None

        if self.debug:
            cv.CvtColor(color_filtered, frame, cv.CV_GRAY2RGB)
            libvision.misc.draw_lines(frame, vertical_lines)
            libvision.misc.draw_lines(frame, horizontal_lines)

            #cv.ShowImage("Hedge", cv.CloneImage(frame))
            svr.debug("Hedge", cv.CloneImage(frame))

        # populate self.output with infos
        self.output.seen_crossbar = self.seen_crossbar
        self.output.left_pole = self.left_pole
        self.output.right_pole = self.right_pole
        self.output.r = self.r
        self.output.crossbar_depth = self.crossbar_depth

        self.return_output()
        print self
示例#27
0
    def process_frame(self, frame):
        # Creation of frames
        self.debug_frame = cv.CreateImage(cv.GetSize(frame), 8, 3)
        cv.Copy(frame, self.debug_frame)
        self.debug_numpy_frame = cv_to_cv2(self.debug_frame)

        # CV2 Transforms
        self.numpy_frame = cv_to_cv2(self.debug_frame)

        self.numpy_frame = cv2.medianBlur(self.numpy_frame, 7)
        self.numpy_frame = cv2.cvtColor(self.numpy_frame, cv.CV_BGR2HSV)

        [self.frame1, self.frame2, self.frame3] = numpy.dsplit(
            self.numpy_frame, 3)
        # Change the frame number to determine what channel to focus on
        self.numpy_frame = self.frame3

        '''Temporarily converts the image to a cv frame to do the adaptive threshold '''
        self.temp_cv2frame = cv2_to_cv(self.numpy_frame)

        cv.AdaptiveThreshold(self.temp_cv2frame, self.temp_cv2frame,
                             255,
                             cv.CV_ADAPTIVE_THRESH_MEAN_C,
                             cv.CV_THRESH_BINARY_INV,
                             self.adaptive_thresh_blocksize,
                             self.adaptive_thresh,
                             )

        kernel = cv.CreateStructuringElementEx(5, 5, 3, 3, cv.CV_SHAPE_ELLIPSE)
        cv.Erode(self.temp_cv2frame, self.temp_cv2frame, kernel, 1)
        cv.Dilate(self.temp_cv2frame, self.temp_cv2frame, kernel, 1)

        self.adaptive_frame = cv.CreateImage(cv.GetSize(frame), 8, 1)
        cv.Copy(self.temp_cv2frame, self.adaptive_frame)
        '''Returns the frame to a cv2 image'''
        self.numpy_frame = cv_to_cv2(self.temp_cv2frame)

        '''Begins finding contours'''
        contours, hierarchy = cv2.findContours(
            self.numpy_frame, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
        self.raw_bins = []
        if len(contours) > 1:
            cnt = contours[0]
            cv2.drawContours(
                self.numpy_frame, contours, -1, (255, 255, 255), 3)
            self.masks = []
            pts = []
            for h, cnt in enumerate(contours):
                mask = numpy.zeros(self.numpy_frame.shape, numpy.uint8)
                cv2.drawContours(mask, [cnt], 0, 255, -1)
                mean = cv2.mean(cv_to_cv2(self.debug_frame), mask=mask)
                self.masks.append(mask)

                hull = cv2.convexHull(cnt)
                rect = cv2.minAreaRect(cnt)
                box = cv2.cv.BoxPoints(rect)
                box = numpy.int0(box)
                new_bin = Bin(tuple(box[0]), tuple(
                    box[1]), tuple(box[2]), tuple(box[3]))
                new_bin.id = self.recent_id
                self.recent_id = self.recent_id + 1
                self.raw_bins.append(new_bin)
                for pt in box:
                    type(tuple(pt))
                    cv2.circle(self.numpy_frame, tuple(
                        pt), 5, (255, 255, 255), -1, 8, 0)
                    pts.append(pt)

                '''Removes bins that have centers too close to others (to prevent bins inside bins), and bins that are too small'''
                for bin1 in self.raw_bins[:]:
                    for bin2 in self.raw_bins[:]:
                        if bin1 in self.raw_bins and bin2 in self.raw_bins and math.fabs(bin1.midx - bin2.midx) < self.mid_sep and math.fabs(bin1.midy - bin2.midy) < self.mid_sep:
                            if bin1.area < bin2.area:
                                self.raw_bins.remove(bin1)
                            elif bin2.area < bin1.area:
                                self.raw_bins.remove(bin2)
                        if bin1 in self.raw_bins and bin2 in self.raw_bins:
                            if bin1.area < self.min_area:
                                self.raw_bins.remove(bin1)
                            if bin2.area < self.min_area and bin2 in self.raw_bins:
                                self.raw_bins.remove(bin2)

        for bin in self.raw_bins:
            self.match_bins(bin)
        self.sort_bins()

        self.numpy_to_cv = cv2_to_cv(self.numpy_frame)
        self.debug_final_frame = cv2_to_cv(self.debug_numpy_frame)
        self.draw_bins()

        svr.debug("CV", self.debug_final_frame)
        svr.debug("CV2", self.numpy_to_cv)
        svr.debug("Adaptive", self.adaptive_frame)
示例#28
0
    def process_frame(self, frame):
        # Debug numpy is CV2
        self.debug_frame = libvision.cv_to_cv2(frame)

        # CV2 Transforms
        self.numpy_frame = self.debug_frame.copy()
        self.numpy_frame = cv2.medianBlur(self.numpy_frame, 5)
        self.numpy_frame = cv2.cvtColor(self.numpy_frame, cv2.COLOR_BGR2HSV)

        (self.frame1, self.frame2, self.frame3) = cv2.split(self.numpy_frame)
        # Change the frame number to determine what channel to focus on
        self.numpy_frame = self.frame2

        # Thresholding
        self.buoy_frame = cv2.adaptiveThreshold(self.numpy_frame,
                                                255,
                                                cv2.ADAPTIVE_THRESH_MEAN_C,
                                                cv2.THRESH_BINARY_INV,
                                                self.adaptive_thresh_blocksize,
                                                self.adaptive_thresh)

        kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (3, 3))
        kernel2 = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (4, 4))

        self.buoy_frame = cv2.erode(self.numpy_frame, kernel)
        self.buoy_frame = cv2.dilate(self.numpy_frame, kernel2)

        self.buoy_adaptive = self.buoy_frame.copy()

    # Find contours
        contours, hierarchy = cv2.findContours(self.numpy_frame,
                                               cv2.RETR_TREE,
                                               cv2.CHAIN_APPROX_SIMPLE)
        self.raw_led = []
        self.raw_buoys = []

        if len(contours) > 1:
            cnt = contours[0]
            cv2.drawContours(
                self.numpy_frame, contours, -1, (255, 255, 255), 3)

            for h, cnt in enumerate(contours):
                approx = cv2.approxPolyDP(
                    cnt, 0.01 * cv2.arcLength(cnt, True), True)

                center, radius = cv2.minEnclosingCircle(cnt)
                x, y = center

                if len(approx) > 12:
                    if (radius > 30):
                        new_buoy = Buoy(int(x), int(y), int(radius), "unknown")
                        new_buoy.id = self.recent_id
                        self.recent_id += 1
                        self.raw_buoys.append(new_buoy)
                        cv2.drawContours(
                            self.numpy_frame, [cnt], 0, (0, 0, 255), -1)
                        self.raw_buoys.append(new_buoy)

        for buoy1 in self.raw_buoys[:]:
            for buoy2 in self.raw_buoys[:]:
                if buoy1 is buoy2:
                    continue
                if buoy1 in self.raw_buoys and buoy2 in self.raw_buoys and \
                   math.fabs(buoy1.centerx - buoy2.centerx) > self.mid_sep and \
                   math.fabs(buoy1.centery - buoy2.centery) > self.mid_sep:
                    if buoy1.area < buoy2.area:
                        self.raw_buoys.remove(buoy1)
                    elif buoy2.area < buoy1.area:
                        self.raw_buoys.remove(buoy2)

        for buoy in self.raw_buoys:
            self.match_buoys(buoy)

        self.sort_buoys()
        self.draw_buoys()

        self.return_output()

        self.debug_to_cv = libvision.cv2_to_cv(self.debug_frame)
        self.numpy_to_cv = libvision.cv2_to_cv(self.numpy_frame)
        self.adaptive_to_cv = libvision.cv2_to_cv(self.adaptive_frame)

        svr.debug("processed", self.numpy_to_cv)
        svr.debug("adaptive", self.adaptive_to_cv)
        svr.debug("debug", self.debug_to_cv)
示例#29
0
    def process_frame(self, frame):
        # This is equivalent to the old routine, but it isn't actually necessary
        #height, width, depth = libvision.cv_to_cv2(frame).shape
        #self.debug_frame = np.zeros((height, width, 3), np.uint8)

        # Debug numpy is CV2
        self.debug_frame = libvision.cv_to_cv2(frame)

        # CV2 Transforms
        self.numpy_frame = self.debug_frame.copy()
        self.numpy_frame = cv2.medianBlur(self.numpy_frame, 5)
        self.numpy_frame = cv2.cvtColor(self.numpy_frame, cv2.COLOR_BGR2HSV)

        (self.frame1, self.frame2, self.frame3) = cv2.split(self.numpy_frame)
        # Change the frame number to determine what channel to focus on
        self.numpy_frame = self.frame2

        # Thresholding
        self.numpy_frame = cv2.adaptiveThreshold(
            self.numpy_frame, 255, cv2.ADAPTIVE_THRESH_MEAN_C,
            cv2.THRESH_BINARY_INV, self.adaptive_thresh_blocksize,
            self.adaptive_thresh)

        kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (3, 3))
        kernel2 = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (4, 4))
        # kernel = np.ones((2,2), np.uint8)
        self.numpy_frame = cv2.erode(self.numpy_frame, kernel)
        self.numpy_frame = cv2.dilate(self.numpy_frame, kernel2)

        self.adaptive_frame = self.numpy_frame.copy()

        # Find contours
        contours, hierarchy = cv2.findContours(self.numpy_frame,
                                               cv2.RETR_EXTERNAL,
                                               cv2.CHAIN_APPROX_SIMPLE)
        self.raw_buoys = []

        if len(contours) > 0:
            cnt = contours[0]
            cv2.drawContours(self.numpy_frame, contours, -1, (255, 255, 255),
                             3)

            for h, cnt in enumerate(contours):
                approx = cv2.approxPolyDP(cnt, 0.01 * cv2.arcLength(cnt, True),
                                          True)

                center, radius = cv2.minEnclosingCircle(cnt)
                x, y = center

                if len(approx) > 12:
                    if (radius > 30):
                        new_buoy = Buoy(int(x), int(y), int(radius), "unknown")
                        new_buoy.id = self.recent_id
                        self.recent_id += 1
                        self.raw_buoys.append(new_buoy)
                        cv2.drawContours(self.numpy_frame, [cnt], 0,
                                         (0, 0, 255), -1)
                        self.raw_buoys.append(new_buoy)

        for buoy1 in self.raw_buoys[:]:
            for buoy2 in self.raw_buoys[:]:
                if buoy1 is buoy2:
                    continue
                if buoy1 in self.raw_buoys and buoy2 in self.raw_buoys and \
                   math.fabs(buoy1.centerx - buoy2.centerx) > self.mid_sep and \
                   math.fabs(buoy1.centery - buoy2.centery) > self.mid_sep:
                    if buoy1.radius < buoy2.radius:
                        self.raw_buoys.remove(buoy1)
                    elif buoy2.radius < buoy1.radius:
                        self.raw_buoys.remove(buoy2)

        for buoy in self.raw_buoys:
            self.match_buoys(buoy)

        self.sort_buoys()
        self.draw_buoys()

        self.return_output()

        self.debug_to_cv = libvision.cv2_to_cv(self.debug_frame)
        self.numpy_to_cv = libvision.cv2_to_cv(self.numpy_frame)
        self.adaptive_to_cv = libvision.cv2_to_cv(self.adaptive_frame)

        svr.debug("processed", self.numpy_to_cv)
        svr.debug("adaptive", self.adaptive_to_cv)
        svr.debug("debug", self.debug_to_cv)

        # Convert to output format
        self.output.buoys = []
        if self.raw_buoys is not None and len(self.raw_buoys) > 0:
            for buoy in self.raw_buoys:
                x = buoy.centerx
                y = buoy.centery
                buoy = Container()
                buoy.theta = x
                buoy.phi = y
                buoy.id = 1
                self.output.buoys.append(buoy)

        if self.output.buoys:
            self.return_output()
        return self.output
示例#30
0
    def process_frame(self, frame):

        # Get Channels
        hsv = cv.CreateImage(cv.GetSize(frame), 8, 3)
        cv.CvtColor(frame, hsv, cv.CV_BGR2HSV)
        grey = libvision.misc.get_channel(hsv, 2)

        # load a haar classifier
        #hc = cv.Load("/home/seawolf/software/seawolf5/vision/output.xml")
        hc = cv.Load(
            os.path.join(os.path.dirname(os.path.dirname(__file__)),
                         "buoy_cascade_4.xml"))
        #hc = cv.Load("/home/seawolf/software/seawolf5/vision/buoy_cascade_4.xml")

        # use classifier to detect buoys
        minsize = (int(self.minsize), int(self.minsize))
        maxsize = (int(self.maxsize), int(self.maxsize))
        buoys = cv.HaarDetectObjects(grey,
                                     hc,
                                     cv.CreateMemStorage(),
                                     min_size=minsize)

        # compute average buoy size and extract to a list
        avg_w = 0
        for (x, y, w, h), n in buoys:

            # create a buoy class for this new buoys
            new_buoy = self.new_buoy(x, y, w)

            # make note of size
            avg_w = avg_w + w

            # add this buoy to our list of new buoys
            self.new.append(new_buoy)

        # update search size based on sizes found this frame
        if buoys:
            avg_w = avg_w / len(buoys)
            self.minsize = int(avg_w * .7)
            self.maxsize = int(avg_w * 1.3)

            # determine color of new buoys (if possible)
            libvision.buoy_analyzer(frame, self.new)

        # sort these new buoys into appropriate tier
        self.sort_buoys()  # must be called every frame for upkeep

        #######  DEBUG #######
        if self.debug:

            # display confirmed buoys
            for confirmed in self.confirmed:
                x = confirmed.x
                y = confirmed.y
                w = confirmed.width

                # draw rectangles on frame
                cv.Rectangle(frame, (x, y), (x + w, y + w),
                             confirmed.debug_color,
                             thickness=6)
                cv.Rectangle(frame, (x, y), (x + w, y + w),
                             COLORS[confirmed.color],
                             thickness=-1)

            # show debug frame
            svr.debug("BuoyTest", frame)

        ####### END DEBUG #######

        self.output.buoys = self.confirmed
        for buoy in self.output.buoys:
            buoy.theta = (buoy.x - frame.width / 2) * 37 / (frame.width / 2)
            buoy.phi = -1 * (buoy.y - frame.height / 2) * 36 / (frame.height /
                                                                2)
        self.return_output()
示例#31
0
    def process_frame(self, frame):
        frametest = cv.CreateImage(cv.GetSize(frame), 8, 3)
        binarytest = cv.CreateImage(cv.GetSize(frame), 8, 1)

        cv.Copy(frame, frametest)
        cv.SetImageCOI(frametest, 3)
        cv.Copy(frametest, binarytest)
        cv.SetImageCOI(frametest, 0)
        svr.debug("R?",binarytest)


        # Resize image to 320x240
        #copy = cv.CreateImage(cv.GetSize(frame), 8, 3)
        #cv.Copy(frame, copy)
        #cv.SetImageROI(frame, (0, 0, 320, 240))
        #cv.Resize(copy, frame, cv.CV_INTER_NN)

        found_gate = False

        unchanged_frame = cv.CreateImage(cv.GetSize(frame), 8, 3)
        cv.Copy(frame,unchanged_frame)

        cv.Smooth(frame, frame, cv.CV_MEDIAN, 7, 7)

        # Set binary image to have saturation channel
        hsv = cv.CreateImage(cv.GetSize(frame), 8, 3)
        binary = cv.CreateImage(cv.GetSize(frame), 8, 1)
        cv.CvtColor(frame, hsv, cv.CV_BGR2HSV)
        cv.SetImageCOI(hsv, 1)
        cv.Copy(hsv, binary)
        cv.SetImageCOI(hsv, 0)

        cv.AdaptiveThreshold(binary, binary,
            255,
            cv.CV_ADAPTIVE_THRESH_MEAN_C,
            cv.CV_THRESH_BINARY_INV,
            self.adaptive_thresh_blocksize,
            self.adaptive_thresh,
        )

        # Morphology
        kernel = cv.CreateStructuringElementEx(5, 5, 3, 3, cv.CV_SHAPE_ELLIPSE)
        cv.Erode(binary, binary, kernel, 1)
        cv.Dilate(binary, binary, kernel, 1)
        if self.debug:
            color_filtered = cv.CloneImage(binary)

        # Get Edges
        cv.Canny(binary, binary, 30, 40)

        # Hough Transform
        line_storage = cv.CreateMemStorage()
        raw_lines = cv.HoughLines2(binary, line_storage, cv.CV_HOUGH_STANDARD,
                                   rho=1,
                                   theta=math.pi/180,
                                   threshold=self.hough_threshold,
                                   param1=0,
                                   param2=0
                                   )

        # Get vertical lines
        vertical_lines = []
        for line in raw_lines:
            if line[1] < self.vertical_threshold or \
               line[1] > math.pi-self.vertical_threshold:

                #absolute value does better grouping currently
                vertical_lines.append((abs(line[0]), line[1]))

        # Group vertical lines
        vertical_line_groups = []  # A list of line groups which are each a line list
        for line in vertical_lines:
            group_found = False
            for line_group in vertical_line_groups:

                if line_group_accept_test(line_group, line, self.max_range):
                    line_group.append(line)
                    group_found = True

            if not group_found:
                vertical_line_groups.append([line])

        # Average line groups into lines
        vertical_lines = []
        for line_group in vertical_line_groups:
            rhos = map(lambda line: line[0], line_group)
            angles = map(lambda line: line[1], line_group)
            line = (sum(rhos)/len(rhos), circular_average(angles, math.pi))
            vertical_lines.append(line)

        # Get horizontal lines
        horizontal_lines = []
        for line in raw_lines:
            dist_from_horizontal = (math.pi/2 + line[1]) % math.pi
            if dist_from_horizontal < self.horizontal_threshold or \
               dist_from_horizontal > math.pi-self.horizontal_threshold:

                horizontal_lines.append( (abs(line[0]), line[1]) )

        # Group horizontal lines
        horizontal_line_groups = []  # A list of line groups which are each a line list
        for line in horizontal_lines:
            group_found = False
            for line_group in horizontal_line_groups:

                if line_group_accept_test(line_group, line, self.max_range):
                    line_group.append(line)
                    group_found = True

            if not group_found:
                horizontal_line_groups.append([line])

        if len(horizontal_line_groups) is 1:
            self.seen_crossbar = True
            if self.debug:
                rhos = map(lambda line: line[0], horizontal_line_groups[0])
                angles = map(lambda line: line[1], horizontal_line_groups[0])
                line = (sum(rhos)/len(rhos), circular_average(angles, math.pi))
                horizontal_lines = [line]
        else:
            self.seen_crossbar = False
            horizontal_lines = []

        self.left_pole = None
        self.right_pole = None
        print vertical_lines
        self.returning = 0
        self.found = False
        if len(vertical_lines) is 2:
            roi = cv.GetImageROI(frame)
            width = roi[2]
            height = roi[3]
            self.left_pole = round(min(vertical_lines[0][0], vertical_lines[1][0]), 2) - width/2
            self.right_pole = round(max(vertical_lines[0][0], vertical_lines[1][0]), 2) - width/2

            self.returning = (self.left_pole + self.right_pole)/2
	    print "Returning ", self.returning

            if self.last_seen < 0:
                self.last_center = None
                self.last_seen = 0
            if self.last_center is None:
                self.last_center = self.returning
                self.seen_count = 1
            elif math.fabs(self.last_center - self.returning) < self.center_trans_thresh:
                self.seen_count += 1
                self.last_seen += 2
            else:
                self.last_seen -= 1

            if self.seen_count < self.seen_count_thresh:
                self.left_pole = None
                self.right_pole = None
            else: 
                print "FOUND CENTER AND RETURNED IT"
                self.found = True
        else:
            self.returning = 0
            if self.last_seen < 0:
                self.last_center = None
                self.last_seen = 0
            self.last_seen -= 1
            self.left_pole = None
            self.right_pole = None

            
            
            

        #TODO: If one pole is seen, is it left or right pole?
    
        if self.debug:
            cv.CvtColor(color_filtered, frame, cv.CV_GRAY2RGB)
            libvision.misc.draw_lines(frame, vertical_lines)
            libvision.misc.draw_lines(frame, horizontal_lines)

            if self.found:
                cv.Circle(frame, (int(frame.width/2 + self.returning), int(frame.height/2)),
                       15, (0, 255,0), 2, 8, 0)
                font = cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, 1, 1, 1, 3)
                cv.PutText(frame, "Gate Sent to Mission Control", (100, 400) , font, (255, 255, 0))
                print frame.width

            #cv.ShowImage("Gate", cv.CloneImage(frame))
            svr.debug("Gate", cv.CloneImage(frame))
            svr.debug("Unchanged",cv.CloneImage(unchanged_frame))


        #populate self.output with infos
        self.output.seen_crossbar = self.seen_crossbar
        self.output.left_pole = self.left_pole
        self.output.right_pole = self.right_pole



        self.return_output()
        print self
示例#32
0
    def process_frame(self, frame):
        ################
        #setup CV ######
        ################
        print "processing frame"
        (w,h) = cv.GetSize(frame)

        #generate hue selection frames
        ones = np.ones((h,w,1), dtype='uint8')
        a = ones*(180 - self.target_hue)
        b = ones*(180 - self.target_hue + 20)
        a_array = cv.fromarray(a)
        b_array = cv.fromarray(b)

        #create locations for the test frame and binary frame
        frametest = cv.CreateImage(cv.GetSize(frame), 8, 3)
        binarytest = cv.CreateImage(cv.GetSize(frame), 8, 1)

        #use the red channel for the binary frame (just for debugging purposes)
        cv.Copy(frame, frametest)
        cv.SetImageCOI(frametest, 3)
        cv.Copy(frametest, binarytest)

        #reset the COI for test frame to RGB.
        cv.SetImageCOI(frametest, 0)


        # Resize image to 320x240
        #copy = cv.CreateImage(cv.GetSize(frame), 8, 3)
        #cv.Copy(frame, copy)
        #cv.SetImageROI(frame, (0, 0, 320, 240))
        #cv.Resize(copy, frame, cv.CV_INTER_NN)
        found_gate = False

        #create a new frame for comparison purposes
        unchanged_frame = cv.CreateImage(cv.GetSize(frame), 8, 3)
        cv.Copy(frame,unchanged_frame)

        #apply noise filter #1
        cv.Smooth(frame, frame, cv.CV_MEDIAN, 7, 7)

        # Set binary image to have saturation channel
        hsv = cv.CreateImage(cv.GetSize(frame), 8, 3)
        binary = cv.CreateImage(cv.GetSize(frame), 8, 1)
        cv.CvtColor(frame, hsv, cv.CV_BGR2HSV)
        cv.SetImageCOI(hsv, 1)
        cv.Copy(hsv, binary)

        #spin the color wheel (psuedo-code for later if necessary)
        # truncate spectrum marked as end
        # shift all values up based on truncating value (mask out 0 regions)
        # take truncated bits, and flip them (180->0, 179->1...)
        # dnow that truncated bits are flipped, add them back in to final image

	    #Reset hsv COI
        cv.SetImageCOI(hsv, 0)

        #correct for wraparound on red spectrum
        cv.InRange(binary,a_array,b_array,binarytest) #generate mask
        cv.Add(binary,cv.fromarray(ones*180),binary,mask=binarytest) #use mask to selectively add values

        #run adaptive threshold for edge detection
        cv.AdaptiveThreshold(binary, binary,
            255,
            cv.CV_ADAPTIVE_THRESH_MEAN_C,
            cv.CV_THRESH_BINARY_INV,
            self.adaptive_thresh_blocksize,
            self.adaptive_thresh,
        )

        # Morphology
        kernel = cv.CreateStructuringElementEx(5, 5, 3, 3, cv.CV_SHAPE_ELLIPSE)
        cv.Erode(binary, binary, kernel, 1)
        cv.Dilate(binary, binary, kernel, 1)
        if self.debug:
            color_filtered = cv.CloneImage(binary)

        # Get Edges
        cv.Canny(binary, binary, 30, 40)

        # Hough Transform
        line_storage = cv.CreateMemStorage()
        raw_lines = cv.HoughLines2(binary, line_storage, cv.CV_HOUGH_STANDARD,
                                   rho=1,
                                   theta=math.pi/180,
                                   threshold=self.hough_threshold,
                                   param1=0,
                                   param2=0
        )
        
        # Get vertical lines
        vertical_lines = []
        i = 0
        for line in raw_lines:
            if line[1] < self.vertical_threshold or \
                line[1] > (math.pi-self.vertical_threshold):

                #absolute value does better grouping currently
                vertical_lines.append( (abs(line[0]),line[1]) )
            i += 1

        # print message to user for performance purposes
        logging.debug("{} possibilities reduced to {} lines".format(
                      i, len(vertical_lines)
                      )
                    )
        
        # Group vertical lines
        vertical_line_groups = [] #A list of line groups which are each a line list
        i = 0
        for line in vertical_lines:
            group_found = False
            for line_group in vertical_line_groups:
                i+=1
                if line_group_accept_test(line_group, line, self.max_range):
                    line_group.append(line)
                    group_found = True

            if not group_found:
                vertical_line_groups.append([line])
        
        #quick debugging statement
        logging.debug("{} internal iterations for {} groups".format(i, len(vertical_line_groups)))              

        # Average line groups into lines
        vertical_lines = []
        for line_group in vertical_line_groups:
            rhos = map(lambda line: line[0], line_group) #get rho of each line
            angles = map(lambda line: line[1], line_group)
            line = (sum(rhos)/len(rhos), circular_average(angles, math.pi))
            vertical_lines.append(line)


        self.left_pole = None
        self.right_pole = None
        self.returning = 0
        self.found = False

        if len(vertical_lines) is 2:
            roi = cv.GetImageROI(frame)
            width = roi[2]
            height = roi[3]
            self.left_pole = round(min(vertical_lines[0][0], vertical_lines[1][0]), 2) - width/2
            self.right_pole = round(max(vertical_lines[0][0], vertical_lines[1][0]), 2) - width/2
            
            
            self.returning = (self.left_pole + self.right_pole)/2
            logging.info("Returning {}".format(self.returning))

            #If this is first iteration, count this as seeing the gate
            if self.last_seen < 0:
                self.last_center = None
                self.last_seen = 0

            #increment a counter if result is good.
            if self.last_center is None:
                self.last_center = self.returning
                self.seen_count = 1
            elif math.fabs(self.last_center - self.returning) < self.center_trans_thresh:
                self.seen_count += 1
                self.last_seen += 2
            else:
                self.last_seen -= 1

            #if not convinced, forget left/right pole. Else, proclaim success.
            if self.seen_count < self.seen_count_thresh:
                self.left_pole = None
                self.right_pole = None
            else:
                print "FOUND CENTER AND RETURNED IT"
                self.found = True

        else:
            self.returning = 0

            if self.last_seen < 0:
                self.last_center = None
                self.last_seen = 0

            self.last_seen -= 1
            self.left_pole = None
            self.right_POLE = None

        #extra debugging stuff
        if self.debug:
            cv.CvtColor(color_filtered, frame, cv.CV_GRAY2RGB)
            libvision.misc.draw_lines(frame, vertical_lines)

            if self.found:
                cv.Circle(frame, (int(frame.width/2 + self.returning), int(frame.height/2)),
                       15, (0, 255,0), 2, 8, 0)
                font = cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, 1, 1, 1, 3)
                cv.PutText(frame, "Gate Sent to Mission Control", (100, 400) , font, (255, 255, 0))
                #print frame.width


        #cv.ShowImage("Gate", cv.CloneImage(frame))
        svr.debug("Gate", cv.CloneImage(frame))
        svr.debug("Unchanged", cv.CloneImage(unchanged_frame))

        self.return_output()
示例#33
0
    def process_frame(self, frame):
        if self.path_manager.start_angle is None:
            self.path_manager.start_angle = get_yaw()

        self.output.found = False

        cv.Smooth(frame, frame, cv.CV_MEDIAN, 7, 7)

        # Use RGB color finder
        binary = libvision.cmodules.target_color_rgb.find_target_color_rgb(frame, 250, 125, 0, 1500, 500, .3)
        color_filtered = cv.CloneImage(binary)

        blob_map = cv.CloneImage(binary)
        blobs = libvision.blob.find_blobs(binary, blob_map, min_blob_size=50, max_blobs=10)

        if not blobs:
            return

        binary = cv.CloneImage(blob_map)
        mapping = [0] * 256
        for blob in blobs:
            mapping[blob.id] = 255
        libvision.greymap.greymap(blob_map, binary, mapping)

        # Get Edges
        cv.Canny(binary, binary, 30, 40)

        # Hough Transform
        line_storage = cv.CreateMemStorage()
        lines = cv.HoughLines2(binary, line_storage, cv.CV_HOUGH_STANDARD,
                               rho=1,
                               theta=math.pi/180,
                               threshold=self.hough_threshold,
                               param1=0,
                               param2=0
        )
        lines = lines[:self.lines_to_consider]  # Limit number of lines

        if not lines:
            return

        paths = self.path_manager.process(lines, blobs)

        if paths and not self.path:
            # If path[1] is clockwise of paths[0]
            distance = circular_distance(paths[0].angle, paths[1].angle)

            print
            print "Distance: ", distance
            print paths[0].theta, paths[0].angle
            print paths[1].theta, paths[1].angle

            if distance > 0:
                self.path = paths[self.which_path]
            else:
                self.path = paths[1 - self.which_path]

            print self.path.angle, self.path.theta
            print

        if paths and self.path in paths and self.path.blobs:
            temp_map = cv.CloneImage(blob_map)

            mapping = [0] * 256
            for blob in self.path.blobs:
                mapping[blob.id] = 255
            libvision.greymap.greymap(blob_map, temp_map, mapping)
            center = self.find_centroid(temp_map)

            svr.debug("map", temp_map)

            self.path.center = (
                 center[0] - (frame.width / 2),
                -center[1] + (frame.height / 2)
            )

            self.output.found = True
            self.output.theta = self.path.theta
            self.output.x = self.path.center[0] / (frame.width / 2)
            self.output.y = self.path.center[1] / (frame.height / 2)
            print self.output

        if self.debug:
            # Show color filtered
            color_filtered_rgb = cv.CreateImage(cv.GetSize(frame), 8, 3)
            cv.CvtColor(color_filtered, color_filtered_rgb, cv.CV_GRAY2RGB)
            cv.SubS(color_filtered_rgb, (255, 0, 0), color_filtered_rgb)
            cv.Sub(frame, color_filtered_rgb, frame)

            # Show edges
            binary_rgb = cv.CreateImage(cv.GetSize(frame), 8, 3)
            cv.CvtColor(binary, binary_rgb, cv.CV_GRAY2RGB)
            cv.Add(frame, binary_rgb, frame)  # Add white to edge pixels
            cv.SubS(binary_rgb, (0, 0, 255), binary_rgb)
            cv.Sub(frame, binary_rgb, frame)  # Remove all but Red

            theta = math.radians(circular_distance(self.path_manager.start_angle, get_yaw()))
            if theta < 0:
                scale = math.cos(-2 * theta)
                theta = pi + theta
                libvision.misc.draw_lines(frame, [((-frame.width/2)*scale, theta)])
            else:
                libvision.misc.draw_lines(frame, [(frame.width/2, theta)])

            # Show lines
            if self.output.found:
                rounded_center = (
                    int(round(center[0])),
                    int(round(center[1])),
                    )
                cv.Circle(frame, rounded_center, 5, (0, 255, 0))
                libvision.misc.draw_lines(frame, [(frame.width/2, self.path.theta)])

            else:
                libvision.misc.draw_lines(frame, lines)

            svr.debug("Path", frame)

        self.return_output()
示例#34
0
    def process_frame(self, frame):
        # frame types:
        #self.debug_frame -- Frame containing helpful debug information

        # Debug numpy in CV2
        raw_frame        = libvision.cv_to_cv2(frame)
        self.debug_frame = raw_frame

        # CV2 blur
        blur_frame = cv2.medianBlur(self.debug_frame, 5)

        # collect brightly colored areas
        frame1 = self.adaptive_threshold(blur_frame, 4,
                                self.adaptive_thresh_blocksize,
                                self.adaptive_thresh)

        # collect shadowes under colored areas
        frame2 = self.adaptive_threshold(blur_frame, 1,
                                self.shadow_thresh_blocksize,
                                self.shadow_thresh)
        
        # use composite as the adaptive threshold
        adaptive_frame = cv2.add(frame1, frame2*0)
        frame          = adaptive_frame
        
        # morphology
        sequence = ([-self.erode_factor, self.erode_factor]*1 
                   +[self.bloom_factor, -self.bloom_factor]*1)

        despeckled_frame = self.morphology(frame, sequence)
        frame            = despeckled_frame

        self.debug_stream("despeckled", despeckled_frame)

        # collect edges
        # ROI_edge detection
        edge_frame = self.ROI_edge_detection(raw_frame, frame, self.edge_threshold, 0, True)
        
        # collect buoy candidates using hough circles
        self.raw_circles = []
        self.raw_buoys = []
        self.raw_circles = cv2.HoughCircles(
                                image   =edge_frame, 
                                method  =cv2.cv.CV_HOUGH_GRADIENT,
                                dp      =self.inv_res_ratio, 
                                minDist =self.center_sep,
                                param1  =self.upper_canny_thresh,
                                param2  =self.acc_thresh,
                                minRadius=self.min_radius,
                                maxRadius=self.max_radius,
                        )
        if self.raw_circles is not None:
            self.raw_circles = np.round(self.raw_circles[:,0]).astype(int)


        # create a new buoy object for every circle that is detected
        #print(self.raw_circles)
        if self.raw_circles is not None:
            #print self.confirmed
            for circle in self.raw_circles:
                (x, y, radius) = circle
                new_buoy = Buoy(x, y, radius, "unknown", self.next_id)
                self.next_id += 1
                self.raw_buoys.append(new_buoy) 
                self.match_buoys(new_buoy)

                cv2.circle(self.debug_frame, (x, y),
                            int(radius), (0, 255, 0), 5)

        # sort buoys among confirmed/canditates
        self.sort_buoys()
        
        # self.debug_frame= cv2.add(<HUD_FRAME>,cv2.cvtColor(<annotated_frame>, cv2.COLOR_GRAY2BGR) )
        # perform color detection
        self.hsv_frame = cv2.cvtColor(raw_frame, cv2.COLOR_BGR2HSV)[:,:,:]
        if self.confirmed is not None and len(self.confirmed) > 0:
        
            # vvv start color detection 
            for buoy in self.confirmed:
                self.debug_frame = self.detect_buoy(buoy,self.debug_frame,self.hsv_frame)
                """
                # draw a cirle around the confirmed bouy
                cv2.circle(self.debug_frame, (int(buoy.centerx), int(buoy.centery)),
                            int(buoy.radius) + 10, (255, 255, 255), 5)
                           
                # attain hue from a pixel on the buoy
                color_pick_point = ( int(buoy.centerx), int(buoy.centery - buoy.radius/2) )
                _c  = color_pick_point
                # ^^offset a couple pixels upward for some reason
                (total_height, total_width, _) = self.hsv_frame.shape
                colorHue = np.mean(self.hsv_frame[in_range(_c[1]-buoy.radius/2,0,total_width) 
                                                    : in_range(_c[1]+buoy.radius/2, 0, total_width),
                                                  in_range(_c[0]-buoy.radius/2, 0, total_height) 
                                                    : in_range(_c[0]+buoy.radius/2, 0, total_height),
                                                     
                                                  0])
                print(_c[0],_c[1], buoy.radius/2)
                print(buoy.centery-20, buoy.centerx)
                
                if BUOY_COLOR_PRINTS:
                    print("buoy%d has a hue of %d" %(buoy.id,int(colorHue)))
                
                # note: color wraps around at 180. Range is 0->180
                if (colorHue >= 0 and colorHue < 45) or colorHue >= 95: # 105->180->45
                    cv2.putText(self.debug_frame,str(buoy.id)+"RED", (int(buoy.centerx), int(buoy.centery)), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255))
                    buoy.color = "red"
                elif (colorHue >= 80 and colorHue < 95): # green is hardest to detect
                    cv2.putText(self.debug_frame,str(buoy.id)+"GRE", (int(buoy.centerx), int(buoy.centery)), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255))
                    
                    #if buoy.color != "red" and buoy.color != "yellow":
                    #print "switched from ", buoy.color
                    buoy.color = "green"
                        
                else: #yellow is about 50->80
                    cv2.putText(self.debug_frame,str(buoy.id)+"YEL", (int(buoy.centerx), int(buoy.centery)), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255))
                    buoy.color = "yellow"
                
                
                #print(buoy.centerx)
                
                cv2.putText(self.debug_frame,"HUE="+str(int(colorHue)), (int(buoy.centerx), int(buoy.centery-20)), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255))
                cv2.putText(self.debug_frame,"last_seen="+str(int(buoy.lastseen)), (int(buoy.centerx), int(buoy.centery-40)), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255))
                cv2.putText(self.debug_frame,"candidate="+str(int(buoy in self.candidates)), (int(buoy.centerx), int(buoy.centery-60)), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255))
           
            # ^^^ end color detection
                """

        # debug frames
        self.debug_to_cv = libvision.cv2_to_cv(self.debug_frame)
        #self.numpy_to_cv = libvision.cv2_to_cv(self.numpy_frame)
        self.adaptive_to_cv = libvision.cv2_to_cv(adaptive_frame)
        #svr.debug("processed", self.numpy_to_cv)
        svr.debug("adaptive", self.adaptive_to_cv)
        svr.debug("debug", self.debug_to_cv)

        # generate vision output
        FOV_x = 71.0
        FOV_y = 40.0
        x_resolution = frame.shape[1]
        y_resolution = frame.shape[0]


        self.output.buoys = []
        if self.confirmed is not None and len(self.confirmed) > 0:
            for buoy in self.confirmed:

                buoy.theta = (buoy.centerx - x_resolution/2.0) / (x_resolution/2.0) * (FOV_x/2.0) #<- a rough approximation
                buoy.phi = -(buoy.centery - y_resolution/2.0) / (y_resolution/2.0) * (FOV_y/2.0)  #<- a rough approximation
                buoy.id = buoy.id
                self.output.buoys.append(buoy)

        # publish output
        #print ("%d buoys currently confirmed." % len(self.confirmed))
        if self.output.buoys:
            self.return_output()
        return self.output
示例#35
0
    def process_frame(self, frame):

        # Resize image to 320x240
        #copy = cv.CreateImage(cv.GetSize(frame), 8, 3)
        #cv.Copy(frame, copy)
        #cv.SetImageROI(frame, (0, 0, 320, 240))
        #cv.Resize(copy, frame, cv.CV_INTER_NN)

        found_hedge = False

        cv.Smooth(frame, frame, cv.CV_MEDIAN, 7, 7)

        # Set binary image to have value channel
        hsv = cv.CreateImage(cv.GetSize(frame), 8, 3)
        binary = cv.CreateImage(cv.GetSize(frame), 8, 1)
        cv.CvtColor(frame, hsv, cv.CV_BGR2HSV)
        cv.SetImageCOI(hsv, 1)
        cv.Copy(hsv, binary)
        cv.SetImageCOI(hsv, 0)

        cv.AdaptiveThreshold(binary, binary,
            255,
            cv.CV_ADAPTIVE_THRESH_MEAN_C,
            cv.CV_THRESH_BINARY_INV,
            self.adaptive_thresh_blocksize,
            self.adaptive_thresh,
        )

        # Morphology
        '''
        kernel = cv.CreateStructuringElementEx(3, 3, 1, 1, cv.CV_SHAPE_ELLIPSE)
        cv.Erode(binary, binary, kernel, 1)
        cv.Dilate(binary, binary, kernel, 1)
        '''
        if self.debug:
            color_filtered = cv.CloneImage(binary)

        # Get Edges
        #cv.Canny(binary, binary, 30, 40)

        # Hough Transform
        line_storage = cv.CreateMemStorage()
        raw_lines = cv.HoughLines2(binary, line_storage, cv.CV_HOUGH_STANDARD,
            rho=1,
            theta=math.pi/180,
            threshold=self.hough_threshold,
            param1=0,
            param2=0
        )

        # Get vertical lines
        vertical_lines = []
        for line in raw_lines:
            if line[1] < self.vertical_threshold or \
                line[1] > math.pi-self.vertical_threshold:

                vertical_lines.append( (abs(line[0]), line[1]) )

        # Group vertical lines
        vertical_line_groups = []  # A list of line groups which are each a line list
        for line in vertical_lines:
            group_found = False
            for line_group in vertical_line_groups:

                if line_group_accept_test(line_group, line, self.max_range):
                    line_group.append(line)
                    group_found = True

            if not group_found:
                vertical_line_groups.append([line])

        # Average line groups into lines
        vertical_lines = []
        for line_group in vertical_line_groups:
            rhos = map(lambda line: line[0], line_group)
            angles = map(lambda line: line[1], line_group)
            line = (sum(rhos)/len(rhos), circular_average(angles, math.pi))
            vertical_lines.append(line)

        # Get horizontal lines
        horizontal_lines = []
        for line in raw_lines:
            dist_from_horizontal = (math.pi/2 + line[1]) % math.pi
            if dist_from_horizontal < self.horizontal_threshold or \
                dist_from_horizontal > math.pi-self.horizontal_threshold:

                horizontal_lines.append( (abs(line[0]), line[1]) )

        # Group horizontal lines
        horizontal_line_groups = []  # A list of line groups which are each a line list
        for line in horizontal_lines:
            group_found = False
            for line_group in horizontal_line_groups:

                if line_group_accept_test(line_group, line, self.max_range):
                    line_group.append(line)
                    group_found = True

            if not group_found:
                horizontal_line_groups.append([line])

        if len(horizontal_line_groups) is 1:
            self.seen_crossbar = True
            rhos = map(lambda line: line[0], horizontal_line_groups[0])
            angles = map(lambda line: line[1], horizontal_line_groups[0])
            line = (sum(rhos)/len(rhos), circular_average(angles, math.pi))
            horizontal_lines = [line]
        else:
            self.seen_crossbar = False
            horizontal_lines = []

        self.left_pole = None
        self.right_pole = None
        if len(vertical_lines) is 2:
            roi = cv.GetImageROI(frame)
            width = roi[2]
            height = roi[3]
            self.left_pole = round(min(vertical_lines[0][0], vertical_lines[1][0]), 2) - width/2
            self.right_pole = round(max(vertical_lines[0][0], vertical_lines[1][0]), 2) - width/2
        #TODO: If one pole is seen, is it left or right pole?

        # Calculate planar distance r (assuming we are moving perpendicular to
        # the hedge)
        if self.left_pole and self.right_pole:
            theta = abs(self.left_pole - self.right_pole)
            self.r = 3 / tan(radians(theta/2))
        else:
            self.r = None

        if self.r and self.seen_crossbar:
            bar_phi = (-1*horizontal_lines[0][0] + frame.height/2) / (frame.height/2) * 32
            self.crossbar_depth = self.r * atan(radians(bar_phi))
        else:
            self.crossbar_depth = None


        if self.debug:
            cv.CvtColor(color_filtered, frame, cv.CV_GRAY2RGB)
            libvision.misc.draw_lines(frame, vertical_lines)
            libvision.misc.draw_lines(frame, horizontal_lines)

            #cv.ShowImage("Hedge", cv.CloneImage(frame))
            svr.debug("Hedge", cv.CloneImage(frame))

        #populate self.output with infos
        self.output.seen_crossbar = self.seen_crossbar
        self.output.left_pole = self.left_pole
        self.output.right_pole = self.right_pole
        self.output.r = self.r
        self.output.crossbar_depth = self.crossbar_depth

        self.return_output()
        print self
示例#36
0
文件: hedgeY.py 项目: tarora2/seawolf
    def process_frame(self, frame):
        self.numpy_frame = libvision.cv_to_cv2(frame)
        self.debug_frame = self.numpy_frame.copy()
        self.test_frame = self.numpy_frame.copy()

        self.numpy_frame = cv2.medianBlur(self.numpy_frame, 7)
        self.numpy_frame = cv2.cvtColor(self.numpy_frame, cv2.COLOR_BGR2HSV)

        (rf1, rf2, rf3) = cv2.split(self.numpy_frame)

        Rbinary = rf3
        Gbinary = rf1

        # Adaptive Threshold
        Rbinary = cv2.adaptiveThreshold(Rbinary, 255,
                                        cv2.ADAPTIVE_THRESH_MEAN_C,
                                        cv2.THRESH_BINARY_INV,
                                        self.adaptive_thresh_blocksize,
                                        self.adaptive_thresh)

        Gbinary = cv2.adaptiveThreshold(Gbinary, 255,
                                        cv2.ADAPTIVE_THRESH_MEAN_C,
                                        cv2.THRESH_BINARY_INV,
                                        self.Gadaptive_thresh_blocksize,
                                        self.Gadaptive_thresh)

        # Morphology
        kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (3, 3))

        Rbinary = cv2.erode(Rbinary, kernel)
        Rbinary = cv2.dilate(Rbinary, kernel)
        Gbinary = cv2.erode(Gbinary, kernel)
        Gbinary = cv2.dilate(Gbinary, kernel)

        Rframe = cv2.cvtColor(Rbinary, cv2.COLOR_GRAY2RGB)
        Gframe = cv2.cvtColor(Gbinary, cv2.COLOR_GRAY2RGB)

        # Hough Transform
        raw_linesG = cv2.HoughLines(Gbinary,
                                    rho=1,
                                    theta=math.pi / 180,
                                    threshold=self.hough_thresholdG)

        # Get vertical lines
        vertical_linesG = []

        if raw_linesG is None:
            raw_linesG = []

        if len(raw_linesG) > 0:
            for line in raw_linesG[0]:
                rho = line[0]
                theta = line[1]
                if theta < self.vertical_thresholdG or theta > (
                        math.pi - self.vertical_thresholdG):
                    vertical_linesG.append((rho, theta))

        # Group vertical lines
        vertical_line_groupsG = [
        ]  # A list of line groups which are each a line list
        for line in vertical_linesG:
            #print "Green Line Grouping Possibility:", line[0], ", ", line[1]
            group_found = False
            for line_group in vertical_line_groupsG:

                if line_group_accept_test(line_group, line, self.max_range):
                    line_group.append(line)
                    group_found = True

            if not group_found:
                vertical_line_groupsG.append([line])

        # Average line groups into lines
        vertical_linesG = []
        for line_group in vertical_line_groupsG:
            rhos = map(lambda line: line[0], line_group)
            angles = map(lambda line: line[1], line_group)
            line = (sum(rhos) / len(rhos), circular_average(angles, math.pi))
            vertical_linesG.append(line)

        # Get horizontal lines
        horizontal_lines = []
        if len(raw_linesG) > 0:
            for line in raw_linesG[0]:
                rho = line[0]
                theta = line[1]
                dist_from_horizontal = (math.pi / 2 + line[1]) % math.pi
                if dist_from_horizontal < self.horizontal_threshold or dist_from_horizontal > math.pi - self.horizontal_threshold:
                    horizontal_lines.append((abs(line[0]), line[1]))

        # Group horizontal lines
        horizontal_line_groups = [
        ]  # A list of line groups which are each a line list
        for line in horizontal_lines:
            group_found = False
            for line_group in horizontal_line_groups:

                if line_group_accept_test(line_group, line, self.max_range):
                    line_group.append(line)
                    group_found = True

            if not group_found:
                horizontal_line_groups.append([line])

        if len(horizontal_line_groups) is 1:
            self.seen_crossbar = True
            rhos = map(lambda line: line[0], horizontal_line_groups[0])
            angles = map(lambda line: line[1], horizontal_line_groups[0])
            line = (sum(rhos) / len(rhos), circular_average(angles, math.pi))
            horizontal_lines = [line]
        else:
            self.seen_crossbar = False
            horizontal_lines = []

        self.left_pole = None
        self.right_pole = None

        Rframe = libvision.cv2_to_cv(Rframe)
        Gframe = libvision.cv2_to_cv(self.debug_frame)
        Rbinary = libvision.cv2_to_cv(Rbinary)
        self.debug_frame = libvision.cv2_to_cv(self.debug_frame)
        self.test_frame = libvision.cv2_to_cv(self.test_frame)
        Gbinary = libvision.cv2_to_cv(Gbinary)

        if len(vertical_linesG) is 2:
            roi = cv.GetImageROI(frame)
            width = roi[2]
            height = roi[3]
            self.left_pole = round(
                min(vertical_linesG[0][0], vertical_linesG[1][0]),
                2) - width / 2
            self.right_pole = round(
                max(vertical_linesG[0][0], vertical_linesG[1][0]),
                2) - width / 2

        # TODO: If one pole is seen, is it left or right pole?

        # Calculate planar distance r (assuming we are moving perpendicular to
        # the hedge)
        if self.left_pole and self.right_pole:
            theta = abs(self.left_pole - self.right_pole)
            self.r = 3 / math.tan(math.radians(theta / 2))
        else:
            self.r = None

        if self.r and self.seen_crossbar:
            bar_phi = (-1 * horizontal_lines[0][0] +
                       Gframe.height / 2) / (Gframe.height / 2) * 32
            self.crossbar_depth = self.r * math.atan(math.radians(bar_phi))
        else:
            self.crossbar_depth = None

        # Line Finding on Red pvc
        # Hough Transform
        line_storage = cv.CreateMemStorage()
        raw_linesR = cv.HoughLines2(Rbinary,
                                    line_storage,
                                    cv.CV_HOUGH_STANDARD,
                                    rho=1,
                                    theta=math.pi / 180,
                                    threshold=self.hough_thresholdR,
                                    param1=0,
                                    param2=0)

        # Get vertical lines
        vertical_linesR = []
        for line in raw_linesR:
            if line[1] < self.vertical_thresholdR or \
               line[1] > math.pi - self.vertical_thresholdR:

                vertical_linesR.append((abs(line[0]), line[1]))

        # Group vertical lines
        vertical_line_groupsR = [
        ]  # A list of line groups which are each a line list
        for line in vertical_linesR:
            group_found = False
            for line_group in vertical_line_groupsR:

                if line_group_accept_test(line_group, line, self.max_range):
                    line_group.append(line)
                    group_found = True

            if not group_found:
                vertical_line_groupsR.append([line])

        # Average line groups into lines
        vertical_linesR = []
        for line_group in vertical_line_groupsR:
            rhos = map(lambda line: line[0], line_group)
            angles = map(lambda line: line[1], line_group)
            line = (sum(rhos) / len(rhos), circular_average(angles, math.pi))
            vertical_linesR.append(line)
        '''
        for red_line in vertical_linesR:
            print "Red Line:", red_line[0],", ",red_line[1]
        for green_line in vertical_linesG:
            print "Green Line:", green_line[0],", ",green_line[1]
        '''
        for red_line in vertical_linesR:
            for green_line in vertical_linesG[:]:
                if math.fabs(green_line[0] - red_line[0]) < self.GR_Threshold0 and \
                   math.fabs(green_line[1] - red_line[1]) < self.GR_Threshold1:
                    vertical_linesG.remove(green_line)

        for red_line in vertical_linesR:
            print "New Red Line:", red_line[0], ", ", red_line[1]
        for green_line in vertical_linesG:
            print "New Green VLine:", green_line[0], ", ", green_line[1]
        for green_line in horizontal_lines:
            print "New Green HLine:", green_line[0], ", ", green_line[1]

        if len(vertical_linesR) is 0:
            print "No Red Found"

        self.left_pole = None
        self.right_pole = None
        if len(vertical_linesR) is 2:
            roi = cv.GetImageROI(frame)
            width = roi[2]
            height = roi[3]
            self.left_pole = round(
                min(vertical_linesR[0][0], vertical_linesR[1][0]),
                2) - width / 2
            self.right_pole = round(
                max(vertical_linesR[0][0], vertical_linesR[1][0]),
                2) - width / 2
        # TODO: If one pole is seen, is it left or right pole?

        # Calculate planar distance r (assuming we are moving perpendicular to
        # the hedge)
        if self.left_pole and self.right_pole:
            theta = abs(self.left_pole - self.right_pole)
            self.r = 3 / math.tan(math.radians(theta / 2))
        else:
            self.r = None

        for i in range(len(vertical_linesR[:])):
            if vertical_linesR[i][1] > math.pi / 2:
                vertical_linesR[i] = (vertical_linesR[i][0],
                                      -(math.pi - vertical_linesR[i][1]))
                print "Line changed to ", vertical_linesR[i]
        for line in vertical_linesR:
            print line
            if line[1] > math.pi / 2:
                line = (line[0], math.pi - line[1])
                print "Line changed to ", line

        libvision.misc.draw_lines(Gframe, vertical_linesG)
        libvision.misc.draw_lines(Gframe, horizontal_lines)
        libvision.misc.draw_lines(Rframe, vertical_linesR)

        # there was a merge error, these 3 lines conflicted b/c your copy out of date

        for line in vertical_linesR:
            roi = cv.GetImageROI(frame)
            width = roi[2]
            height = roi[3]
            x = line[0] * math.cos(line[1])
            y = line[0] * math.sin(line[1])
            cv.Circle(Rframe, (int(x), int(y)), 5, (0, 255, 0), -1, 8, 0)
            if x > width or y > width or x < 0 or y < 0:
                print "Lost point  ", x

        svr.debug("Original", self.test_frame)
        svr.debug("Red", Rframe)
        svr.debug("Green", Gframe)
        svr.debug("Green Binary", Gbinary)
示例#37
0
    def process_frame(self, frame):
        # frame directors
        #self.debug_frame -- Frame containing helpful debug information

        # Debug numpy in CV2
        raw_frame        = libvision.cv_to_cv2(frame)
        self.debug_frame = raw_frame

        # CV2 blur
        blur_frame = cv2.medianBlur(self.debug_frame, 5)
        hsv_blur_frame = 


        # collect brightly colored areas
        frame1 = self.adaptive_threshold(blur_frame, 0,
                                self.adaptive_thresh_blocksize,
                                self.adaptive_thresh)

        # collect shadowes under colored areas
        frame2 = self.adaptive_threshold(blur_frame, 1,
                                self.shadow_thresh_blocksize,
                                self.shadow_thresh)
        
        # use composite as the adaptive threshold
        adaptive_frame = cv2.add(frame1, frame2*0)
        frame          = adaptive_frame
        
        #self.debug_stream("help", <frame>)
        

        
        # morphology
        sequence = ([-self.erode_factor, self.erode_factor]*1 
                   +[self.bloom_factor, -self.bloom_factor]*1)

        despeckled_frame = self.morphology(frame, sequence)
        frame            = despeckled_frame

        self.debug_stream("despeckled", despeckled_frame)

        # collect edges
        #a = 800
        # TODO: ROI_edge detection
        edge_frame = self.ROI_edge_detection(raw_frame, frame, True)
   

        #edge_frame = cv2.Canny(frame, 150, 250, apertureSize=3)
        
        
        # collect buoy candidates using hough circles
        self.raw_circles = []
        self.raw_buoys = []
        self.raw_circles = cv2.HoughCircles(
                                edge_frame, 
                                cv2.cv.CV_HOUGH_GRADIENT,
                                self.inv_res_ratio, 
                                self.center_sep,
                                np.array([]),
                                self.upper_canny_thresh,
                                self.acc_thresh,
                                self.min_radius,
                                self.max_radius,
                        )
  
        # create a new buoy object for every circle that is detected
        if self.raw_circles is not None and len(self.raw_circles[0] > 0):
            #print self.confirmed
            for circle in self.raw_circles[0]:
                (x, y, radius) = circle
                new_buoy = Buoy(x, y, radius, "unknown", self.next_id)
                self.next_id += 1
                self.raw_buoys.append(new_buoy) 
                self.match_buoys(new_buoy)

        # sort buoys among confirmed/canditates
        self.sort_buoys()
        
        # self.debug_frame= cv2.add(<HUD_FRAME>,cv2.cvtColor(<annotated_frame>, cv2.COLOR_GRAY2BGR) )
        # perform color detection
        if self.confirmed is not None and len(self.confirmed) > 0:
        
            # vvv start color detection 
            for buoy in self.confirmed:
                # draw a cirle around the confirmed bouy
                cv2.circle(self.debug_frame, (int(buoy.centerx), int(buoy.centery)),
                            int(buoy.radius) + 10, (255, 255, 255), 5)
                           
                # attain hue from a pixel on the buoy
                color_pick_point = ( int(buoy.centerx), int(buoy.centery - buoy.radius/2) )
                _c  = color_pick_point
                # ^^offset a couple pixels upward for some reason
                colorHue = np.mean(self.hsv_frame[_c[1]-buoy.radius/2 : _c[1]+buoy.radius/2, 
                                                  _c[0]-buoy.radius/2 : _c[0]+buoy.radius/2, 
                                                  0])
                
                if BUOY_COLOR_PRINTS:
                    print("buoy%d has a hue of %d" %(buoy.id,int(colorHue)))
                
                # note: color wraps around at 180. Range is 0->180
                if (colorHue >= 0 and colorHue < 45) or colorHue >= 95: # 105->180->45
                    cv2.putText(self.debug_frame,str(buoy.id)+"RED", (int(buoy.centerx), int(buoy.centery)), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255))
                    buoy.color = "red"
                elif (colorHue >= 80 and colorHue < 95): # green is hardest to detect
                    cv2.putText(self.debug_frame,str(buoy.id)+"GRE", (int(buoy.centerx), int(buoy.centery)), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255))
                    
                    #if buoy.color != "red" and buoy.color != "yellow":
                    #print "switched from ", buoy.color
                    buoy.color = "green"
                        
                else: #yellow is about 50->80
                    cv2.putText(self.debug_frame,str(buoy.id)+"YEL", (int(buoy.centerx), int(buoy.centery)), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255))
                    buoy.color = "yellow"
                
                cv2.putText(self.debug_frame,"HUE="+str(int(colorHue)), (int(buoy.centerx), int(buoy.centery-20)), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255))
           
            # ^^^ end color detection

        # debug frames
        self.debug_to_cv = libvision.cv2_to_cv(self.debug_frame)
        #self.numpy_to_cv = libvision.cv2_to_cv(self.numpy_frame)
        self.adaptive_to_cv = libvision.cv2_to_cv(adaptive_frame)
        #svr.debug("processed", self.numpy_to_cv)
        svr.debug("adaptive", self.adaptive_to_cv)
        svr.debug("debug", self.debug_to_cv)

        # generate vision output
        self.output.buoys = []
        if self.confirmed is not None and len(self.confirmed) > 0:
            for buoy in self.confirmed:
                buoy.theta = buoy.centerx #<- a rough approximation
                buoy.phi = buoy.centery   #<- a rough approximation
                buoy.id = buoy.id
                self.output.buoys.append(buoy)

        # publish output
        #print ("%d buoys currently confirmed." % len(self.confirmed))
        if self.output.buoys:
            self.return_output()
        return self.output
示例#38
0
    def process_frame(self, frame):
        numpy_frame = libvision.cv_to_cv2(frame)
        svr.debug("Original", frame)

        numpy_frame = cv2.medianBlur(numpy_frame, 7)
        debug_frame = numpy_frame.copy()
        numpy_frame = cv2.cvtColor(numpy_frame, cv2.COLOR_BGR2HSV)

        (h, s, v) = cv2.split(numpy_frame)

        binary = h

        binary = cv2.adaptiveThreshold(binary, 255,
                                       cv2.ADAPTIVE_THRESH_MEAN_C,
                                       cv2.THRESH_BINARY_INV,
                                       self.adaptive_thresh_blocksize,
                                       self.adaptive_thresh)

        # Morphology
        kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (3, 3))

        binary = cv2.dilate(binary, kernel)
        binary = cv2.dilate(binary, kernel)

        # Hough Transform
        raw_lines = cv2.HoughLinesP(binary,
                                    rho=1,
                                    theta=math.pi / 180,
                                    threshold=self.hough_threshold,
                                    minLineLength=50,
                                    maxLineGap=10)

        if raw_lines is None:
            raw_lines = []
        else:
            raw_lines = raw_lines[0]

        def slope(line):
            """ Determine the slope [in degrees] of a line """
            (x1, y1, x2, y2) = line

            p1 = (x1, y1)
            p2 = (x2, y2)

            leftx = min(x1, x2)

            if p1[0] == leftx:
                left = p1
                right = p2
            else:
                right = p1
                left = p2

            slope = (right[1]-left[1]) / (right[0]-left[0])

            return slope

        def angle(line):
            sl = slope(line)
            return math.degrees(math.atan2(sl, 1))

        def length(line):
            (x1, y1, x2, y2) = line
            return ((x2-x1)**2 + (y2-y1)**2) ** .5

        def center(line):
            """ Determine the center of a line """
            (x1, y1, x2, y2) = line

            p1 = (x1, y1)
            p2 = (x2, y2)

            leftx = min(x1, x2)

            if p1[0] == leftx:
                left = p1
                right = p2
            else:
                right = p1
                left = p2

            centerx = int(left[0] + length(line)/2*math.cos(math.atan2(slope(line), 1)))
            centery = int(left[1] + length(line)/2*math.sin(math.atan2(slope(line), 1)))

            return (centerx, centery)

        def is_vertical(line):
            return 60 <= abs(angle(line)) <= 90

        def is_horizontal(line):
            return 0 <= abs(angle(line)) <= 30

        def get_avg_endpoints(lines):
            lefts = []
            rights = []

            for line in lines:
                (x1, y1, x2, y2) = line

                p1 = (x1, y1)
                p2 = (x2, y2)

                leftx = min(x1, x2)

                if p1[0] == leftx:
                    left = p1
                    right = p2
                else:
                    right = p1
                    left = p2

                lefts.append(left)
                rights.append(right)

            return (average_pts(lefts), average_pts(rights))

        def get_med_endpoints(lines):
            lefts = []
            rights = []

            for line in lines:
                (x1, y1, x2, y2) = line

                p1 = (x1, y1)
                p2 = (x2, y2)

                leftx = min(x1, x2)

                if p1[0] == leftx:
                    left = p1
                    right = p2
                else:
                    right = p1
                    left = p2

                lefts.append(left)
                rights.append(right)

            return (bad_median(lefts, .25), bad_median(rights, .75))

        def average_pts(pts):
            num = len(pts)

            if num == 0:
                return None

            avg_x = sum(x for (x, y) in pts) / num
            avg_y = sum(y for (x, y) in pts) / num
            return (int(avg_x), int(avg_y))

        def median_pts(pts):
            num = len(pts)

            if num == 0:
                return None

            pts = sorted(pts, key=lambda x: x[0])
            return pts[num//2]

        def bad_median(pts, val=.5):
            num = len(pts)

            if num == 0:
                return None

            pts = sorted(pts, key=lambda x: x[0])
            return pts[int(num*val)]

        def get_normal_vec(line):
            sl = slope(line)
            return line

        h_lines = []
        v_lines = []

        for line in raw_lines:
            if is_horizontal(line):
                h_lines.append(line)
            elif is_vertical(line):
                v_lines.append(line)
            else:
                (x1, y1, x2, y2) = line
                cv2.line(debug_frame, (x1, y1), (x2, y2), (0, 255, 0), 2)

        if h_lines:
            self.seen_crossbar = False
            self.crossbar_depth = None

        for line in h_lines:
            (x1, y1, x2, y2) = line
            cv2.line(debug_frame, (x1, y1), (x2, y2), (0, 0, 255), 2)

        for line in v_lines:
            (x1, y1, x2, y2) = line
            cv2.line(debug_frame, (x1, y1), (x2, y2), (255, 0, 0), 2)

        h_centers = [center(line) for line in h_lines]
        v_centers = sorted([center(line) for line in v_lines], key=lambda x: x[0])

        h_avg_center = median_pts(h_centers)
        v_avg_center = average_pts(v_centers)

        if h_avg_center:
            cv2.circle(debug_frame, h_avg_center, 5, (0, 0, 0), -1)

        if v_avg_center:
            cv2.circle(debug_frame, v_avg_center, 5, (0, 0, 0), -1)

        split_pt = None

        for i in range(len(v_centers)):

            if i < len(v_centers)-1 and v_centers[i+1][0] - v_centers[i][0] > 40:
                split_pt = i+1
                break

        left_pole_center = None
        right_pole_center = None

        if split_pt:
            left_centers = v_centers[:split_pt]
            right_centers = v_centers[split_pt:]

            avg_left = average_pts(left_centers)
            avg_right = average_pts(right_centers)

            left_pole_center = avg_left
            right_pole_center = avg_right

        elif v_avg_center and h_avg_center and h_avg_center[0] - v_avg_center[0] > 60:
            left_pole_center = v_avg_center
            cv2.circle(debug_frame, v_avg_center, 5, (0, 0, 0), -1)

        elif v_avg_center and h_avg_center and h_avg_center[0] - v_avg_center[0] < -60:
            right_pole_center = v_avg_center
            cv2.circle(debug_frame, v_avg_center, 5, (0, 0, 0), -1)

        else:
            avg_endpoints = get_med_endpoints(h_lines)
            lefts = avg_endpoints[0]
            rights = avg_endpoints[1]

            if lefts:
                cv2.circle(debug_frame, lefts, 5, (0, 0, 0), -1)
                left_pole_center = (lefts[0], lefts[1] - 80)

            if rights:
                cv2.circle(debug_frame, rights, 5, (0, 0, 0), -1)
                right_pole_center = (rights[0], rights[1] - 80)

        if left_pole_center:
            self.left_pole = left_pole_center[0]
            cv2.circle(debug_frame, left_pole_center, 5, (0, 0, 0), -1)

        if right_pole_center:
            self.right_pole = right_pole_center[0]
            cv2.circle(debug_frame, right_pole_center, 5, (0, 0, 0), -1)

        # median_slope_h = np.median(list(slope(line) for line in h_lines))
        # average_slope_v = None if len(v_lines) == 0 else sum(slope(line) for line in v_lines) / len(v_lines)

        # center_horiz =

        # points = []

        # for x1, y1, x2, y2 in raw_lines:
        #     points.append((x1, y1))
        #     points.append((x2, y2))

        # if points:
        #     rect = cv2.minAreaRect(np.array(points))
        #     box = cv2.cv.BoxPoints(rect)
        #     box = np.int0(box)

        #     # test aspect ratio & area, create bin if matches
        #     (x, y), (w, h), theta = rect

        #     cv2.drawContours(debug_frame, [box], 0, (0, 0, 255), 2)

        binary = libvision.cv2_to_cv(binary)
        svr.debug("Binary", binary)

        debug_frame = libvision.cv2_to_cv(debug_frame)
        svr.debug("Debug", debug_frame)
示例#39
0
    def process_frame(self, frame):

        # Get Channels
        hsv = cv.CreateImage(cv.GetSize(frame), 8, 3)
        cv.CvtColor(frame, hsv, cv.CV_BGR2HSV)
        grey = libvision.misc.get_channel(hsv, 2)

        # load a haar classifier
        #hc = cv.Load("/home/seawolf/software/seawolf5/vision/output.xml")
        hc = cv.Load(os.path.join(os.path.dirname(os.path.dirname(__file__)), "buoy_cascade_4.xml"))
        #hc = cv.Load("/home/seawolf/software/seawolf5/vision/buoy_cascade_4.xml")

        # use classifier to detect buoys
        minsize = (int(self.minsize), int(self.minsize))
        maxsize = (int(self.maxsize), int(self.maxsize))
        buoys = cv.HaarDetectObjects(grey, hc, cv.CreateMemStorage(), min_size=minsize)

        # compute average buoy size and extract to a list
        avg_w = 0
        for (x, y, w, h), n in buoys:

            # create a buoy class for this new buoys
            new_buoy = self.new_buoy(x, y, w)

            # make note of size
            avg_w = avg_w + w

            # add this buoy to our list of new buoys
            self.new.append(new_buoy)

        # update search size based on sizes found this frame
        if buoys:
            avg_w = avg_w / len(buoys)
            self.minsize = int(avg_w * .7)
            self.maxsize = int(avg_w * 1.3)

            # determine color of new buoys (if possible)
            libvision.buoy_analyzer(frame, self.new)

        # sort these new buoys into appropriate tier
        self.sort_buoys()  # must be called every frame for upkeep

        #######  DEBUG #######
        if self.debug:

            # display confirmed buoys
            for confirmed in self.confirmed:
                x = confirmed.x
                y = confirmed.y
                w = confirmed.width

                # draw rectangles on frame
                cv.Rectangle(frame, (x, y), (x + w, y + w), confirmed.debug_color, thickness=6)
                cv.Rectangle(frame, (x, y), (x + w, y + w), COLORS[confirmed.color], thickness=-1)

            # show debug frame
            svr.debug("BuoyTest", frame)

        ####### END DEBUG #######

        self.output.buoys = self.confirmed
        for buoy in self.output.buoys:
            buoy.theta = (buoy.x - frame.width / 2) * 37 / (frame.width / 2)
            buoy.phi = -1 * (buoy.y - frame.height / 2) * 36 / (frame.height / 2)
        self.return_output()
示例#40
0
    def process_frame(self, frame):
        # This is equivalent to the old routine, but it isn't actually necessary
        #height, width, depth = libvision.cv_to_cv2(frame).shape
        #self.debug_frame = np.zeros((height, width, 3), np.uint8)

        # Debug numpy is CV2
        self.debug_frame = libvision.cv_to_cv2(frame)

        # CV2 Transforms
        self.numpy_frame = self.debug_frame.copy()
        self.numpy_frame = cv2.medianBlur(self.numpy_frame, 5)
        self.numpy_frame = cv2.cvtColor(self.numpy_frame, cv2.COLOR_BGR2HSV)

        (self.frame1, self.frame2, self.frame3) = cv2.split(self.numpy_frame)
        # Change the frame number to determine what channel to focus on
        self.numpy_frame = self.frame2

        # Thresholding
        self.numpy_frame = cv2.adaptiveThreshold(self.numpy_frame,
                              255,
                              cv2.ADAPTIVE_THRESH_MEAN_C,
                              cv2.THRESH_BINARY_INV,
                              self.adaptive_thresh_blocksize,
                              self.adaptive_thresh
                              )

        kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (3, 3))
        kernel2 = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (4, 4))
    #     #kernel = np.ones((2,2), np.uint8)
        self.numpy_frame = cv2.erode(self.numpy_frame, kernel)
        self.numpy_frame = cv2.dilate(self.numpy_frame, kernel2)

        self.adaptive_frame = self.numpy_frame.copy()

    #     # Find contours
        contours, hierarchy = cv2.findContours(self.numpy_frame,
                                               cv2.RETR_TREE, 
                                               cv2.CHAIN_APPROX_SIMPLE)
        self.raw_led = []
        self.raw_buoys = []

        if len(contours) > 1:
            cnt = contours[0]
            cv2.drawContours(self.numpy_frame, contours, -1, (255, 255, 255), 3)

            for h, cnt in enumerate(contours):
                approx = cv2.approxPolyDP(cnt, 0.01 * cv2.arcLength(cnt, True), True)

                center, radius = cv2.minEnclosingCircle(cnt)
                x, y = center

                if len(approx) > 10:
                    if (radius > 17):
                        new_buoy = Buoy(int(x), int(y), int(radius))
                        new_buoy.id = self.recent_id
                        self.recent_id += 1
                        self.raw_buoys.append(new_buoy)
                        cv2.drawContours(self.numpy_frame, [cnt], 0, (0, 0, 255), -1)

        for buoy in self.raw_buoys:
            self.match_buoys(buoy)

        self.sort_buoys()
        self.draw_buoys()

        self.return_output()

        self.debug_to_cv = libvision.cv2_to_cv(self.debug_frame)
        self.numpy_to_cv = libvision.cv2_to_cv(self.numpy_frame)
        self.adaptive_to_cv = libvision.cv2_to_cv(self.adaptive_frame)

        svr.debug("processed", self.numpy_to_cv)
        svr.debug("adaptive", self.adaptive_to_cv)
        svr.debug("debug", self.debug_to_cv)
示例#41
0
    def process_frame(self, frame):
        # This is equivalent to the old routine, but it isn't actually necessary
        #height, width, depth = libvision.cv_to_cv2(frame).shape
        #self.debug_frame = np.zeros((height, width, 3), np.uint8)

        inv_res_ratio = 2
        center_sep = 100
        upper_canny_thresh = 40  # 40
        acc_thresh = 10  # 20, 50 with green settings
        min_radius = 3
        max_radius = 50

        # Debug numpy is CV2
        debug_frame = libvision.cv_to_cv2(frame)

        svr.debug("original", frame)

        # CV2 Transforms
        numpy_frame = debug_frame.copy()
        numpy_frame = cv2.medianBlur(numpy_frame, 5)
        numpy_frame = cv2.cvtColor(numpy_frame, cv2.COLOR_BGR2HSV)

        # Kernel for erosion/dilation
        kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (3, 3))

        # Split into HSV frames
        (h_frame, s_frame, v_frame) = cv2.split(numpy_frame)

        # Run inverse adaptive thresh on saturation channel
        s_adapt_thresh = cv2.adaptiveThreshold(s_frame,
                                               255,
                                               cv2.ADAPTIVE_THRESH_MEAN_C,
                                               cv2.THRESH_BINARY_INV,
                                               47,
                                               10)

        # Erode and dilate the value frame
        s_eroded = cv2.erode(s_adapt_thresh, kernel)
        s_dilated = cv2.dilate(s_eroded, kernel)

        # Threshold the value frame
        _, v_thresh = cv2.threshold(v_frame, 250, 255, cv2.THRESH_BINARY)

        # Erode and dilate the value frame
        v_eroded = cv2.erode(v_thresh, kernel)
        v_dilated = cv2.dilate(v_eroded, kernel)

        s_contours = s_dilated.copy()

        # Find contours on the dilated saturation channel
        s_cnt, hy = cv2.findContours(
            s_dilated,
            cv2.RETR_EXTERNAL,
            cv2.CHAIN_APPROX_SIMPLE
        )

        v_contours = v_dilated.copy()

        # Find contours on the dilated
        v_cnt, hy = cv2.findContours(
            v_dilated,
            cv2.RETR_EXTERNAL,
            cv2.CHAIN_APPROX_SIMPLE
        )

        if len(s_contours) > 0:
            cv2.drawContours(s_contours, s_cnt, -1, (255, 255, 255), 3)

        if len(v_contours) > 0:
            cv2.drawContours(v_contours, v_cnt, -1, (255, 255, 255), 3)

        s_circles = cv2.HoughCircles(
            s_contours,
            cv2.cv.CV_HOUGH_GRADIENT,
            inv_res_ratio,
            center_sep,
            np.array([]),
            upper_canny_thresh,
            acc_thresh,
            min_radius,
            max_radius,
        )

        v_circles = cv2.HoughCircles(
            v_contours,
            cv2.cv.CV_HOUGH_GRADIENT,
            inv_res_ratio,
            center_sep,
            np.array([]),
            upper_canny_thresh,
            acc_thresh,
            min_radius,
            max_radius,
        )

        for circle in s_circles[0]:
            (x, y, radius) = circle
            cv2.circle(debug_frame, (int(x), int(y)), int(radius) + 10, (0, 255, 0), 5)

        # for circle in v_circles[0]:
        #     (x, y, radius) = circle
        #     cv2.circle(debug_frame, (int(x), int(y)), int(radius) + 10, (0, 0, 255), 5)


        # debug_to_cv = libvision.cv2_to_cv(v_circles)
        # svr.debug("v_frame", debug_to_cv)

        # debug_to_cv = libvision.cv2_to_cv(s_circles)
        # svr.debug("s_frame", debug_to_cv)

        debug_to_cv = libvision.cv2_to_cv(debug_frame)
        svr.debug("debug_frame", debug_to_cv)
示例#42
0
    def process_frame(self, frame):
        if self.path_manager.start_angle is None:
            self.path_manager.start_angle = get_yaw()

        self.output.found = False

        cv.Smooth(frame, frame, cv.CV_MEDIAN, 7, 7)

        # Use RGB color finder
        binary = libvision.cmodules.target_color_rgb.find_target_color_rgb(
            frame, 250, 125, 0, 1500, 500, .3)
        color_filtered = cv.CloneImage(binary)

        blob_map = cv.CloneImage(binary)
        blobs = libvision.blob.find_blobs(binary,
                                          blob_map,
                                          min_blob_size=50,
                                          max_blobs=10)

        if not blobs:
            return

        binary = cv.CloneImage(blob_map)
        mapping = [0] * 256
        for blob in blobs:
            mapping[blob.id] = 255
        libvision.greymap.greymap(blob_map, binary, mapping)

        # Get Edges
        cv.Canny(binary, binary, 30, 40)

        # Hough Transform
        line_storage = cv.CreateMemStorage()
        lines = cv.HoughLines2(binary,
                               line_storage,
                               cv.CV_HOUGH_STANDARD,
                               rho=1,
                               theta=math.pi / 180,
                               threshold=self.hough_threshold,
                               param1=0,
                               param2=0)
        lines = lines[:self.lines_to_consider]  # Limit number of lines

        if not lines:
            return

        paths = self.path_manager.process(lines, blobs)

        if paths and not self.path:
            # If path[1] is clockwise of paths[0]
            distance = circular_distance(paths[0].angle, paths[1].angle)

            print
            print "Distance: ", distance
            print paths[0].theta, paths[0].angle
            print paths[1].theta, paths[1].angle

            if distance > 0:
                self.path = paths[self.which_path]
            else:
                self.path = paths[1 - self.which_path]

            print self.path.angle, self.path.theta
            print

        if paths and self.path in paths and self.path.blobs:
            temp_map = cv.CloneImage(blob_map)

            mapping = [0] * 256
            for blob in self.path.blobs:
                mapping[blob.id] = 255
            libvision.greymap.greymap(blob_map, temp_map, mapping)
            center = self.find_centroid(temp_map)

            svr.debug("map", temp_map)

            self.path.center = (center[0] - (frame.width / 2),
                                -center[1] + (frame.height / 2))

            self.output.found = True
            self.output.theta = self.path.theta
            self.output.x = self.path.center[0] / (frame.width / 2)
            self.output.y = self.path.center[1] / (frame.height / 2)
            print self.output

        if self.debug:
            # Show color filtered
            color_filtered_rgb = cv.CreateImage(cv.GetSize(frame), 8, 3)
            cv.CvtColor(color_filtered, color_filtered_rgb, cv.CV_GRAY2RGB)
            cv.SubS(color_filtered_rgb, (255, 0, 0), color_filtered_rgb)
            cv.Sub(frame, color_filtered_rgb, frame)

            # Show edges
            binary_rgb = cv.CreateImage(cv.GetSize(frame), 8, 3)
            cv.CvtColor(binary, binary_rgb, cv.CV_GRAY2RGB)
            cv.Add(frame, binary_rgb, frame)  # Add white to edge pixels
            cv.SubS(binary_rgb, (0, 0, 255), binary_rgb)
            cv.Sub(frame, binary_rgb, frame)  # Remove all but Red

            theta = math.radians(
                circular_distance(self.path_manager.start_angle, get_yaw()))
            if theta < 0:
                scale = math.cos(-2 * theta)
                theta = pi + theta
                libvision.misc.draw_lines(
                    frame, [((-frame.width / 2) * scale, theta)])
            else:
                libvision.misc.draw_lines(frame, [(frame.width / 2, theta)])

            # Show lines
            if self.output.found:
                rounded_center = (
                    int(round(center[0])),
                    int(round(center[1])),
                )
                cv.Circle(frame, rounded_center, 5, (0, 255, 0))
                libvision.misc.draw_lines(frame,
                                          [(frame.width / 2, self.path.theta)])

            else:
                libvision.misc.draw_lines(frame, lines)

            svr.debug("Path", frame)

        self.return_output()
示例#43
0
    def process_frame(self, frame):
        self.debug_frame = cv.CreateImage(cv.GetSize(frame), 8, 3)
        self.test_frame = cv.CreateImage(cv.GetSize(frame), 8, 3)

        cv.Copy(frame, self.debug_frame)
        cv.Copy(frame, self.test_frame)

        cv.Smooth(frame, frame, cv.CV_MEDIAN, 7, 7)

        # Set binary image to have saturation channel
        hsv = cv.CreateImage(cv.GetSize(frame), 8, 3)
        binary = cv.CreateImage(cv.GetSize(frame), 8, 1)
        cv.CvtColor(frame, hsv, cv.CV_BGR2HSV)
        cv.SetImageCOI(hsv, 1)
        cv.Copy(hsv, binary)
        cv.SetImageCOI(hsv, 0)

        # Adaptive Threshold
        cv.AdaptiveThreshold(
            binary,
            binary,
            255,
            cv.CV_ADAPTIVE_THRESH_MEAN_C,
            cv.CV_THRESH_BINARY_INV,
            self.adaptive_thresh_blocksize,
            self.adaptive_thresh,
        )

        # Morphology
        kernel = cv.CreateStructuringElementEx(5, 5, 3, 3, cv.CV_SHAPE_ELLIPSE)
        cv.Erode(binary, binary, kernel, 1)
        cv.Dilate(binary, binary, kernel, 1)

        cv.CvtColor(binary, self.debug_frame, cv.CV_GRAY2RGB)

        # Find Corners
        temp1 = cv.CreateImage(cv.GetSize(frame), 8, 1)
        temp2 = cv.CreateImage(cv.GetSize(frame), 8, 1)
        self.corners = cv.GoodFeaturesToTrack(
            binary,
            temp1,
            temp2,
            self.max_corners,
            self.quality_level,
            self.min_distance,
            None,
            self.good_features_blocksize,
            0,
            0.4,
        )

        # Display Corners
        for corner in self.corners:
            corner_color = (0, 0, 255)
            text_color = (0, 255, 0)
            font = cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, 0.6, 0.6, 0, 1, 1)
            cv.Circle(self.debug_frame, (int(corner[0]), int(corner[1])), 15, corner_color, 2, 8, 0)

        # Find Candidates
        for confirmed in self.confirmed:
            confirmed.corner1_repl_check = 0
            confirmed.corner2_repl_check = 0
            confirmed.corner3_repl_check = 0
            confirmed.corner4_repl_check = 0
            for corner in self.corners:
                if (
                    math.fabs(confirmed.corner1[0] - corner[0]) < self.MaxCornerTrans
                    and math.fabs(confirmed.corner1[1] - corner[1]) < self.MaxCornerTrans
                ):
                    confirmed.corner1_repl_check = 1
                    confirmed.corner1_repl = corner
                elif (
                    math.fabs(confirmed.corner2[0] - corner[0]) < self.MaxCornerTrans
                    and math.fabs(confirmed.corner2[1] - corner[1]) < self.MaxCornerTrans
                ):
                    confirmed.corner2_repl_check = 1
                    confirmed.corner2_repl = corner
                elif (
                    math.fabs(confirmed.corner3[0] - corner[0]) < self.MaxCornerTrans
                    and math.fabs(confirmed.corner3[1] - corner[1]) < self.MaxCornerTrans
                ):
                    confirmed.corner3_repl_check = 1
                    confirmed.corner3_repl = corner
                elif (
                    math.fabs(confirmed.corner4[0] - corner[0]) < self.MaxCornerTrans
                    and math.fabs(confirmed.corner4[1] - corner[1]) < self.MaxCornerTrans
                ):
                    confirmed.corner4_repl_check = 1
                    confirmed.corner4_repl = corner
            if (
                confirmed.corner4_repl_check == 1
                and confirmed.corner3_repl_check == 1
                and confirmed.corner2_repl_check == 1
                and confirmed.corner1_repl_check == 1
            ):
                confirmed.corner1 = confirmed.corner1_repl
                confirmed.corner2 = confirmed.corner2_repl
                confirmed.corner3 = confirmed.corner3_repl
                confirmed.corner4 = confirmed.corner4_repl

                confirmed.midx = rect_midpointx(
                    confirmed.corner1, confirmed.corner2, confirmed.corner3, confirmed.corner4
                )
                confirmed.midy = rect_midpointy(
                    confirmed.corner1, confirmed.corner2, confirmed.corner3, confirmed.corner4
                )

                if confirmed.last_seen < self.last_seen_max:
                    confirmed.last_seen += 5

        for corner1 in self.corners:
            for corner2 in self.corners:
                for corner3 in self.corners:
                    for corner4 in self.corners:
                        # Checks that corners are not the same and are in the proper orientation
                        if (
                            corner4[0] != corner3[0]
                            and corner4[0] != corner2[0]
                            and corner4[0] != corner1[0]
                            and corner3[0] != corner2[0]
                            and corner3[0] != corner1[0]
                            and corner2[0] != corner1[0]
                            and corner4[1] != corner3[1]
                            and corner4[1] != corner2[1]
                            and corner4[1] != corner1[1]
                            and corner3[1] != corner2[1]
                            and corner3[1] != corner1[1]
                            and corner2[1] != corner1[1]
                            and corner2[0] >= corner3[0]
                            and corner1[1] >= corner4[1]
                            and corner2[0] >= corner1[0]
                        ):
                            # Checks that the side ratios are correct
                            if (
                                math.fabs(line_distance(corner1, corner3) - line_distance(corner2, corner4))
                                < self.size_threshold
                                and math.fabs(line_distance(corner1, corner2) - line_distance(corner3, corner4))
                                < self.size_threshold
                                and math.fabs(line_distance(corner1, corner3) / line_distance(corner1, corner2))
                                < self.ratio_threshold
                                or math.fabs(line_distance(corner1, corner2) / line_distance(corner1, corner3))
                                < self.ratio_threshold
                            ):
                                # Checks that angles are roughly 90 degrees
                                angle_cnr_2 = math.fabs(
                                    angle_between_lines(line_slope(corner1, corner2), line_slope(corner2, corner4))
                                )
                                if self.angle_min < angle_cnr_2 < self.angle_max:
                                    angle_cnr_3 = math.fabs(
                                        angle_between_lines(line_slope(corner1, corner3), line_slope(corner3, corner4))
                                    )
                                    if self.angle_min2 < angle_cnr_3 < self.angle_max2:
                                        new_bin = Bin(corner1, corner2, corner3, corner4)
                                        self.match_bins(new_bin)
        self.sort_bins()

        """
        #START SHAPE PROCESSING

        #TODO load these ONCE somewhere
        samples = np.loadtxt('generalsamples.data',np.float32)
        responses = np.loadtxt('generalresponses.data',np.float32)
        responses = responses.reshape((responses.size,1))
        model = cv2.KNearest()
        model.train(samples,responses)

        for bin in self.confirmed:
                try:
                        bin.speedlimit
                except:
                        continue
                transf = cv.CreateMat(3, 3, cv.CV_32FC1)
                corner_orders = [
                        [bin.corner1, bin.corner2, bin.corner3, bin.corner4], #0 degrees
                        [bin.corner4, bin.corner3, bin.corner2, bin.corner1], #180 degrees
                        [bin.corner2, bin.corner4, bin.corner1, bin.corner3], #90 degrees
                        [bin.corner3, bin.corner1, bin.corner4, bin.corner2], #270 degrees
                        [bin.corner3, bin.corner4, bin.corner1, bin.corner2], #0 degrees and flipped X
                        [bin.corner2, bin.corner1, bin.corner4, bin.corner3], #180 degrees and flipped X
                        [bin.corner1, bin.corner3, bin.corner2, bin.corner4], #90 degrees and flipped X
                        [bin.corner4, bin.corner2, bin.corner3, bin.corner1]] #270 degrees andf flipped X
                for i in range(0, 8):
                        cv.GetPerspectiveTransform(
                                corner_orders[i],
                                [(0, 0), (0, 256), (128, 0), (128, 256)],
                                transf
                        )
                        shape = cv.CreateImage([128, 256], 8, 3)
                        cv.WarpPerspective(frame, shape, transf)

                        shape_thresh = np.zeros((256-104,128,1), np.uint8)
                        j = 104
                        while j<256:
                            i = 0
                            while i<128:
                                    pixel = cv.Get2D(shape, j, i)
                                if int(pixel[2]) > (int(pixel[1]) + int(pixel[0])) * 0.7:
                                    shape_thresh[j-104,i] = 255
                                else:
                                    shape_thresh[j-104,i] = 0
                                i = i+1
                            j = j+1
                        cv2.imshow("Bin " + str(i), shape_thresh)
                        contours,hierarchy = cv2.findContours(thresh,cv2.RETR_LIST,cv2.CHAIN_APPROX_NONE)
                        for cnt in contours:
                                    if cv2.contourArea(cnt)>50:
                                        [x,y,w,h] = cv2.boundingRect(cnt)
                                        if  h>54 and w>36:
                                                    roi = thresh[y:y+h,x:x+w]
                                                    roismall = cv2.resize(roi,(10,10))
                                                    roismall = roismall.reshape((1,100))
                                                    roismall = np.float32(roismall)
                                                    retval, results, neigh_resp, dists = model.find_nearest(roismall, k = 1)
                                                    digit_tuples.append( (x, int((results[0][0]))) )

                            if len(digit_tuples) == 2:
                                    digit_tuples_sorted = sorted(digit_tuples, key=lambda digit_tuple: digit_tuple[0])
                                speedlimit = 0
                                for i in range(0, len(digit_tuples_sorted)):
                                            speedlimit = speedlimit * 10 + digit_tuples_sorted[i][1]
                                    bin.speedlimit = speedlimit
                                    print "Found speed limit: " + str(speedlimit)
                                    break
                            else:
                                    print "Unable to determine speed limit"

        #... TODO more
        #END SHAPE PROCESSING
        """

        svr.debug("Bins", self.debug_frame)
        svr.debug("Bins2", self.test_frame)

        # Output bins
        self.output.bins = self.confirmed
        anglesum = 0
        for bins in self.output.bins:
            bins.theta = (bins.midx - frame.width / 2) * 37 / (frame.width / 2)
            bins.phi = -1 * (bins.midy - frame.height / 2) * 36 / (frame.height / 2)
            bins.shape = bins.object
            anglesum += bins.angle
        # bins.orientation = bins.angle
        if len(self.output.bins) > 0:
            self.output.orientation = anglesum / len(self.output.bins)
        else:
            self.output.orientation = None
        self.return_output()
示例#44
0
    def process_frame(self, frame):
        self.debug_frame = cv.CreateImage(cv.GetSize(frame), 8, 3)
        og_frame = cv.CreateImage(cv.GetSize(frame), 8, 3)
        cv.Copy(frame, self.debug_frame)
        cv.Copy(self.debug_frame, og_frame)

        cv.Smooth(frame, frame, cv.CV_MEDIAN, 7, 7)

        # Set binary image to have saturation channel
        hsv = cv.CreateImage(cv.GetSize(frame), 8, 3)
        binary = cv.CreateImage(cv.GetSize(frame), 8, 1)
        cv.CvtColor(frame, hsv, cv.CV_BGR2HSV)
        cv.SetImageCOI(hsv, 3)
        cv.Copy(hsv, binary)
        cv.SetImageCOI(hsv, 0)

        cv.AdaptiveThreshold(binary, binary,
                             255,
                             cv.CV_ADAPTIVE_THRESH_MEAN_C,
                             cv.CV_THRESH_BINARY_INV,
                             self.adaptive_thresh_blocksize,
                             self.adaptive_thresh,
        )

        # Morphology
        kernel = cv.CreateStructuringElementEx(5, 5, 3, 3, cv.CV_SHAPE_ELLIPSE)
        cv.Erode(binary, binary, kernel, 1)
        cv.Dilate(binary, binary, kernel, 1)

        # Get Edges
        #cv.Canny(binary, binary, 30, 40)

        cv.CvtColor(binary, self.debug_frame, cv.CV_GRAY2RGB)

        # Hough Transform
        line_storage = cv.CreateMemStorage()
        raw_lines = cv.HoughLines2(binary, line_storage,
                                   cv.CV_HOUGH_PROBABILISTIC,
                                   rho=1,
                                   theta=math.pi / 180,
                                   threshold=self.hough_threshold,
                                   param1=self.min_length,
                                   param2=self.max_gap
        )

        lines = []
        corners = []

        for line in raw_lines:
            lines.append(line)

        #Grouping lines depending on endpoint simularities

        for line1 in lines[:]:
            for line2 in lines[:]:
                if line1 in lines and line2 in lines and line1 != line2:
                    if (math.fabs(line1[0][0] - line2[0][0]) < self.max_corner_range and
                        math.fabs(line1[0][1] - line2[0][1]) < self.max_corner_range and
                        math.fabs(line1[1][0] - line2[1][0]) < self.max_corner_range and
                        math.fabs(line1[1][1] - line2[1][1]) < self.max_corner_range):
                        if line_distance(line1[0], line1[1]) > line_distance(line2[0], line2[1]):
                            lines.remove(line2)
                        else:
                            lines.remove(line1)
                    elif (math.fabs(line1[0][0] - line2[1][0]) < self.max_corner_range and
                          math.fabs(line1[0][1] - line2[1][1]) < self.max_corner_range and
                          math.fabs(line1[1][0] - line2[0][0]) < self.max_corner_range and
                          math.fabs(line1[1][1] - line2[0][1]) < self.max_corner_range):
                        if line_distance(line1[0], line1[1]) > line_distance(line2[0], line2[1]):
                            lines.remove(line2)
                        else:
                            lines.remove(line1)

        for line in lines:
            corners.append(line[0])
            corners.append(line[1])

        for corner1 in corners:
            for corner2 in corners:
                for corner3 in corners:
                    for corner4 in corners:
                        #Checks that corners are not the same and are in the proper orientation
                        if corner4[0] != corner3[0] and corner4[0] != corner2[0] and corner4[0] != corner1[0] and \
                           corner3[0] != corner2[0] and corner3[0] != corner1[0] and corner2[0] != corner1[0] and \
                           corner4[1] != corner3[1] and corner4[1] != corner2[1] and corner4[1] != corner1[1] and \
                           corner3[1] != corner2[1] and corner3[1] != corner1[1] and corner2[1] != corner1[1] and \
                           corner2[0] >= corner3[0] and corner1[1] >= corner4[1] and corner2[0] >= corner1[0] and \
                           math.fabs(corner1[0] - corner4[0]) > self.min_corner_distance and \
                           math.fabs(corner1[1] - corner4[1]) > self.min_corner_distance and \
                           math.fabs(corner2[0] - corner3[0]) > self.min_corner_distance and \
                           math.fabs(corner2[1] - corner3[1]) > self.min_corner_distance:
                            #Checks that the side ratios are correct
                            if math.fabs(line_distance(corner1, corner3) - line_distance(corner2, corner4)) < self.size_threshold and \
                               math.fabs(line_distance(corner1, corner2) - line_distance(corner3, corner4)) < self.size_threshold and \
                               math.fabs(line_distance(corner1, corner3) / line_distance(corner1, corner2)) < .5 * self.ratio_threshold:
                            #^^^ CHANGED OR TO AND --> DID MUCH BETTER. CONSIDER CHANGING ON BINSCORNER

                                #Checks that angles are roughly 90 degrees
                                angle_cnr_2 = math.fabs(angle_between_lines(line_slope(corner1, corner2), line_slope(corner2, corner4)))
                                if self.angle_min < angle_cnr_2 < self.angle_max:
                                    angle_cnr_3 = math.fabs(angle_between_lines(line_slope(corner1, corner3), line_slope(corner3, corner4)))
                                    if self.angle_min2 < angle_cnr_3 < self.angle_max2:
                                        new_box = Pizza(corner1, corner2, corner3, corner4)
                                        self.match_Boxes(new_box)

        self.min_perimeter = 500000
        for Box in self.Boxes[:]:
            Box.lastseen -= 2
            if Box.lastseen < 0:
                self.Boxes.remove(Box)
                if math.fabs(line_distance(Box.corner1, Box.corner3) * 2 + math.fabs(line_distance(Box.corner1, Box.corner2) * 2) - self.min_perimeter) > self.min_perimeter * self.perimeter_threshold and \
                line_distance(Box.corner1, Box.corner3) * 2 + line_distance(Box.corner1, Box.corner2) * 2 > self.min_perimeter:
                    print "perimeter error (this is a good thing)"
                    print math.fabs(line_distance(Box.corner1, Box.corner3) * 2 + math.fabs(line_distance(Box.corner1, Box.corner2) * 2) - self.min_perimeter), "is greater than", self.min_perimeter * self.perimeter_threshold

        self.draw_pizza()

        for line in lines:
            cv.Circle(self.debug_frame, line[0], 15, (255, 0, 0), 2, 8, 0)
            cv.Circle(self.debug_frame, line[1], 15, (255, 0, 0), 2, 8, 0)

        self.output.pizza = self.Boxes
        anglesum = 0
        for Box in self.Boxes:
            Box.theta = (Box.center[0] - frame.width / 2) * 37 / (frame.width / 2)
            Box.phi = -1 * (Box.center[1] - frame.height / 2) * 36 / (frame.height / 2)
            anglesum += Box.angle
        if len(self.output.pizza) > 0:
            self.output.orientation = anglesum / len(self.output.pizza)
        else:
            self.output.orientation = None
        self.return_output()

        svr.debug("Pizza", self.debug_frame)
        svr.debug("Original", og_frame)
示例#45
0
    def process_frame(self, frame):
        (w,h) = cv.GetSize(frame)

        #generate hue selection frames


        #create locations for the a pair of test frames
        frametest = cv.CreateImage(cv.GetSize(frame), 8, 3)
        binarytest = cv.CreateImage(cv.GetSize(frame), 8, 1)
        
        #use the red channel for the binary frame (just for debugging purposes)
        cv.Copy(frame, frametest)
        cv.SetImageCOI(frametest, 3)
        cv.Copy(frametest, binarytest)
        cv.SetImageCOI(frametest, 0)    #reset COI
        #svr.debug("R?",binarytest)

        # Resize image to 320x240
        #copy = cv.CreateImage(cv.GetSize(frame), 8, 3)
        #cv.Copy(frame, copy)
        #cv.SetImageROI(frame, (0, 0, 320, 240))
        #cv.Resize(copy, frame, cv.CV_INTER_NN)

        found_gate = False

        #create a new frame just for comparison purposes
        unchanged_frame = cv.CreateImage(cv.GetSize(frame), 8, 3)
        cv.Copy(frame,unchanged_frame)

        #apply a course noise filter
        cv.Smooth(frame, frame, cv.CV_MEDIAN, 7, 7)

        # Set binary image to have saturation channel
        hsv = cv.CreateImage(cv.GetSize(frame), 8, 3)
        binary = cv.CreateImage(cv.GetSize(frame), 8, 1)
        cv.CvtColor(frame, hsv, cv.CV_BGR2HSV)
        cv.SetImageCOI(hsv, 1)
        cv.Copy(hsv, binary)
        cv.SetImageCOI(hsv, 0)  #reset COI
        
        #shift hue of image such that orange->red are at top of spectrum
        binary = libvision.misc.cv_to_cv2(binary)
        binary = libvision.misc.shift_hueCV2(binary, self.target_shift)
        binary = libvision.misc.cv2_to_cv(binary)

        #correct for wraparound on red spectrum
        #cv.InRange(binary,a_array,b_array,binarytest) #generate mask
        #cv.Add(binary,cv.fromarray(ones*180),binary,mask=binarytest) #use mask to selectively add values
        #svr.debug("R2?",binary)
        svr.debug("R2?",binary)

        #run adaptive threshold for edge detection and more noise filtering
        cv.AdaptiveThreshold(binary, binary,
            255,
            cv.CV_ADAPTIVE_THRESH_MEAN_C,
            cv.CV_THRESH_BINARY_INV,
            self.adaptive_thresh_blocksize,
            self.adaptive_thresh,
        )

        # Morphology
        kernel = cv.CreateStructuringElementEx(5, 5, 3, 3, cv.CV_SHAPE_ELLIPSE)
        cv.Erode(binary, binary, kernel, 1)
        cv.Dilate(binary, binary, kernel, 1)
        if self.debug:
            color_filtered = cv.CloneImage(binary)

        # Get Edges
        cv.Canny(binary, binary, 30, 40)

        # Hough Transform
        line_storage = cv.CreateMemStorage()
        raw_lines = cv.HoughLines2(binary, line_storage, cv.CV_HOUGH_STANDARD,
                                   rho=1,
                                   theta=math.pi/180,
                                   threshold=self.hough_threshold,
                                   param1=0,
                                   param2=0
                                   )

        # Get vertical lines
        vertical_lines = []
        for line in raw_lines:
            if line[1] < self.vertical_threshold or \
               line[1] > math.pi-self.vertical_threshold:

                #absolute value does better grouping currently
                vertical_lines.append((abs(line[0]), line[1]))

        #print message to user for performance purposes
        logging.debug("{} possibilities reduced to {} lines".format(
                        len(raw_lines), len(vertical_lines) ))

        # Group vertical lines
        vertical_line_groups = []  # A list of line groups which are each a line list
        i = 0
        for line in vertical_lines:
            group_found = False
            for line_group in vertical_line_groups:
                i += 1
                if line_group_accept_test(line_group, line, self.max_range):
                    line_group.append(line)
                    group_found = True

            if not group_found:
                vertical_line_groups.append([line])

        #quick debugging statement
        logging.debug("{} internal iterations for {} groups".format(i, len(vertical_line_groups)))

        # Average line groups into lines
        vertical_lines = []
        for line_group in vertical_line_groups:
            rhos = map(lambda line: line[0], line_group)
            angles = map(lambda line: line[1], line_group)
            line = (sum(rhos)/len(rhos), circular_average(angles, math.pi))
            vertical_lines.append(line)

        ####################################################
        #vvvv Horizontal line code isn't used for anything

        # Get horizontal lines
        horizontal_lines = []
        for line in raw_lines:
            dist_from_horizontal = (math.pi/2 + line[1]) % math.pi
            if dist_from_horizontal < self.horizontal_threshold or \
               dist_from_horizontal > math.pi-self.horizontal_threshold:

                horizontal_lines.append( (abs(line[0]), line[1]) )

        # Group horizontal lines
        horizontal_line_groups = []  # A list of line groups which are each a line list
        for line in horizontal_lines:
            group_found = False
            for line_group in horizontal_line_groups:

                if line_group_accept_test(line_group, line, self.max_range):
                    line_group.append(line)
                    group_found = True

            if not group_found:
                horizontal_line_groups.append([line])

        if len(horizontal_line_groups) is 1:
            self.seen_crossbar = True
            if self.debug:
                rhos = map(lambda line: line[0], horizontal_line_groups[0])
                angles = map(lambda line: line[1], horizontal_line_groups[0])
                line = (sum(rhos)/len(rhos), circular_average(angles, math.pi))
                horizontal_lines = [line]
        else:
            self.seen_crossbar = False
            horizontal_lines = []

        #^^^ Horizontal line code isn't used for anything
        ###################################################

        self.left_pole = None
        self.right_pole = None
        #print vertical_lines
        self.returning = 0
        self.found = False

        if len(vertical_lines) is 2:
            roi = cv.GetImageROI(frame)
            width = roi[2]
            height = roi[3]
            self.left_pole = round(min(vertical_lines[0][0], vertical_lines[1][0]), 2) - width/2
            self.right_pole = round(max(vertical_lines[0][0], vertical_lines[1][0]), 2) - width/2

            self.returning = (self.left_pole + self.right_pole)/2
            logging.info("Returning {} as gate center delta.".format(self.returning))

            #initialize first iteration with 2 known poles
            if self.last_seen < 0:
                self.last_center = None
                self.last_seen = 0

            #increment a counter if result is good.
            if self.last_center is None:
                self.last_center = self.returning
                self.seen_count = 1
            elif math.fabs(self.last_center - self.returning) < self.center_trans_thresh:
                self.seen_count += 1
                self.last_seen += 2
            else:
                self.last_seen -= 1

            #if not conviced, forget left/right pole. Else proclaim success.
            if self.seen_count < self.seen_count_thresh:
                self.left_pole = None
                self.right_pole = None
            else: 
                print "FOUND CENTER AND RETURNED IT"
                self.found = True
        else:
            self.returning = 0
            if self.last_seen < 0:
                self.last_center = None
                self.last_seen = 0
            self.last_seen -= 1
            self.left_pole = None
            self.right_pole = None

            
            
            

        #TODO: If one pole is seen, is it left or right pole?
    
        if self.debug:
            cv.CvtColor(color_filtered, frame, cv.CV_GRAY2RGB)
            libvision.misc.draw_lines(frame, vertical_lines)
            libvision.misc.draw_lines(frame, horizontal_lines)

            if self.found:
                cv.Circle(frame, (int(frame.width/2 + self.returning), int(frame.height/2)),
                       15, (0, 255,0), 2, 8, 0)
                font = cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, 1, 1, 1, 3)
                cv.PutText(frame, "Gate Sent to Mission Control", (100, 400) , font, (255, 255, 0))
                #print frame.width

            #cv.ShowImage("Gate", cv.CloneImage(frame))
            svr.debug("Gate", cv.CloneImage(frame))
            svr.debug("Unchanged",cv.CloneImage(unchanged_frame))


        #populate self.output with infos
        self.output.seen_crossbar = self.seen_crossbar
        self.output.left_pole = self.left_pole
        self.output.right_pole = self.right_pole



        self.return_output()
示例#46
0
    def process_frame(self, frame):

        # Debug numpy is CV2
        self.debug_frame = libvision.cv_to_cv2(frame)

        # CV2 Transforms
        self.numpy_frame = self.debug_frame.copy()
        self.numpy_frame = cv2.medianBlur(self.numpy_frame, 5)
        self.numpy_frame = cv2.cvtColor(self.numpy_frame, cv2.COLOR_BGR2HSV)

        self.hsv_frame = self.numpy_frame

        (self.frame1, self.frame2, self.frame3) = cv2.split(self.numpy_frame)
        # Change the frame number to determine what channel to focus on
        self.numpy_frame = self.frame2

        # Thresholding
        self.numpy_frame = cv2.adaptiveThreshold(
            self.numpy_frame, 255, cv2.ADAPTIVE_THRESH_MEAN_C,
            cv2.THRESH_BINARY_INV, self.adaptive_thresh_blocksize,
            self.adaptive_thresh)

        kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (3, 3))
        kernel2 = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (4, 4))

        self.numpy_frame = cv2.erode(self.numpy_frame, kernel)
        self.numpy_frame = cv2.dilate(self.numpy_frame, kernel2)

        self.adaptive_frame = self.numpy_frame.copy()

        self.numpy_frame = cv2.Canny(self.adaptive_frame,
                                     100,
                                     250,
                                     apertureSize=3)

        self.raw_circles = []
        self.raw_buoys = []
        self.raw_circles = cv2.HoughCircles(
            self.numpy_frame,
            cv2.cv.CV_HOUGH_GRADIENT,
            self.inv_res_ratio,
            self.center_sep,
            np.array([]),
            self.upper_canny_thresh,
            self.acc_thresh,
            self.min_radius,
            self.max_radius,
        )

        if self.raw_circles is not None and len(self.raw_circles[0] > 0):
            for circle in self.raw_circles[0]:
                (x, y, radius) = circle
                new_buoy = Buoy(x, y, radius, "unknown", self.next_id)
                self.next_id += 1
                self.raw_buoys.append(new_buoy)
                self.match_buoys(new_buoy)

        self.sort_buoys()

        if self.confirmed is not None and len(self.confirmed) > 0:
            for buoy in self.confirmed:
                cv2.circle(self.debug_frame,
                           (int(buoy.centerx), int(buoy.centery)),
                           int(buoy.radius) + 10, (255, 255, 255), 5)
                colorHue = self.hsv_frame[buoy.centery + buoy.radius / 2,
                                          buoy.centerx][0]
                if (colorHue >= 0 and colorHue < 45) or colorHue >= 300:
                    cv2.putText(self.debug_frame,
                                str(buoy.id) + "RED",
                                (int(buoy.centerx), int(buoy.centery)),
                                cv2.FONT_HERSHEY_SIMPLEX, 2, (0, 0, 255))
                    buoy.color = "red"
                elif (colorHue >= 70 and colorHue < 180):
                    cv2.putText(self.debug_frame,
                                str(buoy.id) + "GRE",
                                (int(buoy.centerx), int(buoy.centery)),
                                cv2.FONT_HERSHEY_SIMPLEX, 2, (0, 0, 255))
                    if buoy.color != "red" and buoy.color != "yellow":
                        print "switched from ", buoy.color
                        buoy.color = "green"
                else:
                    cv2.putText(self.debug_frame,
                                str(buoy.id) + "YEL",
                                (int(buoy.centerx), int(buoy.centery)),
                                cv2.FONT_HERSHEY_SIMPLEX, 2, (0, 0, 255))
                    buoy.color = "yellow"

        self.debug_to_cv = libvision.cv2_to_cv(self.debug_frame)
        self.numpy_to_cv = libvision.cv2_to_cv(self.numpy_frame)
        self.adaptive_to_cv = libvision.cv2_to_cv(self.adaptive_frame)

        svr.debug("processed", self.numpy_to_cv)
        svr.debug("adaptive", self.adaptive_to_cv)
        svr.debug("debug", self.debug_to_cv)

        # Convert to output format
        self.output.buoys = []
        if self.confirmed is not None and len(self.confirmed) > 0:
            for buoy in self.confirmed:
                buoy.theta = buoy.centerx
                buoy.phi = buoy.centery
                buoy.id = buoy.id
                self.output.buoys.append(buoy)

        if self.output.buoys:
            self.return_output()
        return self.output
示例#47
0
    def process_frame(self, frame):
        # Debug numpy is CV2
        self.debug_frame = libvision.cv_to_cv2(frame)

        # CV2 Transforms
        self.numpy_frame = self.debug_frame.copy()
        self.numpy_frame = cv2.medianBlur(self.numpy_frame, 5)
        self.numpy_frame = cv2.cvtColor(self.numpy_frame, cv2.COLOR_BGR2HSV)

        (self.frame1, self.frame2, self.frame3) = cv2.split(self.numpy_frame)
        # Change the frame number to determine what channel to focus on
        self.numpy_frame = self.frame2

        # Thresholding
        self.buoy_frame = cv2.adaptiveThreshold(self.numpy_frame, 255,
                                                cv2.ADAPTIVE_THRESH_MEAN_C,
                                                cv2.THRESH_BINARY_INV,
                                                self.adaptive_thresh_blocksize,
                                                self.adaptive_thresh)

        kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (3, 3))
        kernel2 = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (4, 4))

        self.buoy_frame = cv2.erode(self.numpy_frame, kernel)
        self.buoy_frame = cv2.dilate(self.numpy_frame, kernel2)

        self.buoy_adaptive = self.buoy_frame.copy()

        # Find contours
        contours, hierarchy = cv2.findContours(self.numpy_frame, cv2.RETR_TREE,
                                               cv2.CHAIN_APPROX_SIMPLE)
        self.raw_led = []
        self.raw_buoys = []

        if len(contours) > 1:
            cnt = contours[0]
            cv2.drawContours(self.numpy_frame, contours, -1, (255, 255, 255),
                             3)

            for h, cnt in enumerate(contours):
                approx = cv2.approxPolyDP(cnt, 0.01 * cv2.arcLength(cnt, True),
                                          True)

                center, radius = cv2.minEnclosingCircle(cnt)
                x, y = center

                if len(approx) > 12:
                    if (radius > 30):
                        new_buoy = Buoy(int(x), int(y), int(radius), "unknown")
                        new_buoy.id = self.recent_id
                        self.recent_id += 1
                        self.raw_buoys.append(new_buoy)
                        cv2.drawContours(self.numpy_frame, [cnt], 0,
                                         (0, 0, 255), -1)
                        self.raw_buoys.append(new_buoy)

        for buoy1 in self.raw_buoys[:]:
            for buoy2 in self.raw_buoys[:]:
                if buoy1 is buoy2:
                    continue
                if buoy1 in self.raw_buoys and buoy2 in self.raw_buoys and \
                   math.fabs(buoy1.centerx - buoy2.centerx) > self.mid_sep and \
                   math.fabs(buoy1.centery - buoy2.centery) > self.mid_sep:
                    if buoy1.area < buoy2.area:
                        self.raw_buoys.remove(buoy1)
                    elif buoy2.area < buoy1.area:
                        self.raw_buoys.remove(buoy2)

        for buoy in self.raw_buoys:
            self.match_buoys(buoy)

        self.sort_buoys()
        self.draw_buoys()

        self.return_output()

        self.debug_to_cv = libvision.cv2_to_cv(self.debug_frame)
        self.numpy_to_cv = libvision.cv2_to_cv(self.numpy_frame)
        self.adaptive_to_cv = libvision.cv2_to_cv(self.adaptive_frame)

        svr.debug("processed", self.numpy_to_cv)
        svr.debug("adaptive", self.adaptive_to_cv)
        svr.debug("debug", self.debug_to_cv)
示例#48
0
    def process_frame(self, frame):

        # Resize image to 320x240
        #copy = cv.CreateImage(cv.GetSize(frame), 8, 3)
        #cv.Copy(frame, copy)
        #cv.SetImageROI(frame, (0, 0, 320, 240))
        #cv.Resize(copy, frame, cv.CV_INTER_NN)

        found_hedge = False

        test_frame = cv.CreateImage(cv.GetSize(frame), 8, 3)

        cv.Copy(frame, test_frame)

        cv.Smooth(frame, frame, cv.CV_MEDIAN, 7, 7)

        # Set binary image to have value channel
        hsv = cv.CreateImage(cv.GetSize(frame), 8, 3)
        binary = cv.CreateImage(cv.GetSize(frame), 8, 1)
        cv.CvtColor(frame, hsv, cv.CV_BGR2HSV)
        cv.SetImageCOI(hsv, 2)
        cv.Copy(hsv, binary)
        cv.SetImageCOI(hsv, 0)

        cv.AdaptiveThreshold(binary, binary,
                             255,
                             cv.CV_ADAPTIVE_THRESH_MEAN_C,
                             cv.CV_THRESH_BINARY_INV,
                             self.adaptive_thresh_blocksize,
                             self.adaptive_thresh,
                             )

        # Morphology
        
        kernel = cv.CreateStructuringElementEx(3, 3, 1, 1, cv.CV_SHAPE_ELLIPSE)
        #cv.Erode(binary, binary, kernel, 1)
        cv.Dilate(binary, binary, kernel, 4)
        
        if self.debug:
            color_filtered = cv.CloneImage(binary)

        # Get Edges
        #cv.Canny(binary, binary, 30, 40)

        # Hough Transform
        '''
        line_storage = cv.CreateMemStorage()
        raw_lines = cv.HoughLines2(binary, line_storage, cv.CV_HOUGH_STANDARD,
            rho=1,
            theta=math.pi/180,
            threshold=self.hough_threshold,
            param1=0,
            param2=0
        )
        '''
        # Hough Transform
        line_storage = cv.CreateMemStorage()
        raw_lines = cv.HoughLines2(binary, line_storage, cv.CV_HOUGH_PROBABILISTIC,
                                   rho=1,
                                   theta=math.pi / 180,
                                   threshold=self.hough_threshold,
                                   param1=self.min_length,
                                   param2=self.max_gap
                                   )

        self.hor_lines = []

        for line in raw_lines:
            slope = line_slope(line[0], line[1])
            if slope is None:
                continue
            if math.fabs(line_slope(line[0], line[1])) < self.hor_threshold:
                self.hor_lines.append(line)

        max_length = 0

        for line in self.hor_lines:
            print line
            if math.fabs(line_distance(line[0], line[1])) > max_length:
                max_length = math.fabs(line_distance(line[0], line[1]))
                crossbar_seg = line

        '''
        # Get vertical lines
        vertical_lines = []
        for line in raw_lines:
            if line[1] < self.vertical_threshold or \
                line[1] > math.pi-self.vertical_threshold:

                vertical_lines.append( (abs(line[0]), line[1]) )

        # Group vertical lines
        vertical_line_groups = []  # A list of line groups which are each a line list
        for line in vertical_lines:
            group_found = False
            for line_group in vertical_line_groups:

                if line_group_accept_test(line_group, line, self.max_range):
                    line_group.append(line)
                    group_found = True

            if not group_found:
                vertical_line_groups.append([line])

        # Average line groups into lines
        vertical_lines = []
        for line_group in vertical_line_groups:
            rhos = map(lambda line: line[0], line_group)
            angles = map(lambda line: line[1], line_group)
            line = (sum(rhos)/len(rhos), circular_average(angles, math.pi))
            vertical_lines.append(line)

        # Get horizontal lines
        horizontal_lines = []
        for line in raw_lines:
            dist_from_horizontal = (math.pi/2 + line[1]) % math.pi
            if dist_from_horizontal < self.horizontal_threshold or \
                dist_from_horizontal > math.pi-self.horizontal_threshold:

                horizontal_lines.append( (abs(line[0]), line[1]) )

        # Group horizontal lines
        horizontal_line_groups = []  # A list of line groups which are each a line list
        for line in horizontal_lines:
            group_found = False
            for line_group in horizontal_line_groups:

                if line_group_accept_test(line_group, line, self.max_range):
                    line_group.append(line)
                    group_found = True

            if not group_found:
                horizontal_line_groups.append([line])

        if len(horizontal_line_groups) is 1:
            self.seen_crossbar = True
            rhos = map(lambda line: line[0], horizontal_line_groups[0])
            angles = map(lambda line: line[1], horizontal_line_groups[0])
            line = (sum(rhos)/len(rhos), circular_average(angles, math.pi))
            horizontal_lines = [line]
        else:
            self.seen_crossbar = False
            horizontal_lines = []

        self.left_pole = None
        self.right_pole = None
        if len(vertical_lines) is 2:
            roi = cv.GetImageROI(frame)
            width = roi[2]
            height = roi[3]
            self.left_pole = round(min(vertical_lines[0][0], vertical_lines[1][0]), 2) - width/2
            self.right_pole = round(max(vertical_lines[0][0], vertical_lines[1][0]), 2) - width/2
        #TODO: If one pole is seen, is it left or right pole?

        # Calculate planar distance r (assuming we are moving perpendicular to
        # the hedge)
        if self.left_pole and self.right_pole:
            theta = abs(self.left_pole - self.right_pole)
            self.r = 3 / tan(radians(theta/2))
        else:
            self.r = None

        if self.r and self.seen_crossbar:
            bar_phi = (-1*horizontal_lines[0][0] + frame.height/2) / (frame.height/2) * 32
            self.crossbar_depth = self.r * atan(radians(bar_phi))
        else:
            self.crossbar_depth = None
        '''
        self.left_pole = None
        self.right_pole = None
        self.seen_crossbar = False
        self.crossbar_depth = None

        if self.debug and max_length != 0:
            cv.CvtColor(color_filtered, frame, cv.CV_GRAY2RGB)

            #libvision.misc.draw_lines(frame, vertical_lines)
            #libvision.misc.draw_lines(frame, horizontal_lines)
            # for line in raw_lines:
            #    cv.Line(frame,line[0],line[1], (255,255,0), 10, cv.CV_AA, 0)
            #    cv.Circle(frame, line[1], 15, (255,0,0), 2,8,0)
            # print len(raw_lines)
            #cv.ShowImage("Hedge", cv.CloneImage(frame))
            if (crossbar_seg[0][0] - frame.width / 2) * 37 / (frame.width / 2) < (crossbar_seg[1][0] - frame.width / 2) * 37 / (frame.width / 2):
                self.left_pole = round((crossbar_seg[0][0] - frame.width / 2) * 37 / (frame.width / 2))
                self.right_pole = round((crossbar_seg[1][0] - frame.width / 2) * 37 / (frame.width / 2))
            else:
                self.left_pole = round((crossbar_seg[1][0] - frame.width / 2) * 37 / (frame.width / 2))
                self.right_pole = round((crossbar_seg[0][0] - frame.width / 2) * 37 / (frame.width / 2))
            self.crossbar_depth = round(-1 * (crossbar_seg[1][1] - frame.height / 2) * 36 / (frame.height / 2))

            if math.fabs(self.left_pole) <= 37 and math.fabs(self.left_pole) >= self.frame_boundary_thresh:
                self.left_pole = None
            if math.fabs(self.right_pole) <= 37 and math.fabs(self.right_pole) >= self.frame_boundary_thresh:
                self.right_pole = None

            self.seen_crossbar = True


            if self.left_pole and self.right_pole:

                self.returning = (self.left_pole + self.right_pole)/2
   	        print "Returning ", self.returning

                if self.last_seen < 0:
                    self.last_center = None
                    self.last_seen = 0
                if self.last_center is None:
                    self.last_center = self.returning
                    self.seen_count = 1
                elif math.fabs(self.last_center - self.returning) < self.center_trans_thresh:
                    self.seen_count += 1
                    self.last_seen += 2
                else:
                    self.last_seen -= 1

                if self.seen_count < self.seen_count_thresh:
                    self.left_pole = None
                    self.right_pole = None
                else: 
                    print "FOUND CENTER AND RETURNED IT"
                    self.found = True
            else:
                self.returning = 0
                if self.last_seen < 0:
                    self.last_center = None
                    self.last_seen = 0
                self.last_seen -= 1
                self.left_pole = None
                self.right_pole = None




            cv.Line(frame, crossbar_seg[0], crossbar_seg[1], (255, 255, 0), 10, cv.CV_AA, 0)
            if self.left_pole and crossbar_seg[0][0] < crossbar_seg[1][0] :

                cv.Line(frame, crossbar_seg[0], (crossbar_seg[0][0], crossbar_seg[0][0] - 500), (255, 0, 0), 10, cv.CV_AA, 0)
            elif self.left_pole:
                cv.Line(frame, crossbar_seg[1], (crossbar_seg[1][0], crossbar_seg[1][1] - 500), (255, 0, 0), 10, cv.CV_AA, 0)

            if self.right_pole and crossbar_seg[0][0] > crossbar_seg[1][0]:

                cv.Line(frame, crossbar_seg[0], (crossbar_seg[0][0], crossbar_seg[0][0] - 500), (255, 0, 0), 10, cv.CV_AA, 0)
            elif self.right_pole:
                cv.Line(frame, crossbar_seg[1], (crossbar_seg[1][0], crossbar_seg[1][1] - 500), (255, 0, 0), 10, cv.CV_AA, 0)

            # populate self.output with infos
            self.output.seen_crossbar = self.seen_crossbar
            self.output.left_pole = self.left_pole
            self.output.right_pole = self.right_pole
            #self.output.r = self.r
            self.output.crossbar_depth = self.crossbar_depth

            self.return_output()
            print self
        else:
            cv.CvtColor(color_filtered, frame, cv.CV_GRAY2RGB)

        svr.debug("Hedge", cv.CloneImage(frame))
        svr.debug("Hedge2", test_frame)
示例#49
0
文件: bins.py 项目: mrwiggin/seawolf5
    def process_frame(self, frame):
        debug_frame = cv.CreateImage(cv.GetSize(frame),8,3)
        cv.Copy(frame, debug_frame)
        
        cv.Smooth(frame, frame, cv.CV_MEDIAN, 7, 7)

        # Set binary image to have saturation channel
        hsv = cv.CreateImage(cv.GetSize(frame), 8, 3)
        binary = cv.CreateImage(cv.GetSize(frame), 8, 1)
        cv.CvtColor(frame, hsv, cv.CV_BGR2HSV)
        cv.SetImageCOI(hsv, 2)
        cv.Copy(hsv, binary)
        cv.SetImageCOI(hsv, 0)

        cv.AdaptiveThreshold(binary, binary,
                             255,
                             cv.CV_ADAPTIVE_THRESH_MEAN_C,
                             cv.CV_THRESH_BINARY_INV,
                             self.adaptive_thresh_blocksize,
                             self.adaptive_thresh,
                             )

        # Morphology
        kernel = cv.CreateStructuringElementEx(5, 5, 3, 3, cv.CV_SHAPE_ELLIPSE)
        cv.Erode(binary, binary, kernel, 1)
        cv.Dilate(binary, binary, kernel, 1)
    
        # Get Edges
        #cv.Canny(binary, binary, 30, 40)
   
        cv.CvtColor(binary, debug_frame, cv.CV_GRAY2RGB)
    
        # Hough Transform
        line_storage = cv.CreateMemStorage()
        raw_lines = cv.HoughLines2(binary, line_storage, cv.CV_HOUGH_STANDARD,
                                   rho=1,
                                   theta=math.pi/180,
                                   threshold=self.hough_threshold,
                                   param1=0,
                                   param2=0
                                   )
    
        line_groups = []  # A list of line groups which are each a line list
            
        for line in raw_lines:
            group_found = False
            for line_group in line_groups:

                if line_group_accept_test(line_group, line, self.max_range):
                    line_group.append(line)
                    group_found = True

            if not group_found:
                line_groups.append([line])

            # Average line groups into lines
            lines = []
            for line_group in line_groups:
                rhos = map(lambda line: line[0], line_group)
                angles = map(lambda line: line[1], line_group)
                line = (sum(rhos)/len(rhos), circular_average(angles, math.pi))
                lines.append(line)

        
        libvision.misc.draw_lines(debug_frame, raw_lines)
           # cv.CvtColor(color_filtered,debug_frame, cv.CV_GRAY2RGB)
        svr.debug("Bins", debug_frame)
示例#50
0
    def process_frame(self, frame):
        debug_frame = cv.CreateImage(cv.GetSize(frame), 8, 3)
        cv.Copy(frame, debug_frame)

        cv.Smooth(frame, frame, cv.CV_MEDIAN, 7, 7)

        # Set binary image to have saturation channel
        hsv = cv.CreateImage(cv.GetSize(frame), 8, 3)
        binary = cv.CreateImage(cv.GetSize(frame), 8, 1)
        cv.CvtColor(frame, hsv, cv.CV_BGR2HSV)
        cv.SetImageCOI(hsv, 2)
        cv.Copy(hsv, binary)
        cv.SetImageCOI(hsv, 0)

        cv.AdaptiveThreshold(
            binary,
            binary,
            255,
            cv.CV_ADAPTIVE_THRESH_MEAN_C,
            cv.CV_THRESH_BINARY_INV,
            self.adaptive_thresh_blocksize,
            self.adaptive_thresh,
        )

        # Morphology
        kernel = cv.CreateStructuringElementEx(5, 5, 3, 3, cv.CV_SHAPE_ELLIPSE)
        cv.Erode(binary, binary, kernel, 1)
        cv.Dilate(binary, binary, kernel, 1)

        # Get Edges
        #cv.Canny(binary, binary, 30, 40)

        cv.CvtColor(binary, debug_frame, cv.CV_GRAY2RGB)

        # Hough Transform
        line_storage = cv.CreateMemStorage()
        raw_lines = cv.HoughLines2(binary,
                                   line_storage,
                                   cv.CV_HOUGH_STANDARD,
                                   rho=1,
                                   theta=math.pi / 180,
                                   threshold=self.hough_threshold,
                                   param1=0,
                                   param2=0)

        line_groups = []  # A list of line groups which are each a line list

        for line in raw_lines:
            group_found = False
            for line_group in line_groups:

                if line_group_accept_test(line_group, line, self.max_range):
                    line_group.append(line)
                    group_found = True

            if not group_found:
                line_groups.append([line])

            # Average line groups into lines
            lines = []
            for line_group in line_groups:
                rhos = map(lambda line: line[0], line_group)
                angles = map(lambda line: line[1], line_group)
                line = (sum(rhos) / len(rhos),
                        circular_average(angles, math.pi))
                lines.append(line)

        libvision.misc.draw_lines(debug_frame, raw_lines)
        # cv.CvtColor(color_filtered,debug_frame, cv.CV_GRAY2RGB)
        svr.debug("Bins", debug_frame)
示例#51
0
    def display(self):
        #Draws objects on frames

        svr.debug("Unchanged",self.default_frame)
示例#52
0
    def process_frame(self, frame):
        self.debug_frame = cv.CreateImage(cv.GetSize(frame), 8, 3)
        self.test_frame = cv.CreateImage(cv.GetSize(frame), 8, 3)

        cv.Copy(frame, self.debug_frame)
        cv.Copy(frame, self.test_frame)

        cv.Smooth(frame, frame, cv.CV_MEDIAN, 7, 7)

        # Set binary image to have saturation channel
        hsv = cv.CreateImage(cv.GetSize(frame), 8, 3)
        binary = cv.CreateImage(cv.GetSize(frame), 8, 1)
        cv.CvtColor(frame, hsv, cv.CV_BGR2HSV)
        cv.SetImageCOI(hsv, 1)
        cv.Copy(hsv, binary)
        cv.SetImageCOI(hsv, 0)

        # Adaptive Threshold
        cv.AdaptiveThreshold(binary, binary,
                             255,
                             cv.CV_ADAPTIVE_THRESH_MEAN_C,
                             cv.CV_THRESH_BINARY_INV,
                             self.adaptive_thresh_blocksize,
                             self.adaptive_thresh,
                             )

        # Morphology
        kernel = cv.CreateStructuringElementEx(5, 5, 3, 3, cv.CV_SHAPE_ELLIPSE)
        cv.Erode(binary, binary, kernel, 1)
        cv.Dilate(binary, binary, kernel, 1)

        cv.CvtColor(binary, self.debug_frame, cv.CV_GRAY2RGB)

        # Find Corners
        temp1 = cv.CreateImage(cv.GetSize(frame), 8, 1)
        temp2 = cv.CreateImage(cv.GetSize(frame), 8, 1)
        self.corners = cv.GoodFeaturesToTrack(binary, temp1, temp2, self.max_corners, self.quality_level, self.min_distance, None, self.good_features_blocksize, 0, 0.4)

        # Display Corners
        for corner in self.corners:
            corner_color = (0, 0, 255)
            text_color = (0, 255, 0)
            font = cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, .6, .6, 0, 1, 1)
            cv.Circle(self.debug_frame, (int(corner[0]), int(corner[1])), 15, corner_color, 2, 8, 0)

        # Find Candidates
        for confirmed in self.confirmed:
            confirmed.corner1_repl_check = 0
            confirmed.corner2_repl_check = 0
            confirmed.corner3_repl_check = 0
            confirmed.corner4_repl_check = 0
            for corner in self.corners:
                if math.fabs(confirmed.corner1[0] - corner[0]) < self.MaxCornerTrans and \
                   math.fabs(confirmed.corner1[1] - corner[1]) < self.MaxCornerTrans:
                    confirmed.corner1_repl_check = 1
                    confirmed.corner1_repl = corner
                elif math.fabs(confirmed.corner2[0] - corner[0]) < self.MaxCornerTrans and \
                        math.fabs(confirmed.corner2[1] - corner[1]) < self.MaxCornerTrans:
                    confirmed.corner2_repl_check = 1
                    confirmed.corner2_repl = corner
                elif math.fabs(confirmed.corner3[0] - corner[0]) < self.MaxCornerTrans and \
                        math.fabs(confirmed.corner3[1] - corner[1]) < self.MaxCornerTrans:
                    confirmed.corner3_repl_check = 1
                    confirmed.corner3_repl = corner
                elif math.fabs(confirmed.corner4[0] - corner[0]) < self.MaxCornerTrans and \
                        math.fabs(confirmed.corner4[1] - corner[1]) < self.MaxCornerTrans:
                    confirmed.corner4_repl_check = 1
                    confirmed.corner4_repl = corner
            if confirmed.corner4_repl_check == 1 and confirmed.corner3_repl_check == 1 and confirmed.corner2_repl_check == 1 and confirmed.corner1_repl_check == 1:
                confirmed.corner1 = confirmed.corner1_repl
                confirmed.corner2 = confirmed.corner2_repl
                confirmed.corner3 = confirmed.corner3_repl
                confirmed.corner4 = confirmed.corner4_repl

                confirmed.midx = rect_midpointx(confirmed.corner1, confirmed.corner2, confirmed.corner3, confirmed.corner4)
                confirmed.midy = rect_midpointy(confirmed.corner1, confirmed.corner2, confirmed.corner3, confirmed.corner4)

                if confirmed.last_seen < self.last_seen_max:
                    confirmed.last_seen += 5

        for corner1 in self.corners:
            for corner2 in self.corners:
                for corner3 in self.corners:
                    for corner4 in self.corners:
                        # Checks that corners are not the same and are in the proper orientation
                        if corner4[0] != corner3[0] and corner4[0] != corner2[0] and corner4[0] != corner1[0] and \
                           corner3[0] != corner2[0] and corner3[0] != corner1[0] and corner2[0] != corner1[0] and \
                           corner4[1] != corner3[1] and corner4[1] != corner2[1] and corner4[1] != corner1[1] and \
                           corner3[1] != corner2[1] and corner3[1] != corner1[1] and corner2[1] != corner1[1] and \
                           corner2[0] >= corner3[0] and corner1[1] >= corner4[1] and corner2[0] >= corner1[0]:
                            # Checks that the side ratios are correct
                            if math.fabs(line_distance(corner1, corner3) - line_distance(corner2, corner4)) < self.size_threshold and \
                               math.fabs(line_distance(corner1, corner2) - line_distance(corner3, corner4)) < self.size_threshold and \
                               math.fabs(line_distance(corner1, corner3) / line_distance(corner1, corner2)) < self.ratio_threshold or \
                               math.fabs(line_distance(corner1, corner2) / line_distance(corner1, corner3)) < self.ratio_threshold:
                                # Checks that angles are roughly 90 degrees
                                angle_cnr_2 = math.fabs(angle_between_lines(line_slope(corner1, corner2), line_slope(corner2, corner4)))
                                if self.angle_min < angle_cnr_2 < self.angle_max:
                                    angle_cnr_3 = math.fabs(angle_between_lines(line_slope(corner1, corner3), line_slope(corner3, corner4)))
                                    if self.angle_min2 < angle_cnr_3 < self.angle_max2:
                                        new_bin = Bin(corner1, corner2, corner3, corner4)
                                        self.match_bins(new_bin)
        self.sort_bins()

        '''
        #START SHAPE PROCESSING

        #TODO load these ONCE somewhere
        samples = np.loadtxt('generalsamples.data',np.float32)
        responses = np.loadtxt('generalresponses.data',np.float32)
        responses = responses.reshape((responses.size,1))
        model = cv2.KNearest()
        model.train(samples,responses)

        for bin in self.confirmed:
                try:
                        bin.speedlimit
                except:
                        continue
                transf = cv.CreateMat(3, 3, cv.CV_32FC1)
                corner_orders = [
                        [bin.corner1, bin.corner2, bin.corner3, bin.corner4], #0 degrees
                        [bin.corner4, bin.corner3, bin.corner2, bin.corner1], #180 degrees
                        [bin.corner2, bin.corner4, bin.corner1, bin.corner3], #90 degrees
                        [bin.corner3, bin.corner1, bin.corner4, bin.corner2], #270 degrees
                        [bin.corner3, bin.corner4, bin.corner1, bin.corner2], #0 degrees and flipped X
                        [bin.corner2, bin.corner1, bin.corner4, bin.corner3], #180 degrees and flipped X
                        [bin.corner1, bin.corner3, bin.corner2, bin.corner4], #90 degrees and flipped X
                        [bin.corner4, bin.corner2, bin.corner3, bin.corner1]] #270 degrees andf flipped X
                for i in range(0, 8):
                        cv.GetPerspectiveTransform(
                                corner_orders[i],
                                [(0, 0), (0, 256), (128, 0), (128, 256)],
                                transf
                        )
                        shape = cv.CreateImage([128, 256], 8, 3)
                        cv.WarpPerspective(frame, shape, transf)

                        shape_thresh = np.zeros((256-104,128,1), np.uint8)
                        j = 104
                        while j<256:
                            i = 0
                            while i<128:
                                    pixel = cv.Get2D(shape, j, i)
                                if int(pixel[2]) > (int(pixel[1]) + int(pixel[0])) * 0.7:
                                    shape_thresh[j-104,i] = 255
                                else:
                                    shape_thresh[j-104,i] = 0
                                i = i+1
                            j = j+1
                        cv2.imshow("Bin " + str(i), shape_thresh)
                        contours,hierarchy = cv2.findContours(thresh,cv2.RETR_LIST,cv2.CHAIN_APPROX_NONE)
                        for cnt in contours:
                                    if cv2.contourArea(cnt)>50:
                                        [x,y,w,h] = cv2.boundingRect(cnt)
                                        if  h>54 and w>36:
                                                    roi = thresh[y:y+h,x:x+w]
                                                    roismall = cv2.resize(roi,(10,10))
                                                    roismall = roismall.reshape((1,100))
                                                    roismall = np.float32(roismall)
                                                    retval, results, neigh_resp, dists = model.find_nearest(roismall, k = 1)
                                                    digit_tuples.append( (x, int((results[0][0]))) )

                            if len(digit_tuples) == 2:
                                    digit_tuples_sorted = sorted(digit_tuples, key=lambda digit_tuple: digit_tuple[0])
                                speedlimit = 0
                                for i in range(0, len(digit_tuples_sorted)):
                                            speedlimit = speedlimit * 10 + digit_tuples_sorted[i][1]
                                    bin.speedlimit = speedlimit
                                    print "Found speed limit: " + str(speedlimit)
                                    break
                            else:
                                    print "Unable to determine speed limit"

        #... TODO more
        #END SHAPE PROCESSING
        '''

        svr.debug("Bins", self.debug_frame)
        svr.debug("Bins2", self.test_frame)

        # Output bins
        self.output.bins = self.confirmed
        anglesum = 0
        for bins in self.output.bins:
            bins.theta = (bins.midx - frame.width / 2) * 37 / (frame.width / 2)
            bins.phi = -1 * (bins.midy - frame.height / 2) * 36 / (frame.height / 2)
            bins.shape = bins.object
            anglesum += bins.angle
           # bins.orientation = bins.angle
        if len(self.output.bins) > 0:
            self.output.orientation = anglesum / len(self.output.bins)
        else:
            self.output.orientation = None
        self.return_output()
示例#53
0
    def process_frame(self, frame):
        self.numpy_frame = libvision.cv_to_cv2(frame)
        self.debug_frame = self.numpy_frame.copy()

        self.numpy_frame = cv2.medianBlur(self.numpy_frame, 7)
        self.numpy_frame = cv2.cvtColor(self.numpy_frame, cv2.COLOR_BGR2HSV)

        (rf1, rf2, rf3) = cv2.split(self.numpy_frame)
        # RF2-inverted for red
        # RF1 for green

        rBinary = rf2
       # rBinary = cv2.bitwise_not(rBinary)
        gBinary = rf1

        # Adaptive Threshold
        rBinary = cv2.adaptiveThreshold(rBinary, 255,
                                        cv2.ADAPTIVE_THRESH_MEAN_C,
                                        cv2.THRESH_BINARY_INV,
                                        self.adaptive_thresh_blocksize,
                                        self.adaptive_thresh)

        gBinary = cv2.adaptiveThreshold(gBinary, 255,
                                        cv2.ADAPTIVE_THRESH_MEAN_C,
                                        cv2.THRESH_BINARY_INV,
                                        self.Gadaptive_thresh_blocksize,
                                        self.Gadaptive_thresh)

        rFrame = rBinary.copy()
        gFrame = gBinary.copy()

        # Morphology
        kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (3, 3))

        rBinary = cv2.erode(rBinary, kernel)
        rBinary = cv2.dilate(rBinary, kernel)
        gBinary = cv2.erode(gBinary, kernel)
        gBinary = cv2.dilate(gBinary, kernel)

        gray = cv2.cvtColor(self.numpy_frame, cv2.COLOR_BGR2GRAY)

        edges = cv2.Canny(gray, 150, 200, apertureSize=3)

        lines = cv2.HoughLines(edges, 1, np.pi / 180, 275)
        for rho, theta in lines[0]:
            a = np.cos(theta)
            b = np.sin(theta)
            x0 = a * rho
            y0 = b * rho
            x1 = int(x0 + 1000 * (-b))  # Here i have used int() instead of rounding the decimal value, so 3.8 --> 3
            y1 = int(y0 + 1000 * (a))  # But if you want to round the number, then use np.around() function, then 3.8 --> 4.0
            x2 = int(x0 - 1000 * (-b))  # But we need integers, so use int() function after that, ie int(np.around(x))
            y2 = int(y0 - 1000 * (a))
            cv2.line(self.debug_frame, (x1, y1), (x2, y2), (0, 255, 0), 2)

        rFrame = libvision.cv2_to_cv(rFrame)
        gFrame = libvision.cv2_to_cv(gFrame)
        self.debug_frame = libvision.cv2_to_cv(self.debug_frame)
        # svr.debug("Rframe", rFrame)
        # svr.debug("Gframe", gFrame)
        svr.debug("debug", self.debug_frame)