예제 #1
0
    def __init__(self,
                 objectDetector=None,
                 laneDetector=None,
                 grassDetector=None):
        # Subscribe to PointCloud2
        self.subImage = rospy.Subscriber("/left/image_rect_color",\
            Image, self.cbImage, queue_size=1)
        self.subPoints = rospy.Subscriber("/points2",\
            PointCloud2, self.cbPoints, queue_size=1)

        self.images = []
        self.mask = None
        self.imageStamps = []
        self.imageInd = 0

        self.bridge = CvBridge()

        self.showDetected = False

        self.obstacleCloud = np.array([])
        if objectDetector:
            self.objectDetector = objectDetector
        else:
            self.objectDetector = ObstacleDetector((240, 320))
            # We assume that the normal of the plane is pointing straight up
            # in the vision (-Y direction)
            self.objectDetector.setNormalConstraint(np.array([0., -1., -0.2]))
            # And allow the search to deviate only 30 degrees from that
            self.objectDetector.angleConstraint = 30.

        if laneDetector:
            self.laneDetector = laneDetector
        else:
            self.laneDetector = LaneDetector((240, 320))

        if grassDetector:
            self.grassDetector = grassDetector
        else:
            # Init with H,S,V thresholds
            self.grassDetector = GrassDetector(45, 35, 35)

        if VERBOSE:
            print "Subscribed to /left/image_rect_color"
            print "Subscribed to /points2"
예제 #2
0
    def __init__(self, camera_calibration_file):
        self.first_image = True

        self.camera_calibration_file = camera_calibration_file
        self.M = None
        self.M_inv = None
        self.undistorter = None  # will be set with first processed image because it needs image size

        self.lane_det = ld.LaneDetector(n_windows=9,
                                        margin=80,
                                        min_pix=40,
                                        poly_margin=50)
예제 #3
0
    def __init__(self, objectDetector=None, laneDetector=None, grassDetector=None):
        # Subscribe to PointCloud2
        self.subImage = rospy.Subscriber("/left/image_rect_color",\
            Image, self.cbImage, queue_size=1)
        self.subPoints = rospy.Subscriber("/points2",\
            PointCloud2, self.cbPoints, queue_size=1)

        self.images = []
        self.mask = None
        self.imageStamps = []
        self.imageInd = 0

        self.bridge = CvBridge()

        self.showDetected = False

        self.obstacleCloud = np.array([])
        if objectDetector:
            self.objectDetector = objectDetector
        else:
            self.objectDetector = ObstacleDetector((240,320))
            # We assume that the normal of the plane is pointing straight up
            # in the vision (-Y direction)
            self.objectDetector.setNormalConstraint( np.array([0.,-1.,-0.2]) )
            # And allow the search to deviate only 30 degrees from that
            self.objectDetector.angleConstraint = 30.

        if laneDetector:
            self.laneDetector = laneDetector
        else:
            self.laneDetector = LaneDetector((240,320))

        if grassDetector:
            self.grassDetector = grassDetector
        else:
            # Init with H,S,V thresholds
            self.grassDetector = GrassDetector(45, 35, 35)

        if VERBOSE:
            print "Subscribed to /left/image_rect_color"
            print "Subscribed to /points2"
예제 #4
0
def main():

    camera = None
    detector = None
    color_Picker = None
    enable_Color_Picker = False
    color_Picker_Type = "BGR_Picker"
    work_image = None
    complete_mask_image = None
    direction_line_image = None
    detector_type = "RGBDetector"
    video_resize_width = 1080

    blue_upper = []
    yellow_upper = []
    object_upper = []

    blue_lower = []
    yellow_lower = []
    object_lower = []

    videonumber = 1
    url = ('Videos/video' + str(videonumber) + '.mp4')

    camera = Camera.Camera(video_source=0)

    seri = Serial2.Serial2('COM8', 9600, 8, serial.PARITY_NONE, 1, 2, False,
                           True, 2, False, None)

    camera.open_video()

    work_image = camera.get_frame()

    if color_Picker_Type is "BGR_Picker":
        detector_type = "RGBDetector"

    if color_Picker_Type is "HSV_Picker":
        detector_type = "HSVDetector"

    detector_manager = LaneDetector.lane_detector_manager(
        detector_type=detector_type)

    detector = detector_manager.get_detector()

    if enable_Color_Picker:
        color_manager = ColorPicker.color_picker_manager(
            color_type=color_Picker_Type)
        color_picker = color_manager.get_detector()

        blue_upper, blue_lower = color_picker.select_color(
            image=None, message="pick blue lane")
        yellow_upper, yellow_lower = color_picker.select_color(
            image=None, message="pick yellow lane")
        object_upper, object_lower = color_picker.select_color(
            image=None, message="pick object")

        detector.set_colors(blue_upper, yellow_upper, object_upper, blue_lower,
                            yellow_lower, object_lower)

    while camera.playing():
        while camera.get_frame() is None:
            continue

        work_image = camera.get_resize_image(width_size=video_resize_width)
        cropped_image = camera.crop_border_image(image=work_image)

        complete_mask = detector.get_lanes(base_frame=work_image,
                                           cropped_frame=cropped_image)
        if complete_mask is not None:
            direction_line_image, value = detector.draw_direction_lines(
                mask_image=complete_mask)
        # Send a value to the arduino fofr the servo angle
        value = str((int((value - 320) / 2)) + 90)
        speed = 1420

        message = "{\"sensor\":\"gps\",\"time\":1351824120,\"data\":[" + str(
            speed) + "," + value + "]}"
        seri.sendMessage(message, 11)

        # Stack source image with lane image and display
        display = np.hstack((work_image, direction_line_image))
        display = cv2.resize(display, (1080, 500))
        cv2.imshow("Lane Detection", display)
        if cv2.waitKey(1) & 0xFF == ord('q'):
            break
예제 #5
0
    # Cropping out upper half b/c doesn't map to road
    cropped_img = img[img.shape[0] / 2:-1, :]

    h = 1.518  # meters
    t = np.asarray([0, 0, -h], np.float32)
    # Map from world frame to camera frame
    R = np.asarray([[0, -1, 0], [0, 0, -1], [1, 0, 0]], np.float32)

    K = np.asarray(
        [[617.2716, 0, 327.2818], [0, 617.1263, 245.0939], [0, 0, 1]],
        np.float32)

    D = np.asarray([0, 0, 0, 0, 0], np.float32)
    FOV_h = np.radians(91.2)
    FOV_v = np.radians(65.5)
    params = LaneDetector.CameraParams()
    params.K = K
    params.D = D
    params.FOV_h = FOV_h
    params.FOV_v = FOV_v
    params.height = height
    params.width = width

    det = LaneDetector.LaneDetector(R, t, params)
    mask_img = det.filter(img)
    blur_img = det.blur_mask(mask_img)
    warped_img = det.perspective_warp(blur_img)
    (left, center, right) = det.sliding_window(warped_img)
    waypoints = det.generate_waypoints(img, center)

    plt.figure(1)
예제 #6
0
class Ros2Python:

    def __init__(self, objectDetector=None, laneDetector=None, grassDetector=None):
        # Subscribe to PointCloud2
        self.subImage = rospy.Subscriber("/left/image_rect_color",\
            Image, self.cbImage, queue_size=1)
        self.subPoints = rospy.Subscriber("/points2",\
            PointCloud2, self.cbPoints, queue_size=1)

        self.images = []
        self.mask = None
        self.imageStamps = []
        self.imageInd = 0

        self.bridge = CvBridge()

        self.showDetected = False

        self.obstacleCloud = np.array([])
        if objectDetector:
            self.objectDetector = objectDetector
        else:
            self.objectDetector = ObstacleDetector((240,320))
            # We assume that the normal of the plane is pointing straight up
            # in the vision (-Y direction)
            self.objectDetector.setNormalConstraint( np.array([0.,-1.,-0.2]) )
            # And allow the search to deviate only 30 degrees from that
            self.objectDetector.angleConstraint = 30.

        if laneDetector:
            self.laneDetector = laneDetector
        else:
            self.laneDetector = LaneDetector((240,320))

        if grassDetector:
            self.grassDetector = grassDetector
        else:
            # Init with H,S,V thresholds
            self.grassDetector = GrassDetector(45, 35, 35)

        if VERBOSE:
            print "Subscribed to /left/image_rect_color"
            print "Subscribed to /points2"

    def run(self):
        rospy.init_node('Ros2Python', anonymous=True)
        try:
            rospy.spin()
        except KeyboardInterrupt:
            print "Shutting down liveObstacleDetection module"

    def cbPoints(self, point_msg):
        if VERBOSE:
            print 'Received points. Size: (', point_msg.width, ',', point_msg.height, ')'
        # Convert to numpy array (http://goo.gl/s6OGja)
        cloud = np.array(points2cloud_to_array(point_msg))

        # ObDetector only recalculates everything when the cloud is updated
        cloud = np.dstack((cloud['x'], cloud['y'], cloud['z']))

        pointT = point_msg.header.stamp.to_sec()

        #print 'Point Time:', pointT
        #print 'Image Times:', self.imageStamps
        image = None
        for i in range(len(self.imageStamps)):
            imageT = self.imageStamps[i]
            if imageT > pointT:
                image = self.images[i]
                break

        if image==None:
            image = self.images[i]
        self.images = self.images[i:]
        self.imageStamps = self.imageStamps[i:]

        self.objectDetector.updateImage(image)
        self.laneDetector.updateImage(image)
        self.grassDetector.updateImage(image)

        self.objectDetector.updatePoints(cloud)


        ##### Jared, these are the 2 most useful variables for you:

        ''' mask: is a numpy array the same size as the camera image. The
            elements in the mask are set to 1 if that pixel is part of the
            plane/ground, 2 if that pixel is an object, and 0 otherwise.
        '''

        self.mask = self.objectDetector.getPlaneObjectMask()
        self.grassDetector.updateMask(self.mask==1)
        grassMask = np.logical_and(self.grassDetector.findGrass(),self.mask!=2)
        self.mask[grassMask] = 1

        model = self.objectDetector.model
        planeInd = model.coord2ind( np.array( np.where(self.mask==1) ).T )
        planeInd = planeInd.astype(int)
        success, coeff = model.optimiseModelCoefficients(\
            planeInd,self.objectDetector.coeff)
        success, newInd = model.selectWithinDistance(coeff, 0.15)
        self.objectDetector.coeff = coeff

        #print newInd.shape
        #print newInd
        self.mask[ model.ind2coord(newInd) ] = 1

        ''' obsacleCloud: a list of 2D points relative to the camera.
            The positive X-axis goes to the right of the camera.
            The positive Y-axis goes away from the camera.
            TODO: Double check this =S
        '''
        self.obstacleCloud = self.objectDetector.getObstacleCloud()

        # Do the lane detect also
        detectedLanes = self.laneDetector.findLines()
        self.detectedLanes = np.logical_and(detectedLanes, self.mask!=2)
        self.laneDetector.showImage()
        self.mask[self.detectedLanes] = 2

        self.maskall = self.mask

        if self.showDetected:
            #print 'Showing Detected'
            self.objectDetector.showDetected(self.mask)
            self.showDetected = False
            cv2.waitKey(10)
        #self.objectDetector.showDepth()
        # Show the plane and abjects
        # im = self.objectDetector.image.copy()
        # im[grassMask] = [255,0,0]
        # im[mask==2] = [0,0,255]
        # cv2.imshow('DetectedObjects', im)

        # Show the lines
        #self.laneDetector.showImage()
        # cv2.imshow('DetectedLines', detectedLanes.astype(float)*255)


        # cv2.waitKey(15)
    def getMaskAll(self):
        return self.maskall

    def cbImage(self, img_msg):
        image = np.asarray(self.bridge.imgmsg_to_cv(img_msg))
        self.images.append(image)
        self.imageStamps.append(img_msg.header.stamp.to_sec())
예제 #7
0
class Ros2Python:
    def __init__(self,
                 objectDetector=None,
                 laneDetector=None,
                 grassDetector=None):
        # Subscribe to PointCloud2
        self.subImage = rospy.Subscriber("/left/image_rect_color",\
            Image, self.cbImage, queue_size=1)
        self.subPoints = rospy.Subscriber("/points2",\
            PointCloud2, self.cbPoints, queue_size=1)

        self.images = []
        self.mask = None
        self.imageStamps = []
        self.imageInd = 0

        self.bridge = CvBridge()

        self.showDetected = False

        self.obstacleCloud = np.array([])
        if objectDetector:
            self.objectDetector = objectDetector
        else:
            self.objectDetector = ObstacleDetector((240, 320))
            # We assume that the normal of the plane is pointing straight up
            # in the vision (-Y direction)
            self.objectDetector.setNormalConstraint(np.array([0., -1., -0.2]))
            # And allow the search to deviate only 30 degrees from that
            self.objectDetector.angleConstraint = 30.

        if laneDetector:
            self.laneDetector = laneDetector
        else:
            self.laneDetector = LaneDetector((240, 320))

        if grassDetector:
            self.grassDetector = grassDetector
        else:
            # Init with H,S,V thresholds
            self.grassDetector = GrassDetector(45, 35, 35)

        if VERBOSE:
            print "Subscribed to /left/image_rect_color"
            print "Subscribed to /points2"

    def run(self):
        rospy.init_node('Ros2Python', anonymous=True)
        try:
            rospy.spin()
        except KeyboardInterrupt:
            print "Shutting down liveObstacleDetection module"

    def cbPoints(self, point_msg):
        if VERBOSE:
            print 'Received points. Size: (', point_msg.width, ',', point_msg.height, ')'
        # Convert to numpy array (http://goo.gl/s6OGja)
        cloud = np.array(points2cloud_to_array(point_msg))

        # ObDetector only recalculates everything when the cloud is updated
        cloud = np.dstack((cloud['x'], cloud['y'], cloud['z']))

        pointT = point_msg.header.stamp.to_sec()

        #print 'Point Time:', pointT
        #print 'Image Times:', self.imageStamps
        image = None
        for i in range(len(self.imageStamps)):
            imageT = self.imageStamps[i]
            if imageT > pointT:
                image = self.images[i]
                break

        if image == None:
            image = self.images[i]
        self.images = self.images[i:]
        self.imageStamps = self.imageStamps[i:]

        self.objectDetector.updateImage(image)
        self.laneDetector.updateImage(image)
        self.grassDetector.updateImage(image)

        self.objectDetector.updatePoints(cloud)

        ##### Jared, these are the 2 most useful variables for you:
        ''' mask: is a numpy array the same size as the camera image. The
            elements in the mask are set to 1 if that pixel is part of the
            plane/ground, 2 if that pixel is an object, and 0 otherwise.
        '''

        self.mask = self.objectDetector.getPlaneObjectMask()
        self.grassDetector.updateMask(self.mask == 1)
        grassMask = np.logical_and(self.grassDetector.findGrass(),
                                   self.mask != 2)
        self.mask[grassMask] = 1

        model = self.objectDetector.model
        planeInd = model.coord2ind(np.array(np.where(self.mask == 1)).T)
        planeInd = planeInd.astype(int)
        success, coeff = model.optimiseModelCoefficients(\
            planeInd,self.objectDetector.coeff)
        success, newInd = model.selectWithinDistance(coeff, 0.15)
        self.objectDetector.coeff = coeff

        #print newInd.shape
        #print newInd
        self.mask[model.ind2coord(newInd)] = 1
        ''' obsacleCloud: a list of 2D points relative to the camera.
            The positive X-axis goes to the right of the camera.
            The positive Y-axis goes away from the camera.
            TODO: Double check this =S
        '''
        self.obstacleCloud = self.objectDetector.getObstacleCloud()

        # Do the lane detect also
        detectedLanes = self.laneDetector.findLines()
        self.detectedLanes = np.logical_and(detectedLanes, self.mask != 2)
        self.laneDetector.showImage()
        self.mask[self.detectedLanes] = 2

        self.maskall = self.mask

        if self.showDetected:
            #print 'Showing Detected'
            self.objectDetector.showDetected(self.mask)
            self.showDetected = False
            cv2.waitKey(10)
        #self.objectDetector.showDepth()
        # Show the plane and abjects
        # im = self.objectDetector.image.copy()
        # im[grassMask] = [255,0,0]
        # im[mask==2] = [0,0,255]
        # cv2.imshow('DetectedObjects', im)

        # Show the lines
        #self.laneDetector.showImage()
        # cv2.imshow('DetectedLines', detectedLanes.astype(float)*255)

        # cv2.waitKey(15)
    def getMaskAll(self):
        return self.maskall

    def cbImage(self, img_msg):
        image = np.asarray(self.bridge.imgmsg_to_cv(img_msg))
        self.images.append(image)
        self.imageStamps.append(img_msg.header.stamp.to_sec())