def set_pointcloud_variable(): pc = rs.pointcloud() decimate = rs.decimation_filter() decimate.set_option(rs.option.filter_magnitude, 2**1) colorizer = rs.colorizer() return pc, decimate, colorizer
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 prepare_filters(self): # prepare post-processing filters decimate = rs.decimation_filter() decimate.set_option(rs.option.filter_magnitude, 2 ** 3) 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) colorizer = rs.colorizer() self.filters = [rs.disparity_transform(), rs.decimation_filter(), rs.spatial_filter(), rs.temporal_filter(), rs.disparity_transform(False)]
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 __init__(self, color, ip, port, topic, interval): Publisher.__init__(self, ip, port, topic) self.interval = interval self.lastUpdate = self.millis(self) pipe = rs.pipeline() cfg = rs.config() cfg.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30) cfg.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30) profile = pipe.start(cfg) depth_sensor = profile.get_device().first_depth_sensor() depth_sensor.set_option(rs.option.visual_preset, Preset.HighAccuracy) self.running = True self.color = color zero_vec = (0.0, 0.0, 0.0) self.depth_frame = zero_vec self.color_frame = zero_vec self.depth_colormap = zero_vec # Processing blocks self.pc = rs.pointcloud() self.decimate = rs.decimation_filter() #self.decimate.set_option(rs.option.filter_magnitude, 2 ** self.decimate) self.colorizer = rs.colorizer()
def __init__(self, address): asyncore.dispatcher.__init__(self) print("Launching Realsense Camera Server") self.pipeline, self.sensor = open_pipeline() self.create_socket(socket.AF_INET, socket.SOCK_STREAM) print('sending acknowledgement to', address) # reduce the resolution of the depth image using post processing self.decimate_filter = rs.decimation_filter() self.decimate_filter.set_option(rs.option.filter_magnitude, 2) self.frame_data = '' self.frame_length = 0 self.connect((MC_IP_ADDRESS, PORT)) self.packet_id = 0 self.remaining_bytes = 0 self.buffer = bytearray() self.color_image = np.array([]) self.depth_image = np.array([]) self.depth_scale = 0 self.aligned_frames = [] self.intrinsics = rs.intrinsics() # yolov3 detection result boxes = dict() boxes['detection_classes'] = [] boxes['detection_boxes'] = [] boxes['detection_scores'] = [] boxes['bgr_img'] = [] self.boxes = boxes
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 map_func(): pipe_list, DEPTH_SCALE = get_pipes() print("{} pipes created!".format(len(pipe_list))) map_point_cloud = np.zeros((1, 3), np.float32) decimate = rs.decimation_filter() while True: frameset = get_frames( pipe_list, decimate) #get the depth and pose frames from the camera stream points, current_pos = translate_frame_to_points(frameset, DEPTH_SCALE) print("points_shape: ", points.shape) if map_point_cloud.shape[0] >= MAX_POINT_COUNT: print("They don't wanna see me in the ends") map_point_cloud = map_point_cloud[(points.shape[0] - 1):-1] map_point_cloud = np.append( map_point_cloud, points, axis=0) #append the points to the overall map portrait = scan_portrait(map_point_cloud, current_pos) cv2.imshow("circumstances", portrait) if cv2.waitKey(1) & 0xFF == ord('q'): break cv2.destroyAllWindows()
def computeBackground(options, pipeline, align): count = 0 minVal = np.full( (constants.FRAME_DECIMATED_HEIGHT, constants.FRAME_DECIMATED_WIDTH), constants.MAX_FLOAT32, dtype=np.float32) decimation = rs.decimation_filter() decimation.set_option(rs.option.filter_magnitude, 4) temporal = rs.temporal_filter() while count < 60: frames = pipeline.wait_for_frames() alignedFrames = align.process(frames) depth = alignedFrames.get_depth_frame() # depth = frames.get_depth_frame() filtered_depth = decimation.process(depth) # filtered_depth = temporal.process(filtered_depth) # filtered_depth = depth d = np.asanyarray(filtered_depth.get_data()).astype(np.float32) print d.shape, d.dtype zeros = d.size - np.count_nonzero(d) print('Input:zeros:' + str(zeros) + ' total:' + str(d.size)) d[d == 0.] = constants.MAX_FLOAT32 minVal = np.minimum(minVal, d) print('Minval: zeros:' + str(minVal[minVal == constants.MAX_FLOAT32].size) + ' total:' + str(minVal.size)) count = count + 1 return inpaint(options, minVal)
def on(self): # 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, self.arg["width"], self.arg["height"], rs.format.z16, self.arg["fps"]) config.enable_stream(rs.stream.infrared, 1, self.arg["width"], self.arg["height"], rs.format.y8, self.arg["fps"]) config.enable_stream(rs.stream.color, self.arg["width"], self.arg["height"], rs.format.bgr8, self.arg["fps"]) profile = self.pipeline.start(config) # apply advanced mode dev = profile.get_device() advnc_mode = rs.rs400_advanced_mode(dev) json_obj = json.load(open(self.arg["preset_path"])) json_string = str(json_obj).replace("'", '\"') advnc_mode.load_json(json_string) # decimate decimate = rs.decimation_filter() decimate.set_option(rs.option.filter_magnitude, 2**self.arg["decimate_scale"])
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 main(): cam_pipes, depth_scale = c_man.get_pipes() #print("{} pipes created!".format(len(cam_pipes))) dec_filter = rs.decimation_filter() dep_scan = im_scan.scan_portait() traj_recv = Trajectory_Reciever() stay_go_sender = Publisher(3500) while True: frameset = c_man.get_frames(cam_pipes, dec_filter) scan_mat = dep_scan.get_portrait_from_frames(frameset, depth_scale) #dep_scan.conv_reduce() _, trans_position = dep_scan.get_pose() xz_pos = np.delete(trans_position, 1, axis=0) x_vel, y_vel = traj_recv.get_trajectory() control_vec = dep_scan.vector_from_vel(x_vel, y_vel) print("!dir_vec: ", control_vec) soft_danger, rgb_mat = real_oord_test(dep_scan, dir_vec=control_vec, position=xz_pos) stay_go_sender.send({"soft_danger": soft_danger}) print("-------------------------pass_fail: ", soft_danger) cv2.imshow("circumstances", rgb_mat) if cv2.waitKey(1) & 0xFF == ord('q'): break cv2.destroyAllWindows()
def _filter_depth_frame(depth_frame): """ 滤波器,用于获取坐标前的深度图像处理 :param depth_frame: 深度帧 :return: 滤波后的深度帧 """ dec = rs.decimation_filter() dec.set_option(rs.option.filter_magnitude, 1) depth_frame_pro = dec.process(depth_frame) depth2disparity = rs.disparity_transform() depth_frame_pro = depth2disparity.process(depth_frame_pro) spat = rs.spatial_filter() # 启用空洞填充,5为填充所有零像素 spat.set_option(rs.option.holes_fill, 5) depth_frame_pro = spat.process(depth_frame_pro) temp = rs.temporal_filter() depth_frame_pro = temp.process(depth_frame_pro) disparity2depth = rs.disparity_transform(False) depth_frame_pro = disparity2depth.process(depth_frame_pro) # depth_image_pro = np.asanyarray(depth_frame_pro.get_data()) # depth_colormap_pro = cv2.applyColorMap(cv2.convertScaleAbs(depth_image_pro, alpha=0.03), cv2.COLORMAP_JET) return depth_frame_pro
def __init__(self, pipeline): self.pipe = pipeline self.scale = 1 self.w = 640 self.h = 480 self.pc = None # Processing blocks self.pc = rs.pointcloud() self.decimate = rs.decimation_filter() self.decimate.set_option(rs.option.filter_magnitude, 2**state.decimate) self.colorizer = rs.colorizer() self.state = state # General ImageHandler model state self.out = out # General Image handler output if not changed # Data from camera self.image = None self.verts = None # Setup camera try: # Get stream profile and camera intrinsics profile = pipeline.get_active_profile() depth_profile = rs.video_stream_profile( profile.get_stream(rs.stream.depth)) self.depth_intrinsics = depth_profile.get_intrinsics() self.w, self.h = self.depth_intrinsics.width, self.depth_intrinsics.height depth_sensor = profile.get_device().first_depth_sensor() self.scale = depth_sensor.get_depth_scale() except RuntimeError as err: print(err)
def __init__(self, w=640, h=480, depth=True, frame_rate=30): ''' Initializing the Python RealSense Control Flow: w: Int (default = 640, can also be 1280) h: Int (default = 480, can also be 720) depth: Bool (default = True) frame_rate: Int (default = 30) RGB and Depth formats are: bgr8, z16 Note: In this class, variables should not be directly changed. ''' self.width = w self.height = h self.depth_on = depth self._pipeline = rs.pipeline() self._config = rs.config() self._config.enable_stream(rs.stream.color, w, h, rs.format.bgr8, frame_rate) self._intrinsic = None if depth: self.align = rs.align(rs.stream.color) self._preset = 0 # Presets (for D415): # 0: Custom # 1: Default # 2: Hand # 3: High Accuracy # 4: High Density # 5: Medium Density # depth interpolation self.interpolation = cv2.INTER_NEAREST # use nearest neighbor # self.interpolation = cv2.INTER_LINEAR # linear # self.interpolation = cv2.INTER_CUBIC # cubic # beautify depth image for viewing self._config.enable_stream(rs.stream.depth, w, h, rs.format.z16, frame_rate) self.colorizer = rs.colorizer() # initialize filters self.decimation = rs.decimation_filter() self.decimation.set_option(rs.option.filter_magnitude, 4) self.depth_to_disparity = rs.disparity_transform(True) self.spatial = rs.spatial_filter() self.spatial.set_option(rs.option.filter_magnitude, 5) self.spatial.set_option(rs.option.filter_smooth_alpha, 0.5) self.spatial.set_option(rs.option.filter_smooth_delta, 20) self.temporal = rs.temporal_filter() self.disparity_to_depth = rs.disparity_transform(False) print( "Initialized RealSense Camera\nw: {}, h: {}, depth: {}, frame_rate: {}" .format(w, h, depth, frame_rate))
def post_process_depth_frame(depth_frame, decimation_magnitude=1.0, spatial_magnitude=2.0, spatial_smooth_alpha=0.5, spatial_smooth_delta=20, temporal_smooth_alpha=0.4, temporal_smooth_delta=20): assert (depth_frame.is_depth_frame()) # Available filters and control options for the filters decimation_filter = rs.decimation_filter() spatial_filter = rs.spatial_filter() temporal_filter = rs.temporal_filter() filter_magnitude = rs.option.filter_magnitude filter_smooth_alpha = rs.option.filter_smooth_alpha filter_smooth_delta = rs.option.filter_smooth_delta # Apply the control parameters for the filter decimation_filter.set_option(filter_magnitude, decimation_magnitude) spatial_filter.set_option(filter_magnitude, spatial_magnitude) spatial_filter.set_option(filter_smooth_alpha, spatial_smooth_alpha) spatial_filter.set_option(filter_smooth_delta, spatial_smooth_delta) temporal_filter.set_option(filter_smooth_alpha, temporal_smooth_alpha) temporal_filter.set_option(filter_smooth_delta, temporal_smooth_delta) # Apply the filters filtered_frame = decimation_filter.process(depth_frame) filtered_frame = spatial_filter.process(filtered_frame) filtered_frame = temporal_filter.process(filtered_frame) return filtered_frame
def post_process_depth_frame(depth_frame, decimation_magnitude=1.0, spatial_magnitude=2.0, spatial_smooth_alpha=0.5, spatial_smooth_delta=20, temporal_smooth_alpha=0.4, temporal_smooth_delta=20): """ RS에서 획득한 깊이 프레임 전처리 필터링 하는 함수 Return: filtered_frame : rs.frame() """ # Post processing possible only on the depth_frame assert (depth_frame.is_depth_frame()) # Available filters and control options for the filters decimation_filter = rs.decimation_filter() spatial_filter = rs.spatial_filter() temporal_filter = rs.temporal_filter() filter_magnitude = rs.option.filter_magnitude filter_smooth_alpha = rs.option.filter_smooth_alpha filter_smooth_delta = rs.option.filter_smooth_delta # Apply the control parameters for the filter decimation_filter.set_option(filter_magnitude, decimation_magnitude) spatial_filter.set_option(filter_magnitude, spatial_magnitude) spatial_filter.set_option(filter_smooth_alpha, spatial_smooth_alpha) spatial_filter.set_option(filter_smooth_delta, spatial_smooth_delta) temporal_filter.set_option(filter_smooth_alpha, temporal_smooth_alpha) temporal_filter.set_option(filter_smooth_delta, temporal_smooth_delta) # Apply the filters filtered_frame = decimation_filter.process(depth_frame) filtered_frame = spatial_filter.process(filtered_frame) filtered_frame = temporal_filter.process(filtered_frame) return filtered_frame
def get_depth_filter_list(decimate=True, d2d=True, spatial=True, temporal=True): filters = [] if decimate: dec_filt = rs.decimation_filter() dec_filt.set_option(rs.option.filter_magnitude, 2) filters.append(dec_filt) if d2d: depth2disparity = rs.disparity_transform() filters.append(depth2disparity) if spatial: spat = rs.spatial_filter() spat.set_option(rs.option.holes_fill, FILL_ALL_ZERO_PIXELS) filters.append(spat) if temporal: temp = rs.temporal_filter() filters.append(temp) if d2d: disparity2depth = rs.disparity_transform(False) filters.append(disparity2depth) return filters
def post_process_depth_frame(depth_frame, decimation_magnitude=1.0, spatial_magnitude=2.0, spatial_smooth_alpha=0.5, spatial_smooth_delta=20, temporal_smooth_alpha=0.4, temporal_smooth_delta=20): """ Filter the depth frame acquired using the Intel RealSense device Parameters: ----------- depth_frame : rs.frame() The depth frame to be post-processed decimation_magnitude : double The magnitude of the decimation filter spatial_magnitude : double The magnitude of the spatial filter spatial_smooth_alpha : double The alpha value for spatial filter based smoothening spatial_smooth_delta : double The delta value for spatial filter based smoothening temporal_smooth_alpha : double The alpha value for temporal filter based smoothening temporal_smooth_delta : double The delta value for temporal filter based smoothening Return: ---------- filtered_frame : rs.frame() The post-processed depth frame """ # Post processing possible only on the depth_frame assert (depth_frame.is_depth_frame()) # Available filters and control options for the filters decimation_filter = rs.decimation_filter() spatial_filter = rs.spatial_filter() temporal_filter = rs.temporal_filter() filter_magnitude = rs.option.filter_magnitude filter_smooth_alpha = rs.option.filter_smooth_alpha filter_smooth_delta = rs.option.filter_smooth_delta # Apply the control parameters for the filter decimation_filter.set_option(filter_magnitude, decimation_magnitude) spatial_filter.set_option(filter_magnitude, spatial_magnitude) spatial_filter.set_option(filter_smooth_alpha, spatial_smooth_alpha) spatial_filter.set_option(filter_smooth_delta, spatial_smooth_delta) temporal_filter.set_option(filter_smooth_alpha, temporal_smooth_alpha) temporal_filter.set_option(filter_smooth_delta, temporal_smooth_delta) # Apply the filters filtered_frame = decimation_filter.process(depth_frame) filtered_frame = spatial_filter.process(filtered_frame) filtered_frame = temporal_filter.process(filtered_frame) return filtered_frame
def decimation(self, frame): #apply decimation filter to the frame that is sent in. Downscaling #must call depthImageCV2 to display decimation = rs.decimation_filter() decimation.set_option(rs.option.filter_magnitude, 2) decimated_depth = decimation.process(frame) return decimated_depth
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 mainSegment(options = None): pipeline, depthScale, align = initCamera() print '--------------------------------------------' print options counter = 0 if constants.PROFILE_ON: pr = cProfile.Profile() #profile pr.enable() #profile try: warmUp(pipeline) minVal = computeBackground(options, pipeline, align) net = bp.newNet() t0 = time.time() counterOld = counter decimation = rs.decimation_filter() decimation.set_option(rs.option.filter_magnitude, constants.DECIMATION_FACTOR) temporal = rs.temporal_filter(0.5, 20, 5)#valid 1 of last 2 while True: counter = counter + 1 frames = pipeline.wait_for_frames() # alignedFrames = align.process(frames) # depth = alignedFrames.get_depth_frame() depth = frames.get_depth_frame() filtered_depth = decimation.process(depth) filtered_depth = temporal.process(filtered_depth) # intrinsics already reflects decimation intrinsics = filtered_depth.profile.as_video_stream_profile().intrinsics # filtered_depth = depth # if counter % 2 == 1: #help the temporal filter since we cannot process full rate # continue d = np.asanyarray(filtered_depth.get_data()).astype(np.float32) alpha = subtraction(options, minVal, d) if (options and options.get('display')): cv2.imshow('Contour', alpha) if constants.PROFILE_ON: result = pr.runcall(bp.process, options, net, d, alpha, depthScale, intrinsics) #profile else: result = bp.process(options, net, d, alpha, depthScale, intrinsics) if (options and options.get('display')): if cv2.waitKey(1) & 0xFF == ord('q'): break if counter % 10 == 0: t1 = time.time() print '{:.3f} images/sec'.format((counter - counterOld)/(t1-t0)) t0 = t1 counterOld = counter yield json.dumps(result) finally: if constants.PROFILE_ON: pr.disable() pr.print_stats()#profile pipeline.stop()
def post_process_depth_frame(depth_frame, decimation_magnitude=1.0, spatial_magnitude=2.0, spatial_smooth_alpha=0.5, spatial_smooth_delta=20, temporal_smooth_alpha=0.4, temporal_smooth_delta=20): """ Filter the depth frame acquired using the Intel RealSense device Parameters: ----------- depth_frame : rs.frame() The depth frame to be post-processed decimation_magnitude : double The magnitude of the decimation filter spatial_magnitude : double The magnitude of the spatial filter spatial_smooth_alpha : double The alpha value for spatial filter based smoothening spatial_smooth_delta : double The delta value for spatial filter based smoothening temporal_smooth_alpha : double The alpha value for temporal filter based smoothening temporal_smooth_delta : double The delta value for temporal filter based smoothening Return: ---------- filtered_frame : rs.frame() The post-processed depth frame """ # Post processing possible only on the depth_frame assert (depth_frame.is_depth_frame()) # Available filters and control options for the filters decimation_filter = rs.decimation_filter() spatial_filter = rs.spatial_filter() temporal_filter = rs.temporal_filter() filter_magnitude = rs.option.filter_magnitude filter_smooth_alpha = rs.option.filter_smooth_alpha filter_smooth_delta = rs.option.filter_smooth_delta # Apply the control parameters for the filter decimation_filter.set_option(filter_magnitude, decimation_magnitude) spatial_filter.set_option(filter_magnitude, spatial_magnitude) spatial_filter.set_option(filter_smooth_alpha, spatial_smooth_alpha) spatial_filter.set_option(filter_smooth_delta, spatial_smooth_delta) temporal_filter.set_option(filter_smooth_alpha, temporal_smooth_alpha) temporal_filter.set_option(filter_smooth_delta, temporal_smooth_delta) # Apply the filters filtered_frame = decimation_filter.process(depth_frame) filtered_frame = spatial_filter.process(filtered_frame) filtered_frame = temporal_filter.process(filtered_frame) return filtered_frame
def preprocess_depth_frame(self, depth_frame): decimation = rs.decimation_filter() depth_image = decimation.process(depth_frame) spatial = rs.spatial_filter() #filtered_depth = spatial.process(depth_frame) 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) depth_frame = spatial.process(depth_frame) return depth_frame
def __init__(self, profile): depth_profile = rs.video_stream_profile(profile.get_stream(rs.stream.depth)) self.depth_intrinsics = depth_profile.get_intrinsics() self.w, self.h = self.depth_intrinsics.width, self.depth_intrinsics.height self.state = PointCloudState() self.out = np.empty((self.h, self.w, 3), dtype=np.uint8) self.pc = rs.pointcloud() self.decimate = rs.decimation_filter() self.decimate.set_option(rs.option.filter_magnitude, 2 ** self.state.decimate)
def __init__(self): # Configure depth and color streams self.__pipeline__ = rs.pipeline() self.__config__ = rs.config() 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) self.__profile__ = None self.__depth_profile__ = None self.__depth_intrinsics__ = None self.__decimate__ = rs.decimation_filter() self.__colorizer__ = rs.colorizer()
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 takeDepth(self, showMillis=0) -> bool: if not self.streaming: if self.startStream(): for i in range(self.numReads ): # let cam adapt to current light situation frames = self.handle.wait_for_frames() _ = frames.get_depth_frame() time.sleep(0.1) else: config.log(f"D415 startStream failed") return False decimate = rs.decimation_filter() decimate.set_option(rs.option.filter_magnitude, 3) frames = self.handle.wait_for_frames() depthFrame = frames.get_depth_frame() if depthFrame is None: config.log(f"could not acquire depth from D415") return False else: decimated = decimate.process( depthFrame) # make 428x240x3 from 1280x720x3 # Grab new intrinsics (may be changed by decimation) # depth_intrinsics = rs.video_stream_profile(depth_frame.profile).get_intrinsics() # w, h = depth_intrinsics.width, depth_intrinsics.height pointCloud = self.pc.calculate(decimated) # Pointcloud data to arrays verts = pointCloud.get_vertices() self.depth = np.asanyarray(verts).view(np.float32).reshape( -1, 3) # xzy # Colorize depth frame to jet colormap depth_color_frame = self.colorizer.colorize(depthFrame) # Convert depth_frame to numpy array to render image in opencv self.depthColored = np.asanyarray(depth_color_frame.get_data()) #depthImage = np.asanyarray(depthFrame.get_data()) #depthColored = cv2.applyColorMap(cv2.convertScaleAbs(depthImage), cv2.COLORMAP_MAGMA) #.COLORMAP_RAINBOW) if showMillis > 0: cv2.imshow(f"D415 depth", self.depthColored) cv2.waitKey(showMillis) cv2.destroyAllWindows() config.log(f"D415 depth points available") return True
def __init__(self, pipeline, streamertype=STREAM_RGB, pc=None, colorizer=None): self.pipeline = pipeline self.streamtype = streamertype self.decimate_filter = rs.decimation_filter() decimate = 1 self.decimate_filter.set_option(rs.option.filter_magnitude, 2**decimate) self.colorizer = colorizer self.pc = pc self.frame_data = ''
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()
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 pipeline.start(config) # Get stream profile and camera intrinsics profile = pipeline.get_active_profile() depth_profile = rs.video_stream_profile(profile.get_stream(rs.stream.depth)) depth_intrinsics = depth_profile.get_intrinsics() w, h = depth_intrinsics.width, depth_intrinsics.height # Processing blocks pc = rs.pointcloud() decimate = rs.decimation_filter() decimate.set_option(rs.option.filter_magnitude, 2 ** state.decimate) colorizer = rs.colorizer() def mouse_cb(event, x, y, flags, param): if event == cv2.EVENT_LBUTTONDOWN: state.mouse_btns[0] = True if event == cv2.EVENT_LBUTTONUP: state.mouse_btns[0] = False if event == cv2.EVENT_RBUTTONDOWN: state.mouse_btns[1] = True