def __init__(self, filters=[]): """ Connect to RealSense and initialize filters :param filters: [String, ...], default=[]: '' TODO list filters """ self.pipe = rs.pipeline() cfg = rs.config() profile = self.pipe.start(cfg) # camera parameters self.depth_scale = profile.get_device().first_depth_sensor( ).get_depth_scale() # filters to apply to depth images self.filters = filters if 'align' in self.filters: self.align = rs.align(rs.stream.color) if 'decimation' in self.filters: self.decimation = rs.decimation_filter() self.decimation.set_option(rs.option.filter_magnitude, 4) if 'spatial' in self.filters: self.spatial = rs.spatial_filter() # self.spatial.set_option(rs.option.holes_fill, 3) self.spatial.set_option(rs.option.filter_magnitude, 5) self.spatial.set_option(rs.option.filter_smooth_alpha, 1) self.spatial.set_option(rs.option.filter_smooth_delta, 50) if 'temporal' in self.filters: # TODO self.temporal = rs.temporal_filter() self.temporal_iters = 3 if 'hole_filling' in self.filters: self.hole_filling = rs.hole_filling_filter() if 'colorize' in self.filters: self.colorizer = rs.colorizer()
def __init__(self, parameters, neural): self.parameters = parameters self.neural = neural self.stream_frame = None config = rs.config() self.pipeline = rs.pipeline() config.enable_stream(rs.stream.depth, self.parameters.depth_width, self.parameters.depth_height, rs.format.z16, self.parameters.depth_fps) config.enable_stream(rs.stream.color, self.parameters.color_width, self.parameters.color_height, rs.format.rgb8, self.parameters.color_fps) profile = self.pipeline.start(config) self.depth_sensor = profile.get_device().first_depth_sensor( ).get_depth_scale() self.align_stream = rs.align(rs.stream.color) self.decimation = rs.decimation_filter() self.hole_filling = rs.hole_filling_filter() self.spatial = rs.spatial_filter() self.neural = neural print("camera loop has stared") threading.Thread.__init__(self)
def get_frame_stream(self): # Wait for a coherent pair of frames: depth and color frames = self.pipeline.wait_for_frames() aligned_frames = self.align.process(frames) depth_frame = aligned_frames.get_depth_frame() color_frame = aligned_frames.get_color_frame() if not depth_frame or not color_frame: # If there is no frame, probably camera not connected, return False print( "Error, impossible to get the frame, make sure that the Intel Realsense camera is correctly connected" ) print("Depth frame: " + str(bool(depth_frame)) + " Color: " + str(bool(color_frame))) return False, None, None # Apply filter to fill the Holes in the depth image spatial = rs.spatial_filter() spatial.set_option(rs.option.holes_fill, 3) filtered_depth = spatial.process(depth_frame) hole_filling = rs.hole_filling_filter() filled_depth = hole_filling.process(filtered_depth) # Create colormap to show the depth of the Objects colorizer = rs.colorizer() depth_colormap = np.asanyarray( colorizer.colorize(filled_depth).get_data()) # Convert images to numpy arrays depth_image = np.asanyarray(filled_depth.get_data()) color_image = np.asanyarray(color_frame.get_data()) return True, color_image, depth_frame
def filtering(self, frame): '''Filter setting''' # Decimation # decimation = rs.decimation_filter() decimation.set_option(rs.option.filter_magnitude, 1) # Spatial # spatial = rs.spatial_filter() # spatial.set_option(rs.option.filter_magnitude, 5) spatial.set_option(rs.option.filter_smooth_alpha, 0.6) spatial.set_option(rs.option.filter_smooth_delta, 8) # spatial.set_option(rs.option.holes_fill, 3) # Temporal # temporal = rs.temporal_filter() temporal.set_option(rs.option.filter_smooth_alpha, 0.5) temporal.set_option(rs.option.filter_smooth_delta, 20) # Hole # hole_filling = rs.hole_filling_filter() ## depth_to_disparity = rs.disparity_transform(True) disparity_to_depth = rs.disparity_transform(False) '''Appling filter''' frame = decimation.process(frame) frame = depth_to_disparity.process(frame) frame = spatial.process(frame) frame = temporal.process(frame) frame = disparity_to_depth.process(frame) frame = hole_filling.process(frame) return frame
def __init__(self): # Initialize RealSense intrinsics self.diagonal = np.linalg.norm((self.W, self.H)) self.pipeline = rs.pipeline() self.aligner = rs.align(rs.stream.color) self.config = rs.config() self.config.enable_stream(rs.stream.depth, 640, 360, rs.format.z16, 30) self.config.enable_stream(rs.stream.color, self.W, self.H, rs.format.bgr8, 30) self.temporal_filter = rs.temporal_filter() self.hole_filling_filter = rs.hole_filling_filter() # Start up camera profile = self.pipeline.start(self.config) # Set camera options sensor = profile.get_device().first_depth_sensor() sensor.set_option(rs.option.enable_auto_exposure, 1) # sensor.set_option(rs.option.exposure, 5000) # Acquire an initial set of frames used for calibration # Flush 10 frames to get the Intel temporal filter warmed up for i in range(30): self.__acquire_raw_aligned() # Save a snapshot of the background for later subtraction, blur it for denoising purposes self.__depth_background = ndimage.gaussian_filter( self.__depth_raw_aligned, 20)
def __init__(self, w=640, h=480, clipping_dist_meters=1): self.w, self.h = w, h # Create a pipeline self.pipeline = rs.pipeline() # Create a config and configure the pipeline to stream 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 = self.pipeline.start(config) # Getting the depth sensor's depth scale (see rs-align example for explanation) depth_sensor = profile.get_device().first_depth_sensor() self.depth_scale = depth_sensor.get_depth_scale() print("Depth Scale is: ", self.depth_scale) # We will be removing the background of objects more than # clipping_distance_in_meters meters away self.clipping_distance = clipping_dist_meters / self.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) #set up depth post-processing filters self.decimation_filter = rs.decimation_filter() #default is 2 self.spatial_filter = rs.spatial_filter(smooth_alpha=.6, smooth_delta=20, magnitude=2, hole_fill=0) self.hole_fill_filter = rs.hole_filling_filter( ) #default is fill according to neighboring pixel farthest from sensor
def __init__(self, cam_id, filter_depth=True, frame=None, registration_mode=RealSenseRegistrationMode.DEPTH_TO_COLOR): self._running = None self.id = cam_id self._registration_mode = registration_mode self._filter_depth = filter_depth self._frame = frame if self._frame is None: self._frame = 'realsense' self._color_frame = '%s_color' % (self._frame) # realsense objects self._pipe = rs.pipeline() self._cfg = rs.config() self._align = rs.align(rs.stream.color) # camera parameters self._depth_scale = None self._intrinsics = np.eye(3) # post-processing filters self._colorizer = rs.colorizer() self._spatial_filter = rs.spatial_filter() self._hole_filling = rs.hole_filling_filter()
def __init__(self): ctx = rs.context() self.devices = ctx.query_devices() self.configs = list() self.filters = list() for device in self.devices: config = rs.config() config.enable_device(device.get_info(rs.camera_info.serial_number)) config.enable_stream(rs.stream.depth, IMG_WIDTH, IMG_HEIGHT, rs.format.z16, 30) config.enable_stream(rs.stream.color, IMG_WIDTH, IMG_HEIGHT, rs.format.bgr8, 30) self.configs.append(config) align = rs.align(rs.stream.color) spatial = rs.spatial_filter() spatial.set_option(rs.option.filter_magnitude, 5) spatial.set_option(rs.option.filter_smooth_alpha, 1) spatial.set_option(rs.option.filter_smooth_delta, 50) spatial.set_option(ts.option.holes_fill, 3) temporal = rs.temporal_filter() hole_filling = rs.hole_filling_filter() depth_to_disparity = rs.disparity_transform(True) disparity_to_depth = rs.disparity_transform(False) decimate = rs.decimation_filter() self.filters.append({ 'align': align, 'spatial': spatial, 'temporal': temporal, 'hole': hole_filling, 'disparity': depth_to_disparity, 'depth': disparity_to_depth, 'decimate': decimate })
def __init__(self): self.filters = [ rs.decimation_filter(RESCALE), rs.disparity_transform(True), rs.hole_filling_filter(1), rs.spatial_filter(0.5, 8, 2, 2), rs.temporal_filter(0.5, 20, 1), rs.disparity_transform(False) ]
def __init__(self): self.pipeline = rs.pipeline() config = rs.config() config.enable_stream(rs.stream.depth, REALSENSE_WIDTH, REALSENSE_HEIGHT, rs.format.z16, 30) config.enable_stream(rs.stream.color, REALSENSE_WIDTH, REALSENSE_HEIGHT, rs.format.bgr8, 30) self.colorizer = rs.colorizer() self.hole_filler = rs.hole_filling_filter() self.profile = self.pipeline.start(config) # Skip 5 first frames to give the Auto-Exposure time to adjust for x in range(5): self.pipeline.wait_for_frames()
def refresh_mat(self): self.frames = pipeline.wait_for_frames() self.depth = self.frames.get_depth_frame() self.color = self.frames.get_color_frame() hole_filling = rs.hole_filling_filter() self.depth = hole_filling.process(self.depth) # depth_profile = depth.get_profile() # color_profile = color.get_profile() # print(depth_profile) # print(color_profile) self.depthmat = np.asanyarray(self.depth.get_data()) self.colormat = np.asanyarray(self.color.get_data())
def GetStandardDeviationsFromBag(bag_file_path, frame_index_difference = 10, do_analysis_every_n_frames = 1, bag_timeout_ms = 500, filter=False): try: pipeline = rs.pipeline() config = rs.config() rs.config.enable_device_from_file(config, bag_file_path, repeat_playback=False) profile = pipeline.start(config).get_device().as_playback().set_real_time(False) depth_frames_deque = deque() SDs = [] FNs = [] all_frame_numbers = [] frames_since_last_analysis = 0 if filter: spatial = rs.spatial_filter() decimation = rs.decimation_filter() hole_filling = rs.hole_filling_filter() hole_filling.set_option(rs.option.holes_fill, 2) while True: frames = pipeline.wait_for_frames(timeout_ms=bag_timeout_ms) fn = frames.frame_number all_frame_numbers += [fn] frames_since_last_analysis += 1 cur_depth_frame = frames.get_depth_frame() if filter: cur_depth_frame = decimation.process(cur_depth_frame) cur_depth_frame = spatial.process(cur_depth_frame) cur_depth_frame = hole_filling.process(cur_depth_frame) depth_frames_deque.append(cur_depth_frame) if len(depth_frames_deque) > frame_index_difference: cur_depth_image = np.asanyarray(cur_depth_frame.get_data()) past_depth_image = np.asanyarray(depth_frames_deque.popleft().get_data()) if frames_since_last_analysis >= do_analysis_every_n_frames: SDs += [calculateSD(cur_depth_image,past_depth_image)] FNs += [fn] frames_since_last_analysis = 0 except Exception as e: print(e) if "arrive" in str(e): pipeline.stop() del(pipeline) del(profile) return np.array(all_frame_numbers),np.array(FNs),np.array(SDs) else: raise(e) pass finally: return np.array(all_frame_numbers),np.array(FNs),np.array(SDs)
def start_pipe(self, align=True, usb3=True): if not self.pipelineStarted: if align: print('Etablissement de la connection caméra') # Create a config and configure the pipeline to stream # different resolutions of color and depth streams self.pipeline = rs.pipeline() # Create a config and configure the pipeline to stream # different resolutions of color and depth streams config = rs.config() config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30) config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30) # Start streaming self.profile = self.pipeline.start(config) align_to = rs.stream.color self.align = rs.align(align_to) time.sleep(1) # self.pipeline = rs.pipeline() # config = rs.config() # # if usb3: # config.enable_stream(rs.stream.depth, 640, 360, rs.format.z16, 30) # config.enable_stream(rs.stream.color, 640, 480, rs.format.rgb8, 30) # # else: # self.profile = config.resolve(self.pipeline) # does not start streaming # # self.profile = self.pipeline.start(config) # self.pipelineStarted = True # # Align the two streams # align_to = rs.stream.color # self.align = rs.align(align_to) self.pipelineStarted = True # Get depth scale depth_sensor = self.profile.get_device().first_depth_sensor() self.depth_scale = depth_sensor.get_depth_scale() # Création des filtres self.hole_filling = rs.hole_filling_filter() self.temporal_filter = rs.temporal_filter() self.spatial_filter = rs.spatial_filter() self.depth_to_disparity = rs.disparity_transform(False) # Get Intrinsic parameters self.get_intrinsic() print('Caméra Ouverte')
def depth_filter(depth_frame): depth_to_disparity = rs.disparity_transform(True) disparity_to_depth = rs.disparity_transform(False) hole_filling = rs.hole_filling_filter(2) spatial = rs.spatial_filter() depth_frame = depth_to_disparity.process(depth_frame) depth_frame = spatial.process(depth_frame) # frame = temporal.process(frame) depth_frame = disparity_to_depth.process(depth_frame) depth_frame = hole_filling.process(depth_frame) return depth_frame
def init_video_capture(self): try: # Create a pipeline self.pipeline = rs.pipeline() # Create a config and configure the pipeline to stream # different resolutions of color and depth streams config = rs.config() config.enable_stream(rs.stream.depth, DEPTH_RES_X, DEPTH_RES_Y, rs.format.z16, DEPTH_FPS) config.enable_stream(rs.stream.color, RGB_RES_X, RGB_RES_Y, rs.format.bgr8, RGB_FPS) # config.enable_stream(rs.stream.infrared, 1, DEPTH_RES_X, DEPTH_RES_Y, rs.format.y8, DEPTH_FPS) # config.enable_stream(rs.stream.infrared, 2, DEPTH_RES_X, DEPTH_RES_Y, rs.format.y8, DEPTH_FPS) # Start streaming profile = self.pipeline.start(config) # Getting the depth sensor's depth scale (see rs-align example for explanation) depth_sensor = profile.get_device().first_depth_sensor() self.depth_scale = depth_sensor.get_depth_scale() # print("[RealSense D435]: Depth scale", self.depth_scale) # TODO: Allow settings to be changed on initializing the function depth_sensor.set_option(rs.option.laser_power, 360) # 0 - 360 depth_sensor.set_option( rs.option.depth_units, 0.001) # Number of meters represented by a single depth unit # 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) self.hole_filling_filter = rs.hole_filling_filter() self.decimation_filter = rs.decimation_filter() self.temporal_filter = rs.temporal_filter() except Exception as e: print('[RealsenseD435Camera]: ERROR:', e, file=sys.stderr) print( '[RealsenseD435Camera]: Could not initialize camera. If the resource is busy, check if any other script ' 'is currently accessing the camera. If this is not the case, replug the camera and try again.', file=sys.stderr) sys.exit(0) self.init_colorizer() self.started = False self.read_lock = threading.Lock()
def filtering(self): depth_to_disparity = rs.disparity_transform(True) disparity_to_depth = rs.disparity_transform(False) spatial = rs.spatial_filter() temporal = rs.temporal_filter() hole_filling = rs.hole_filling_filter() for frame in self.depth_frams: frame = depth_to_disparity.process(frame) frame = spatial.process(frame) frame = temporal.process(frame) frame = disparity_to_depth.process(frame) frame = hole_filling.process(frame) self.aligned_depth_frame = frame.get_data() self.colorized_depth = np.asanyarray( self.colorizer.colorize(frame).get_data())
def post_processing(frame, enable_spatial=True, enable_temporal=True, enable_hole=True, spatial_params=[(rs.option.filter_magnitude, 5), (rs.option.filter_smooth_alpha, 1), (rs.option.filter_smooth_delta, 50), (rs.option.holes_fill, 3)], temporal_params=[], hole_params=[]): """Filters to cleanup depth maps. """ # Filters and settings depth_to_disparity = rs.disparity_transform(True) disparity_to_depth = rs.disparity_transform(False) # Depth to disparity before spatial and temporal filters frame = depth_to_disparity.process(frame) # Spatial filter if enable_spatial: # Settings spatial = rs.spatial_filter() for spatial_param in spatial_params: spatial.set_option(spatial_param[0], spatial_param[1]) # Apply on frame frame = spatial.process(frame) # Temporal filter if enable_temporal: temporal = rs.temporal_filter() for temporal_param in temporal_params: temporal.set_option(temporal_param[0], temporal_param[1]) frame = temporal.process(frame) # Back to depth frame = disparity_to_depth.process(frame) # Hole filling if enable_hole: hole_filling = rs.hole_filling_filter() for hole_param in hole_params: hole_filling.set_option(hole_param[0], hole_param[1]) frame = hole_filling.process(frame) return frame
def __init__(self, configFile): self.config = json.load(open(configFile)) self.advanced_cfg = json.load(open(self.config["advanced"])) self.advanced_cfg = str(self.advanced_cfg).replace("'", '\"') self._running = None, self.id = self.config["id"] self._pipe = rs.pipeline() self._rs_cfg = rs.config() self._align = rs.align(rs.stream.color) self._intrinsics = {} ctx = rs.context() self._spatial_filter = rs.spatial_filter() self._hole_filter = rs.hole_filling_filter() self.depth_cam = True # prints out cams and S/N, which should be defined as id in config for d in ctx.devices: logger.info("Cam {d} connected.", d=d)
def __init__(self): # --------------------------------------------------------- # real sense # --------------------------------------------------------- # # ストリーム(Depth/Color)の設定 print("start Depth camera") width: int = 640 height: int = 480 config = rs.config() config.enable_stream(rs.stream.color, width, height, rs.format.bgr8, 30) config.enable_stream(rs.stream.depth, width, height, rs.format.z16, 30) # self.colorizer = rs.colorizer() #temporal filter self.temporal = rs.temporal_filter() #hole filling filter self.hole_filling = rs.hole_filling_filter() # ストリーミング開始 self.pipeline = rs.pipeline() profile = self.pipeline.start(config) # Skip 5 first frames to give the Auto-Exposure time to adjust for x in range(5): self.pipeline.wait_for_frames() #get device information depth_sensor = profile.get_device().first_depth_sensor() #print("depth sensor:",depth_sensor) self.depth_scale = depth_sensor.get_depth_scale() #print("depth scale:",depth_scale) clipping_distance_in_meters = 1.0 # meter clipping_distance = clipping_distance_in_meters / self.depth_scale print("clipping_distance:", clipping_distance) # Alignオブジェクト生成 align_to = rs.stream.color self.align = rs.align(align_to) self.frameNo = 0 self.timenow = ""
def create_filters(self): spatial = rs.spatial_filter() spatial.set_option(rs.option.filter_magnitude, 5) spatial.set_option(rs.option.filter_smooth_alpha, 1) spatial.set_option(rs.option.filter_smooth_delta, 50) spatial.set_option(rs.option.holes_fill, 3) temporal = rs.temporal_filter() hole_filling = rs.hole_filling_filter() disparity_to_depth = rs.disparity_transform(False) depth_to_disparity = rs.disparity_transform(True) filters = { "S": spatial, "T": temporal, "H": hole_filling, "DZ": disparity_to_depth, "ZD": depth_to_disparity } return filters
def getFrames(self): # フレーム待ち(Color & Depth) frames = self.pipeline.wait_for_frames() #frameNo = frames.get_frame_number() aligned_frames = self.align.process(frames) color_frame = aligned_frames.get_color_frame() depth_frame = aligned_frames.get_depth_frame() self.color_image = np.asanyarray(color_frame.get_data()) self.depth_image = np.asanyarray( self.colorizer.colorize(depth_frame).get_data()) #hole filter hole_filling = rs.hole_filling_filter() filled_depth_frame = hole_filling.process(depth_frame) self.filled_depth_values = np.asanyarray( filled_depth_frame.get_data()) #get values [cm] # print(filled_depth_values[100,100]) self.filled_depth_image = np.asanyarray( self.colorizer.colorize(filled_depth_frame).get_data())
def __init__(self, serial=None, resolution="high", calib_file=None): self._is_start = False self._frame_id = 0 self._auto_exp_skip = 30 self.calib_file = calib_file msg = "`resolution` can only be [`low` or `high`]" assert resolution in ['low', 'high'], msg # query any connected realsense if serial is None: serials = utils.rs_discover_cams() if serials: self._serial = serials[0] else: raise ValueError( "Could not find connected camera. Ensure USB 3.0 is used.") else: self._serial = serial if resolution == "low": self._height = constants.D415.HEIGHT_L.value self._width = constants.D415.WIDTH_L.value self._fps = constants.D415.FPS_L.value else: self._height = constants.D415.HEIGHT_H.value self._width = constants.D415.WIDTH_H.value self._fps = constants.D415.FPS_H.value self._resolution = (self._width, self._height) # pipeline and config self._pipe = rs.pipeline() self._cfg = rs.config() # post-processing self._spatial_filter = rs.spatial_filter() self._hole_filling = rs.hole_filling_filter() self._temporal_filter = rs.temporal_filter() # misc self._colorizer = rs.colorizer() self._align = rs.align(rs.stream.color)
def get_filter(filter_name: str, *args) -> rs2.filter_interface: """ Basically a factory for filters (Maybe change to Dict 'cause OOP) :param filter_name: The filter name :param args: The arguments for the filter :return: A filter which corresponds to the filter name with it's arguments """ if filter_name == 'decimation_filter': return rs2.decimation_filter(*args) elif filter_name == 'threshold_filter': return rs2.threshold_filter(*args) elif filter_name == 'disparity_transform': return rs2.disparity_transform(*args) elif filter_name == 'spatial_filter': return rs2.spatial_filter(*args) elif filter_name == 'temporal_filter': return rs2.temporal_filter(*args) elif filter_name == 'hole_filling_filter': return rs2.hole_filling_filter(*args) else: raise Exception(f'The filter \'{filter_name}\' does not exist!')
def post_process(image): spatial_filter = rs.spatial_filter() temporal_filter = rs.temporal_filter() hole_filling_filter = rs.hole_filling_filter() filter_magnitude = rs.option.filter_magnitude filter_smooth_alpha = rs.option.filter_smooth_alpha filter_smooth_delta = rs.option.filter_smooth_delta spatial_filter.set_option(filter_magnitude, 2) spatial_filter.set_option(filter_smooth_alpha, 0.5) spatial_filter.set_option(filter_smooth_delta, 20) temporal_filter.set_option(filter_smooth_alpha, 0.4) temporal_filter.set_option(filter_smooth_delta, 20) # Apply the filters filtered_frame = spatial_filter.process(image) filtered_frame = temporal_filter.process(filtered_frame) #filtered_frame = hole_filling_filter.process(filtered_frame) return filtered_frame
def filter_depth(frame): """ some filtering processes will greatly decrease FPS (e.g. spatial) decimation process will reduce image size exponentially reference: https://github.com/IntelRealSense/librealsense/blob/jupyter/notebooks/depth_filters.ipynb """ # setup filter processes #decimation = rs.decimation_filter() # decimation will cut frame size (exponentially) #depth_to_disparity = rs.disparity_transform(True) #spatial = rs.spatial_filter() # spatial operation is quite slow #temporal = rs.temporal_filter() #disparity_to_depth = rs.disparity_transform(False) hole_filling = rs.hole_filling_filter() # apply the filters #frame = depth_to_disparity.process(frame) #frame = spatial.process(frame) #frame = temporal.process(frame) #frame = disparity_to_depth.process(frame) frame = hole_filling.process(frame) return frame
def __init__(self, image_w=640, image_h=480, image_d=3, compress_depth=True): self.combined_array = None self.compress_depth = compress_depth self.running = True self.image_w = image_w self.image_h = image_h #self.depth, self.color = None, None #Set stream speed and resolution self.fps = 30 #start post processing self.colorizer = colorizer() self.holefiller = hole_filling_filter(1) self.spatial = spatial_filter() self.temporal = temporal_filter() #start the Realsense pipeline self.pipe = pipeline() #load the configuration cfg = config() cfg.enable_stream(stream.color, stream_index=-1, width=848, height=480, framerate=30) cfg.enable_stream(stream.depth, stream_index=-1, width=848, height=480, framerate=30) #start the pipeline profile = self.pipe.start(cfg) #set units to 100um depth_sensor = profile.get_device().first_depth_sensor() depth_sensor.set_option(option.depth_units, 0.0001)
# Configure depth and color streams pipeline = rs.pipeline() config = rs.config() config.enable_stream(rs.stream.depth, DEPTH_CAMERA_RES[0], DEPTH_CAMERA_RES[1], rs.format.z16, 30) config.enable_stream(rs.stream.color, COLOR_CAMERA_RES[0], COLOR_CAMERA_RES[1], rs.format.bgr8, 30) # zero hole filling filter spatial = rs.spatial_filter() spatial.set_option(rs.option.filter_magnitude, 5) spatial.set_option(rs.option.filter_smooth_alpha, 1) spatial.set_option(rs.option.filter_smooth_delta, 50) spatial.set_option(rs.option.holes_fill, 3) hole_filling = rs.hole_filling_filter() # Align process for realsense frames align_to = rs.stream.color align = rs.align(align_to) # Start streaming for Realsense pipeline.start(config) # Start connecting pupil tracker context = zmq.Context() req = context.socket(zmq.REQ) #open a req port to talk to pupil req.connect("tcp://%s:%s" % (addr, req_port)) req.send(b'SUB_PORT') # ask for the sub port sub_port = req.recv()
depth_scale = depth_sensor.get_depth_scale() align_to = rs.stream.color align = rs.align(align_to) # Setting up filters dec_filter = rs.decimation_filter() dec_filter.set_option(rs.option.filter_magnitude, 4) spa_filter = rs.spatial_filter() spa_filter.set_option(rs.option.filter_magnitude, 5) spa_filter.set_option(rs.option.filter_smooth_alpha, 1) spa_filter.set_option(rs.option.filter_smooth_delta, 50) spa_filter.set_option(rs.option.holes_fill, 3) tmp_filter = rs.temporal_filter() hol_fillin = rs.hole_filling_filter() dep_to_dis = rs.disparity_transform(True) dis_to_dep = rs.disparity_transform(False) # Setting up indices d1, d2 = int(480 / 4), int(640 / 4) idx, jdx = np.indices((d1, d2)) idx_back = np.clip(idx - 1, 0, idx.max()).flatten() idx_front = np.clip(idx + 1, 0, idx.max()).flatten() jdx_back = np.clip(jdx - 1, 0, jdx.max()).flatten() jdx_front = np.clip(jdx + 1, 0, jdx.max()).flatten() idx = idx.flatten() jdx = jdx.flatten() # rand_idx = np.random.choice(np.arange(idx.shape[0]), size=d1*d2, replace=False)
def capture(): framecap = 0 framecap_count = 0 start_capture = False #img = AsciiImage("lena.jpg", scalefactor=0.2) #print(img) #img.render("output.png", fontsize=8, bg_color=(20,20,20), fg_color=(255,255,255)) # Create a pipeline pipeline = rs.pipeline() #Create a config and configure the pipeline to stream # different resolutions of color and depth streams config = rs.config() config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 6) config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 6) # 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.2 #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) # Streaming loop try: while True: # Get frameset of color and depth frames = pipeline.wait_for_frames() # frames.get_depth_frame() is a 640x360 depth image # Align the depth frame to color frame 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() hole_filling = rs.hole_filling_filter() filled_depth = hole_filling.process(aligned_depth_frame) # Validate that both frames are valid if not filled_depth or not color_frame: continue depth_image = np.asanyarray(filled_depth.get_data()) color_image = np.asanyarray(color_frame.get_data()) # Remove background - Set pixels further than clipping_distance to grey grey_color = 255 depth_image_3d = np.dstack( (depth_image, depth_image, depth_image)) #depth image is 1 channel, color is 3 channels bg_removed = np.where( (depth_image_3d > clipping_distance) | (depth_image_3d <= 0), grey_color, color_image) #need to fix this bg_removed2 = np.where( (depth_image_3d > clipping_distance) | (depth_image_3d <= 0), 0, color_image) # Render images for color depth_colormap = cv2.applyColorMap( cv2.convertScaleAbs(depth_image, alpha=0.2), cv2.COLORMAP_HSV) # crop the image using array slices -- it's a NumPy array # after all! cropped = bg_removed[0:720, 280:1000] cropped2 = bg_removed2[0:720, 280:1000] ################################################################ # open cv stuff ################################################################ #color_image = np.asanyarray(color_frame.get_data()) gray = cv2.cvtColor(color_image, cv2.COLOR_BGR2GRAY) faces = faceCascade.detectMultiScale(gray, scaleFactor=1.1, minNeighbors=5, minSize=(30, 30), flags=cv2.CASCADE_SCALE_IMAGE) far = "" farColor = (0, 0, 0) ################################################################ # open cv stuff ################################################################ ############################################################################## # ASCII on term ############################################################################## img = AsciiImage( cropped2, scalefactor=0.19, ) print(img) # Draw a rectangle around the faces for (x, y, w, h) in faces: dist = aligned_depth_frame.get_distance(x, y) if (dist < clipping_distance_in_meters): #far = "range" #far #farColor=(0,255,0) start_capture = True #cv2.putText(color_image,far,(5,15),5, 1,farColor, lineType=cv2.LINE_AA) #cv2.rectangle(color_image, (x, y), (x+w, y+h),farColor, 2) ############################################################################## # saving no background images ############################################################################## if framecap < 7 and start_capture == True: framecap_count = framecap_count + 1 if framecap_count > 2: framecap_string = str(framecap) cv2.imwrite("maked_dump/" + framecap_string + ".jpg", cropped) framecap = framecap + 1 framecap_count = 0 ############################################################################## images = np.hstack((bg_removed, depth_colormap)) # create windows #cv2.namedWindow('Align Example', cv2.WINDOW_AUTOSIZE) #cv2.imshow('Align Example', images) #cv2.imshow('RealSense', color_image) key = cv2.waitKey(1) # Press esc or 'q' to close the image window if framecap == 7 or key & 0xFF == ord('q') or key == 27: #converter() cv2.destroyAllWindows() break finally: pipeline.stop()
def main(): pipeline = rs.pipeline() config = rs.config() config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30) config.enable_stream(rs.stream.color, 640, 480, rs.format.rgb8, 30) # Start streaming #queue =rs.frame_queue(50) cfg = pipeline.start(config) #, queue) align = rs.align(rs.stream.color) profile = cfg.get_stream(rs.stream.depth) vis = o3d.visualization.Visualizer() vis.create_window('PCD', width=1280, height=720) pointcloud = o3d.geometry.PointCloud() geom_added = False intrinsics = profile.as_video_stream_profile().get_intrinsics() print(intrinsics) depth_sensor = cfg.get_device().first_depth_sensor() depth_scale = depth_sensor.get_depth_scale() print("Depth Scale is: ", depth_scale) # clipping_distance_in_meters meters away clipping_distance_in_meters = 3 # 1 meter clipping_distance = clipping_distance_in_meters / depth_scale # filters # decimation filter decimation = rs.decimation_filter() decimation.set_option(rs.option.filter_magnitude, 4) # spatial filter spatial = rs.spatial_filter() spatial.set_option(rs.option.filter_magnitude, 5) spatial.set_option(rs.option.filter_smooth_alpha, 1) spatial.set_option(rs.option.filter_smooth_delta, 50) temporal = rs.temporal_filter() hole_filling = rs.hole_filling_filter() depth_to_disparity = rs.disparity_transform(True) disparity_to_depth = rs.disparity_transform(False) while True: dt0 = datetime.now() frames = pipeline.wait_for_frames() #frames = queue.wait_for_frame().as_frameset() frames = align.process(frames) profile = frames.get_profile() depth_frame = frames.get_depth_frame() # depth_frame = decimation.process(depth_frame) depth_frame = depth_to_disparity.process(depth_frame) depth_frame = spatial.process(depth_frame) depth_frame = temporal.process(depth_frame) depth_frame = disparity_to_depth.process(depth_frame) #depth_frame = hole_filling.process(depth_frame) # We will be removing the background of objects more than color_frame = frames.get_color_frame() if not depth_frame or not color_frame: continue # Convert images to numpy arrays depth_image = np.asanyarray(depth_frame.get_data()) color_image = np.asanyarray(color_frame.get_data()) img_depth = o3d.geometry.Image(depth_image) img_color = o3d.geometry.Image(color_image) rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth( img_color, img_depth, depth_trunc=clipping_distance_in_meters, convert_rgb_to_intensity=False) pinhole_camera_intrinsic = o3d.camera.PinholeCameraIntrinsic( intrinsics.width, intrinsics.height, intrinsics.fx, intrinsics.fy, intrinsics.ppx, intrinsics.ppy) pcd = o3d.geometry.PointCloud.create_from_rgbd_image( rgbd, pinhole_camera_intrinsic) pcd.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]]) pointcloud.points = pcd.points pointcloud.colors = pcd.colors #clusteringDepthImage(pointcloud) if geom_added == False: vis.add_geometry(pointcloud) geom_added = True vis.update_geometry(pointcloud) vis.poll_events() vis.update_renderer() # cv2.imshow('bgr', color_image) # key = cv2.waitKey(1) # if key == ord('q'): # break process_time = datetime.now() - dt0 print("FPS: " + str(1 / process_time.total_seconds())) pipeline.stop() cv2.destroyAllWindows() vis.destroy_window() del vis