def getDepthPipeline(FRAME_WIDTH, FRAME_HEIGHT, id=None): pipeline = rs.pipeline() config = rs.config() try: if (id != None): config.enable_device(id) except Exception as e: print(e) config.enable_stream(rs.stream.color, FRAME_WIDTH, FRAME_HEIGHT, rs.format.bgr8, 30) config.enable_stream(rs.stream.depth, FRAME_WIDTH, FRAME_HEIGHT, rs.format.z16, 30) config.enable_stream(rs.stream.infrared, 1, FRAME_WIDTH, FRAME_HEIGHT, rs.format.y8, 30) config.enable_stream(rs.stream.infrared, 2, FRAME_WIDTH, FRAME_HEIGHT, rs.format.y8, 30) print("[INFO] Starting streaming...") profile = pipeline.start(config) depth_sensor = profile.get_device().first_depth_sensor() depth_scale = depth_sensor.get_depth_scale() align_to = rs.stream.color align = rs.align(align_to) return pipeline, config, profile, align, depth_scale
def pub_img(): '''Callback function of subscribed topic. Here images get converted and features detected''' global stamp # cap = cv2.VideoCapture(1) pipe = rs.pipeline() config = rs.config() width = 640; height = 480; config.enable_stream(rs.stream.depth, width, height, rs.format.z16, 30) config.enable_stream(rs.stream.color, width, height, rs.format.rgb8, 30) profile = pipe.start(config) align_to = rs.stream.color align = rs.align(align_to) pub_rgb = rospy.Publisher("/output/image_raw/compressed_rgb", CompressedImage, queue_size=1) pub_depth = rospy.Publisher("/output/image_raw/compressed_depth", CompressedImage, queue_size=1) print('Hey! I started publishing images.') while(True): temp = pipe.wait_for_frames() aligned_frames = align.process(temp) aligned_depth_frame = aligned_frames.get_depth_frame() # aligned_depth_frame is a 640x480 depth image color_frame = aligned_frames.get_color_frame() if not aligned_depth_frame or not color_frame: pass buffer = np.zeros((height,width,4)) buffer[:,:,0:3] = (np.asanyarray(color_frame.get_data(),dtype=np.uint8)) buffer[:,:,3] = np.asanyarray(aligned_depth_frame.get_data(),dtype=np.uint8) #### Create CompressedImage #### msg_rgb = CompressedImage() msg_depth = CompressedImage() msg_rgb.header.stamp = rospy.Time.now() msg_depth.header.stamp = msg_rgb.header.stamp#rospy.Time.now() msg_rgb.format = "rgb"#f'{stamp}' msg_depth.format = "depth" msg_rgb.data = np.array(cv2.imencode('.jpg', buffer[:,:,0:3])[1]).tostring() msg_depth.data = np.array(cv2.imencode('.jpg', buffer[:,:,3])[1]).tostring() # Publish depth frame # pub = rospy.Publisher("/output/image_raw/compressed", CompressedImage, queue_size=1) # r = rospy.Rate(10) # try: pub_rgb.publish(msg_rgb) pub_depth.publish(msg_depth) # r.sleep() # except: # print('Could not publish') stamp += 1 if stamp >= 100000: stamp = 0
def initialize_camera(width, height): """ Set up camera pipeline for given resolution """ pipe = rs.pipeline() config = rs.config() config.enable_stream(rs.stream.depth, width, height, rs.format.z16, 30) config.enable_stream(rs.stream.color, width, height, rs.format.rgb8, 30) profile = pipe.start(config) align_to = rs.stream.color align = rs.align(align_to)
def __init__(self, resolution=(640, 480), depth_framerate=6, color_framerate=30, include_depth=False, decimation_magnitude=1, spatial_smooth=True, temporal_smooth=True): self.include_depth = include_depth self.pipeline = rs.pipeline() #Create a config and configure the pipeline to stream # different resolutions of color and depth streams self.config = rs.config() w, h = resolution self.config.enable_stream(rs.stream.color, w, h, rs.format.bgr8, color_framerate) if include_depth: self.config.enable_stream(rs.stream.depth, w, h, rs.format.z16, depth_framerate) # Start streaming self.profile = self.pipeline.start(self.config) # Getting the depth sensor's depth scale (see rs-align example for explanation) self.color_sensor = self.profile.get_device().first_color_sensor() if include_depth: self.depth_sensor = self.profile.get_device().first_depth_sensor() self.depth_scale = self.depth_sensor.get_depth_scale() # Create an align object # rs.align allows us to perform alignment of depth frames to others frames # The "align_to" is the stream type to which we plan to align depth frames. align_to = rs.stream.depth self.align = rs.align(align_to) # We'll want to store the intrinsics once we start getting images. self.aligned_depth_K = None self.filters = [] # Decimation filter if decimation_magnitude > 1: decimate_filter = rs.decimation_filter() decimate_filter.set_option(rs.option.filter_magnitude, decimation_magnitude) self.filters.append(decimate_filter) if spatial_smooth or temporal_smooth: self.filters.append(rs.disparity_transform()) if spatial_smooth: self.filters.append(rs.spatial_filter()) if temporal_smooth: self.filters.append(rs.temporal_filter()) if spatial_smooth or temporal_smooth: self.filters.append(rs.disparity_transform(False)) # Pointcloud conversion utility. self.pc = rs.pointcloud() self.depth_profile = rs.video_stream_profile(self.profile.get_stream(rs.stream.depth)) self.depth_intrinsics = self.depth_profile.get_intrinsics() self.w, self.h = self.depth_intrinsics.width, self.depth_intrinsics.height
def __init__(self, config, board, usestream=None): # initialize realsense specific settings self.pipeline = rs.pipeline() self.realsense_config = rs.config() # FIXME: missing frames when using videostream or too slow processing # https://github.com/IntelRealSense/librealsense/issues/2216 # initialize the base class super().__init__(config, board, usestream) # Use recorded depth and color streams and its configuration # If problems with colors occur, check bgr/rgb channels configurations if usestream is not None: rs.config.enable_device_from_file(self.realsense_config, usestream) self.realsense_config.enable_all_streams() # Configure depth and color streams else: self.realsense_config.enable_stream(rs.stream.depth, self.width, self.height, rs.format.z16, 30) self.realsense_config.enable_stream(rs.stream.color, self.width, self.height, rs.format.bgr8, 30) # Create alignment primitive with color as its target stream: self.alignment_stream = rs.align(rs.stream.color) # Start streaming # TODO: optionally rename variable to a more speaking one # FIXME: program ends here without further message try: self.profile = self.pipeline.start(self.realsense_config) # Getting the depth sensor's depth scale depth_sensor = self.profile.get_device().first_depth_sensor() self.depth_scale = depth_sensor.get_depth_scale() self.initialized = True except RuntimeError as e: logger.fatal("camera could not be initialized: {}".format(e)) self.initialized = False # FIXME: follow up? logger.debug("Depth Scale is: {}".format(self.depth_scale))
def __init__(self, config = None): Camera_Base.__init__(self, config) if(self.data['camera_name'] != "d400"): raise TypeExcept("Camera name is not d400") #Set pipeline and config self.pipeline = rs.pipeline() cfg = rs.config() cfg.enable_stream(rs.stream.color, self.data['color']['width'], self.data['color']['height'], rs.format.bgr8, self.data['color']['fps']) cfg.enable_stream(rs.stream.depth, self.data['depth']['width'], self.data['depth']['height'], rs.format.z16, self.data['depth']['fps']) # Start streaming profile = self.pipeline.start(cfg) depth_sensor = profile.get_device().first_depth_sensor() self.depth_scale = depth_sensor.get_depth_scale() # Create an align object # rs.align allows us to perform alignment of depth frames to others frames # The "align_to" is the stream type to which we plan to align depth frames. align_to = rs.stream.color self.align = rs.align(align_to)
def start(): pipeline = rs.pipeline() config = rs.config() print("Start load conf") config.enable_stream(rs.stream.depth, 1024, 768, rs.format.z16, 30) config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30) profile = pipeline.start(config) depth_sensor = profile.get_device().first_depth_sensor() set_short_range(depth_sensor) colorizer = rs.colorizer() print("Conf loaded") align_to = rs.stream.color align = rs.align(align_to) try: while True: color_image, depth_color_image, depth_image = get_color_depth( pipeline, align, colorizer) if color_image is None and color_image is None and color_image is None: continue color_image, depth_color_image, xy0, xy1, idx_to_coordinates = get_right_hand_coords( color_image, depth_color_image) if xy0 is not None or xy1 is not None: z_val_f, z_val_s, m_xy, c_xy, xy0_f, xy1_f, x, y, z = get_filtered_values( depth_image, xy0, xy1) pyautogui.moveTo(int(x), int(3500 - z)) # , duration=0.05 if draw_cam_out(color_image, depth_color_image, xy0_f, xy1_f, c_xy, m_xy): break finally: hands.close() pipeline.stop()
model.cuda() model.eval() # Setup Realsense pipeline pipe = rs.pipeline() configure = rs.config() width = 640; height = 480; configure.enable_stream(rs.stream.depth, width, height, rs.format.z16, 30) configure.enable_stream(rs.stream.color, width, height, rs.format.rgb8, 30) dec_filter = rs.decimation_filter () # Decimation - reduces depth frame density spat_filter = rs.spatial_filter() # Spatial - edge-preserving spatial smoothing temp_filter = rs.temporal_filter() # Temporal - reduces temporal noise pipe.start(configure) align_to = rs.stream.color align = rs.align(align_to) while(1): # temp = pipe.wait_for_frames() # aligned_frames = align.process(temp) # depth_frame = aligned_frames.get_depth_frame() # filtered = dec_filter.process(depth_frame) # filtered = spat_filter.process(fisltered) # filtered = temp_filter.process(filtered) # aligned_depth_frame = np.asanyarray(filtered.get_data(),dtype=np.uint8) # aligned_depth_frame is a 640x480 depth image # color_frame = np.asanyarray(aligned_frames.get_color_frame().get_data(),dtype=np.uint8) img, box = yolo_output(color_frame,model,['cell phone', 'person'], confidence, nms_thesh, CUDA, inp_dim) print('BOX:', box)
def main(): if not os.path.exists(args.out_rdir): os.mkdir(args.out_rdir) print('Created out_rgb_dir') if not os.path.exists(args.out_ddir): os.mkdir(args.out_ddir) print('Created out_depth_dir') if not os.path.exists(args.annotations_dir): os.mkdir(args.annotations_dir) print('Created args_out_dir') #Create pipeline for realsense2 pipe = rs.pipeline() config = rs.config() config.enable_stream(rs.stream.depth, 640, 360, rs.format.z16, 30) config.enable_stream(rs.stream.color, 640, 360, rs.format.rgb8, 30) profile = pipe.start(config) # Declare filters dc_ftr = rs.decimation_filter () # Decimation - reduces depth frame density st_ftr = rs.spatial_filter() # Spatial - edge-preserving spatial smoothing tp_ftr = rs.temporal_filter() # Temporal - reduces temporal noise th_ftr = rs.threshold_filter() align_to = rs.stream.color align = rs.align(align_to) rgbd = np.zeros((1,4,360,640)) i=0 print('################|Initialization seguence completed.|################') try: while(1): # Get frameset of color and depth frames = pipe.wait_for_frames() # Align the depth frame to color frame aligned_frames = align.process(frames) # Get aligned frames aligned_depth_frame = aligned_frames.get_depth_frame() # 640x480 color_frame = aligned_frames.get_color_frame() # Validate that both frames are valid if not aligned_depth_frame or not color_frame: continue # Post processing filtered = tp_ftr.process(st_ftr.process(dc_ftr.process(aligned_depth_frame))) filtered = th_ftr.process(filtered)#can be removed if person is outside frame rgb = np.asanyarray(color_frame.get_data()) rgb = np.transpose(rgb,(2,0,1)) depth = np.asanyarray(aligned_depth_frame.get_data(),dtype=np.uint16) depth = depth im.show(rgb,args.bb) im.rgbsave(i, rgb, str('./' + args.out_rdir)) im.dsave(i, depth, str('./' + args.out_ddir)) np.savetxt('./' + args.annotations_dir + '/' + str(i) + '.csv', args.bb, delimiter=',') i = i+ 1 print('saved'+str(i)+'image') # time.sleep(1) finally: pipe.stop() print('################| Error in pipeline |################')
def start(self): try: # Setup: pipe = rs.pipeline() #configure profiles config = rs.config() config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 6) config.enable_stream(rs.stream.color, 640, 480, rs.format.rgb8, 6) profile = pipe.start(config) # Skip 5 first frames to give the Auto-Exposure time to adjust for x in range(5): pipe.wait_for_frames() while True: # Store next frameset for later processing: frameset = pipe.wait_for_frames() color_frame = frameset.get_color_frame() color = np.asanyarray(color_frame.get_data()) # Standard OpenCV boilerplate for running the net:# Stand height, width = color.shape[:2] expected = 300 aspect = width / height resized_image = cv2.resize( color, (int(round(expected * aspect)), expected)) crop_start = int(round(expected * (aspect - 1) / 2)) crop_img = resized_image[0:expected, crop_start:crop_start + expected] net = cv2.dnn.readNetFromCaffe( "MobileNetSSD_deploy.prototxt", "MobileNetSSD_deploy.caffemodel") in_scale_factor = 0.007843 mean_val = 127.53 class_names = ("background", "aeroplane", "bicycle", "bird", "boat", "bottle", "bus", "car", "cat", "chair", "cow", "diningtable", "dog", "horse", "motorbike", "person", "pottedplant", "sheep", "sofa", "train", "tvmonitor") blob = cv2.dnn.blobFromImage(crop_img, in_scale_factor, (expected, expected), mean_val, False) net.setInput(blob, "data") detections = net.forward("detection_out") label = detections[0, 0, 0, 1] xmin = detections[0, 0, 0, 3] ymin = detections[0, 0, 0, 4] xmax = detections[0, 0, 0, 5] ymax = detections[0, 0, 0, 6] class_name = class_names[int(label)] align = rs.align(rs.stream.color) frameset = align.process(frameset) aligned_depth_frame = frameset.get_depth_frame() depth = np.asanyarray(aligned_depth_frame.get_data()) # Crop depth data: scale = height / expected xmin_depth = int((xmin * expected + crop_start) * scale) ymin_depth = int((ymin * expected) * scale) xmax_depth = int((xmax * expected + crop_start) * scale) ymax_depth = int((ymax * expected) * scale) depth = depth[xmin_depth:xmax_depth, ymin_depth:ymax_depth].astype(float) # Get data scale from the device and convert to meters depth_scale = profile.get_device().first_depth_sensor( ).get_depth_scale() depth = depth * depth_scale dist, _, _, _ = cv2.mean(depth) print("Detected a {0} {1:.3} meters away.".format( class_name, dist)) json_object = json.dumps(DataObject(class_name, dist), default=jsonDefault) self.send_to_hub_callback(json_object) except Exception as e: print(e) pass
def run(self): # ======================== # 1. Configure all streams # ======================== self.rspipeline = rs.pipeline() config = rs.config() config.enable_stream(rs.stream.depth, self.width, self.height, rs.format.z16, 30) config.enable_stream(rs.stream.color, self.width, self.height, rs.format.bgr8, 30) # ====================== # 2. Start the streaming # ====================== print("Starting up the Intel Realsense...") print("") profile = self.rspipeline.start(config) # Load the configuration here self.loadConfiguration(profile, self.json_file) # ================================= # 3. The depth sensor's depth scale # ================================= depth_sensor = profile.get_device().first_depth_sensor() depth_scale = depth_sensor.get_depth_scale() print("Depth Scale is: ", depth_scale) print("") # ========================================== # 4. Create an align object. # Align the depth image to the rgb image. # ========================================== align_to = rs.stream.depth align = rs.align(align_to) try: # =========================================== # 5. Skip the first 30 frames. # This gives the Auto-Exposure time to adjust # =========================================== for x in range(30): frames = self.rspipeline.wait_for_frames() # Align the depth frame to color frame aligned_frames = align.process(frames) print("Intel Realsense started successfully.") print("") # =========================================== # 6. Setup gstreamer # Usefull gst resources / cheatsheets # https://github.com/matthew1000/gstreamer-cheat-sheet/blob/master/rtmp.md # http://wiki.oz9aec.net/index.php/Gstreamer_cheat_sheet # https://github.com/matthew1000/gstreamer-cheat-sheet/blob/master/mixing.md # =========================================== CLI = '' caps = 'caps="video/x-raw,format=BGR,width=' + str( self.width ) + ',height=' + str( self.height * 2 ) + ',framerate=(fraction)30/1,pixel-aspect-ratio=(fraction)1/1"' if platform.system() == "Linux": #assuming Linux means RPI CLI = 'flvmux name=mux streamable=true latency=3000000000 ! rtmpsink location="' + self.rtmp_url + ' live=1 flashver=FME/3.0%20(compatible;%20FMSc%201.0)" \ appsrc name=mysource format=TIME do-timestamp=TRUE is-live=TRUE ' + str( caps) + ' ! \ videoconvert ! omxh264enc ! h264parse ! video/x-h264 ! \ queue max-size-buffers=0 max-size-bytes=0 max-size-time=180000000 min-threshold-buffers=1 leaky=upstream ! mux. \ alsasrc ! audio/x-raw, format=S16LE, rate=44100, channels=1 ! voaacenc bitrate=44100 ! aacparse ! audio/mpeg, mpegversion=4 ! \ queue max-size-buffers=0 max-size-bytes=0 max-size-time=4000000000 min-threshold-buffers=1 ! mux.' elif platform.system() == "Darwin": #macos #CLI='flvmux name=mux streamable=true ! rtmpsink location="'+ self.rtmp_url +' live=1 flashver=FME/3.0%20(compatible;%20FMSc%201.0)" \ # appsrc name=mysource format=TIME do-timestamp=TRUE is-live=TRUE '+ str(caps) +' ! \ # videoconvert ! vtenc_h264 ! video/x-h264 ! h264parse ! video/x-h264 ! \ # queue max-size-buffers=4 ! flvmux name=mux. \ # osxaudiosrc do-timestamp=true ! audioconvert ! audioresample ! audio/x-raw,rate=48000 ! faac bitrate=48000 ! audio/mpeg ! aacparse ! audio/mpeg, mpegversion=4 ! \ # queue max-size-buffers=4 ! mux.' CLI = 'appsrc name=mysource format=TIME do-timestamp=TRUE is-live=TRUE caps="video/x-raw,format=BGR,width=' + str( self.width ) + ',height=' + str( self.height * 2 ) + ',framerate=(fraction)30/1,pixel-aspect-ratio=(fraction)1/1" ! videoconvert ! vtenc_h264 ! video/x-h264 ! h264parse ! video/x-h264 ! queue max-size-buffers=4 ! flvmux name=mux ! rtmpsink location="' + self.rtmp_url + '" sync=true osxaudiosrc do-timestamp=true ! audioconvert ! audioresample ! audio/x-raw,rate=48000 ! faac bitrate=48000 ! audio/mpeg ! aacparse ! audio/mpeg, mpegversion=4 ! queue max-size-buffers=4 ! mux.' #TODO: windows print(CLI) self.gstpipe = Gst.parse_launch(CLI) self.appsrc = self.gstpipe.get_by_name("mysource") self.appsrc.set_property('emit-signals', True) #tell sink to emit signals # Set up a pipeline bus watch to catch errors. self.bus = self.gstpipe.get_bus() self.bus.connect("message", self.on_bus_message) self.gstpipe.set_state(Gst.State.PLAYING) intrinsics = True buff = Gst.Buffer.new_allocate(None, self.width * self.height * 3 * 2, None) buff_depth_index = (self.width * self.height * 3) - 1 hsv = np.zeros((self.height, self.width, 3), dtype=np.float32) hsv8 = np.zeros((self.height, self.width, 3), dtype=np.int8) depth_hsv = np.zeros((self.height, self.width, 3), dtype=np.float32) depth_image = np.zeros((self.height, self.width, 3), dtype=np.float32) start = timer() while not self.exit.is_set(): # ====================================== # 7. Wait for a coherent pair of frames: # ====================================== frames = self.rspipeline.wait_for_frames(1000) framestart = timer() # ======================================= # 8. Align the depth frame to color frame # ======================================= aligned_frames = align.process(frames) # ================================================ # 9. Fetch the depth and colour frames from stream # ================================================ depth_frame = aligned_frames.get_depth_frame() color_frame = aligned_frames.get_color_frame() if not depth_frame or not color_frame: pass # print the camera intrinsics just once. it is always the same if intrinsics: print("Intel Realsense Camera Intrinsics: ") print("========================================") print(depth_frame.profile.as_video_stream_profile(). intrinsics) print(color_frame.profile.as_video_stream_profile(). intrinsics) print("") intrinsics = False # ===================================== # 10. Apply filtering to the depth image # ===================================== # Apply a spatial filter without hole_filling (i.e. holes_fill=0) # depth_frame = self.spatial_filtering(depth_frame, magnitude=2, alpha=0.5, delta=10, holes_fill=1) # Apply hole filling filter # depth_frame = self.hole_filling(depth_frame) # ================================== # 11. Convert images to numpy arrays # ================================== depth_image = np.asanyarray(depth_frame.get_data()).astype( np.float32) color_image = np.asanyarray(color_frame.get_data()) # ====================================================================== # 12. Conver depth to hsv # ================================== # We need to encode/pack the 16bit depth value to RGB # we do this by treating it as the Hue in HSV. # we then encode HSV to RGB and stream that # on the other end we reverse RGB to HSV, H will give us the depth value back. # HSV elements are in the 0-1 range so we need to normalize the depth array to 0-1 # First set a far plane and set everything beyond that to 0 clipped = depth_image > 4000 depth_image[clipped] = 0 # Now normalize using that far plane # cv expects the H in degrees, not 0-1 :( depth_image *= (360 / 4000) depth_hsv[:, :, 0] = depth_image depth_hsv[:, :, 1] = 1 depth_hsv[:, :, 2] = 1 discard = depth_image == 0 s = depth_hsv[:, :, 1] v = depth_hsv[:, :, 2] s[discard] = 0 v[discard] = 0 # cv2.cvtColor to convert HSV to RGB hsv = cv2.cvtColor(depth_hsv, cv2.COLOR_HSV2BGR_FULL) # cv2 needs hsv to 8bit (0-255) to stack with the color image hsv8 = (hsv * 255).astype(np.uint8) buff.fill(0, color_image.tobytes()) buff.fill(buff_depth_index, hsv8.tobytes()) self.appsrc.emit("push-buffer", buff) #process any messages from gstreamer msg = self.bus.pop_filtered(Gst.MessageType.ERROR | Gst.MessageType.WARNING | Gst.MessageType.EOS | Gst.MessageType.INFO | Gst.MessageType.STATE_CHANGED) while (msg): self.on_bus_message(msg) msg = self.bus.pop_filtered( Gst.MessageType.ERROR | Gst.MessageType.WARNING | Gst.MessageType.EOS | Gst.MessageType.INFO | Gst.MessageType.STATE_CHANGED) if (not self.exit.is_set()): try: if (not self.previewQueue.full()): self.previewQueue.put_nowait((color_image, hsv8)) except: pass msgprocesstime = timer() print("gstreamer fps:%s d:%s" % (str(1 / (msgprocesstime - start)), str(framestart - start))) start = timer() except: e = sys.exc_info()[0] print("Unexpected Error: %s" % e) self.statusQueue.put_nowait("ERROR: Unexpected Error: %s" % e) finally: # Stop streaming print("Stop realsense pipeline") self.rspipeline.stop() print("Pause gstreamer pipe") try: if (self.gstpipe.get_state()[1] is not Gst.State.PAUSED): self.gstpipe.set_state(Gst.State.PAUSED) except: self.statusQueue.put_nowait("ERROR: Error pausing gstreamer") print("Error pausing gstreamer") self.statusQueue.put_nowait("INFO: Exiting Realsense Capture process") print("Exiting capture loop")
def capture(): global img_q, save w, h = 640, 480 # Configure depth and color streams pipeline = rs.pipeline() config = rs.config() config.enable_stream(rs.stream.depth, w, h, rs.format.z16, 30) config.enable_stream(rs.stream.color, w, h, rs.format.bgr8, 30) # Start streaming profile = pipeline.start(config) # Getting the depth sensor's depth scale (see rs-align example for explanation) depth_sensor = profile.get_device().first_depth_sensor() depth_scale = depth_sensor.get_depth_scale() print("Depth Scale is: " , depth_scale) # We will be removing the background of objects more than # clipping_distance_in_meters meters away clipping_distance_in_meters = 1 #1 meter clipping_distance = clipping_distance_in_meters / depth_scale # Create an align object # rs.align allows us to perform alignment of depth frames to others frames # The "align_to" is the stream type to which we plan to align depth frames. align_to = rs.stream.color align = rs.align(align_to) thr = 150 l = 10 while True: frames = pipeline.wait_for_frames() aligned_frames = align.process(frames) aligned_depth_frame = aligned_frames.get_depth_frame() color_frame = aligned_frames.get_color_frame() if not aligned_depth_frame or not color_frame: continue depth_image = np.asanyarray(aligned_depth_frame.get_data()) color_image = np.asanyarray(color_frame.get_data()) grey_color = 153 out = np.zeros((h, w, 4),np.uint8) depth_image_3d = np.dstack((depth_image,depth_image,depth_image)) out[:,:,:3] = np.where((depth_image_3d > clipping_distance) | (depth_image_3d <= 0), 0, color_image) out[:,:,3] = np.where((depth_image > clipping_distance) | (depth_image <= 0), 0, 255) depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET) images = np.hstack((color_image, depth_colormap, out[:,:,:3])) if not img_q.full(): img_q.put_nowait(images) if save.value == 1: save_img(out) save.value = 0
def camera_process(_running, _collision, _buffer): FRAME_WIDTH = 640 FRAME_HEIGHT = 480 pipeline = rs.pipeline() config = rs.config() config.enable_stream(rs.stream.color, FRAME_WIDTH, FRAME_HEIGHT, rs.format.bgr8, 30) config.enable_stream(rs.stream.depth, FRAME_WIDTH, FRAME_HEIGHT, rs.format.z16, 30) config.enable_stream(rs.stream.infrared, 1, FRAME_WIDTH, FRAME_HEIGHT, rs.format.y8, 30) config.enable_stream(rs.stream.infrared, 2, FRAME_WIDTH, FRAME_HEIGHT, rs.format.y8, 30) print("[INFO] Starting streaming...") profile = pipeline.start(config) depth_sensor = profile.get_device().first_depth_sensor() depth_scale = depth_sensor.get_depth_scale() align_to = rs.stream.color align = rs.align(align_to) clipping_distance_in_meters = 1.5 collision_percent = 0.15 clipping_distance = clipping_distance_in_meters / depth_scale while True: frames = pipeline.wait_for_frames() aligned_frames = align.process(frames) # Get aligned frames aligned_depth_frame = aligned_frames.get_depth_frame( ) # aligned_depth_frame is a 640x480 depth image color_frame = aligned_frames.get_color_frame() frame = frames.get_color_frame() depth_frame = aligned_frames.get_depth_frame( ) # aligned_depth_frame is a 640x480 depth image color_frame = aligned_frames.get_color_frame() ir_frame_1 = frames.get_infrared_frame(1) ir_frame_2 = frames.get_infrared_frame(2) ir_frame_both = frames.get_infrared_frame() # Convert images to numpy arrays color_image = np.asanyarray(color_frame.get_data()) depth_image = np.asanyarray(aligned_depth_frame.get_data()) grey_color = 0 depth_image_3d = np.dstack( (depth_image, depth_image, depth_image)) #depth image is 1 channel, color is 3 channels ir_image_1 = np.asanyarray(ir_frame_1.get_data()) ir_image_1 = cv2.applyColorMap( cv2.convertScaleAbs(ir_image_1, alpha=1), cv2.COLOR_BGRA2GRAY) ir_image_2 = np.asanyarray(ir_frame_2.get_data()) ir_image_2 = cv2.applyColorMap( cv2.convertScaleAbs(ir_image_2, alpha=1), cv2.COLOR_BGRA2GRAY) depth_image = np.asanyarray(depth_frame.get_data()) depth_colormap = cv2.applyColorMap( cv2.convertScaleAbs(depth_image, alpha=0.02), cv2.COLORMAP_JET) scaled_size = (frame.width, frame.height) # expand image dimensions to have shape: [1, None, None, 3] # i.e. a single-column array, where each item in the column has the pixel RGB value image_expanded = np.expand_dims(color_image, axis=0) frame = cv2.cvtColor(np.asanyarray(frame.get_data()), cv2.COLOR_BGR2RGB) bg_removed = np.where( (depth_image_3d > clipping_distance) | (depth_image_3d <= 0), grey_color, ir_image_1) bg_removed[bg_removed != [0, 0, 0]] = 255 white_count = np.count_nonzero(bg_removed == 255) black_count = np.count_nonzero(bg_removed == 0) pixels = bg_removed.shape[0] * bg_removed.shape[1] * bg_removed.shape[2] if (white_count > pixels * collision_percent): _collision.value = True else: _collision.value = False cv2.imshow("bg_removed", bg_removed) t = cv2.waitKey(1) if t == ord('q'): _running.value = False break # _, _buffer.value = cv2.imencode('.jpg', bg_removed) print(type(base64.b64encode(cv2.imencode('.jpg', bg_removed)[1]))) try: _buffer.value = str( base64.b64encode(cv2.imencode('.jpg', bg_removed)[1])) except Exception as e: print(e)
def init(self): self.config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30) self.config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30) # Start pipeline attempt = 1 # 尝试次数,成功则为-1 while (attempt > 0): try: profile = self.pipeline.start(self.config) except (RuntimeError): print("\033[0;31m未检测到相机!重试%s/5\033[0m" % (attempt)) attempt += 1 if attempt > 5: os._exit(0) else: attempt = 0 while (attempt >= 0): try: self.frames = self.pipeline.wait_for_frames() except (RuntimeError): print("\033[0;31m相机卡死!重试%s/5\033[0m" % attempt) attempt += 1 if attempt > 5: os._exit(0) else: attempt = -1 color_frame = self.frames.get_color_frame() depth_frame = self.frames.get_depth_frame() # 设置相机参数 depth_sensor = profile.get_device().first_depth_sensor() depth_sensor.set_option(rs.option.motion_range, 140) # 最远深度 220 depth_sensor.set_option(rs.option.accuracy, 3) # max=3 depth_sensor.set_option(rs.option.filter_option, 7) # 7 depth_sensor.set_option(rs.option.confidence_threshold, 15) # 15 self.depth_scale = depth_sensor.get_depth_scale() # 用于将深度图像转为以米为单位 # Color Intrinsics intr = color_frame.profile.as_video_stream_profile().intrinsics camera_parameters = { 'fx': intr.fx, 'fy': intr.fy, 'ppx': intr.ppx, 'ppy': intr.ppy, 'height': intr.height, 'width': intr.width } with open('intrinsics.json', 'w') as fp: json.dump(camera_parameters, fp) depth_intr = depth_frame.profile.as_video_stream_profile().intrinsics depth_parameters = { 'fx': depth_intr.fx, 'fy': depth_intr.fy, 'ppx': depth_intr.ppx, 'ppy': depth_intr.ppy, 'height': depth_intr.height, 'width': depth_intr.width } with open('depth_intrinsics.json', 'w') as fp: json.dump(depth_parameters, fp) depth_to_color_extrin = depth_frame.profile.get_extrinsics_to( color_frame.profile) extrin_parameters = { 'rot': depth_to_color_extrin.rotation, 'tran': depth_to_color_extrin.translation } with open('extrinsics.json', 'w') as fp: json.dump(extrin_parameters, fp) align_to = rs.stream.color self.align = rs.align(align_to) for i in range(30): self.frames = self.pipeline.wait_for_frames()