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 __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)
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 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
# 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)
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())
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())