def run_focal_length_calibration(target_size, adjust_side): number_of_images = 25 timeout_s = 30 cfg = rs.config() cfg.enable_stream(rs.stream.infrared, 1, 1280, 720, rs.format.y8, 30) cfg.enable_stream(rs.stream.infrared, 2, 1280, 720, rs.format.y8, 30) lq = rs.frame_queue(capacity=number_of_images, keep_frames=True) rq = rs.frame_queue(capacity=number_of_images, keep_frames=True) counter = 0 flags = [False, False] def cb(frame): nonlocal counter, flags if counter > number_of_images: return for f in frame.as_frameset(): p = f.get_profile() if p.stream_index() == 1: lq.enqueue(f) flags[0] = True if p.stream_index() == 2: rq.enqueue(f) flags[1] = True if all(flags): counter += 1 flags = [False, False] pipe = rs.pipeline(ctx) pp = pipe.start(cfg, cb) dev = pp.get_device() try: print('Starting Focal-Length calibration...') print(f'\tTarget Size:\t{target_size}') print(f'\tSide Adjustment:\t{adjust_side}') stime = time.time() while counter < number_of_images: time.sleep(0.5) if timeout_s < (time.time() - stime): raise RuntimeError( f"Failed to capture {number_of_images} frames in {timeout_s} seconds, got only {counter} frames" ) adev = dev.as_auto_calibrated_device() table, ratio, angle = adev.run_focal_length_calibration( lq, rq, target_size[0], target_size[1], fl_adjust_map[adjust_side], progress_callback) print('Focal-Length calibration finished') print(f'\tRatio:\t{ratio}') print(f'\tAngle:\t{angle}') adev.set_calibration_table(table) adev.write_calibration() finally: pipe.stop()
def __init__(self, callback=None, max_queue_size=100000, statistics=False, serial_number=None): self._id = serial_number self._max_queue_size = max_queue_size self._logger = ModifiedLogger( logging.getLogger('test'), {'serial': self._id}, ) self._process = callback self.lrs_queue = rs.frame_queue(capacity=self._max_queue_size, keep_frames=True) self._post_process_queue = Queue(maxsize=self._max_queue_size) self._producer_thread = None self._consumer_thread = None self._block_queue_event = threading.Event() self.block() # ignoring frames until self.start is called self._terminate_producer_event = threading.Event() self.statistics = statistics self._producing_times = collections.deque() self._producer_queue_sizes = collections.deque() self._consuming_times = collections.deque() self._consumer_queue_sizes = collections.deque() self._memory_samples = collections.deque()
def start(self): """Initialize the queue structure and starting the producer-consumer threads first, unblocking the threading queue and clear all thread's events and then initialize the producer-consumer threads. """ self.unblock() self._terminate_producer_event.clear() if self.lrs_queue is None: self.lrs_queue = rs.frame_queue(capacity=self._max_queue_size, keep_frames=True) if self._post_process_queue is None: self._post_process_queue = Queue(maxsize=self._max_queue_size) # in case self.start is being called multiple times in a row and self.stop is not being called between. if self._producer_thread is None: self._logger.info("initializing a producer thread") self._producer_thread = threading.Thread( target=self._produce_frames, name="producer_thread(LRSFrameQueueManager)") self._producer_thread.setDaemon(True) self._logger.info("starting the producer thread") self._producer_thread.start() # # in case self.start is being called multiple times in a row and self.stop is not being called between. if self._consumer_thread is None: self._logger.info("initializing a consumer thread") self._consumer_thread = threading.Thread( target=self._consume_frames, name="consumer_thread(LRSFrameQueueManager)") self._consumer_thread.setDaemon(True) self._logger.info("starting the consumer thread") self._consumer_thread.start()
def __init__(self, res_w=1280, res_h=720, fps=30, use_color=True, use_depth=True, show_depth=True, preset=1, color_scheme=0): assert use_color or use_depth, "At least 1 of the 2 flags 'use_color' and 'use_depth' must be set to True" self.use_color = use_color self.use_depth = use_depth self.use_color_depth = use_color and use_depth self.show_depth = use_depth and show_depth # Resolution in pixels self.res_w = res_w self.res_h = res_h # Preset: 1=default, 2=hand, 3=high accuracy, # Setup of the sensor self.pipe = rs.pipeline() self.cfg = rs.config() if self.use_color_depth: self.align = rs.align(rs.stream.color) # Color stream if self.use_color: self.cfg.enable_stream(rs.stream.color, res_w, res_h, rs.format.bgr8, 30) # Depth stream to configuration # Format z16: 0<=z<=65535 if self.use_depth: self.cfg.enable_stream(rs.stream.depth, res_w, res_h, rs.format.z16, fps) self.queue = rs.frame_queue(2 if self.use_color_depth else 1) # With a buffer capacity of 2, we reduce latency self.pipe_profile = self.pipe.start(self.cfg, self.queue) # Color scheme: 0: Jet, 1: Classic, 2: WhiteToBlack, 3: BlackToWhite, 4: Bio, 5: Cold, 6: Warm, 7: Quantized, 8: Pattern, 9: Hue if self.show_depth: self.colorizer = rs.colorizer() self.colorizer.set_option(rs.option.color_scheme, color_scheme) if self.use_depth: self.dev = self.pipe_profile.get_device() # self.advnc_mode = rs.rs400_advanced_mode(self.dev) # print("Advanced mode is", "enabled" if self.advnc_mode.is_enabled() else "disabled") self.depth_sensor = self.dev.first_depth_sensor() # Set preset self.preset = preset self.depth_sensor.set_option(rs.option.visual_preset, self.preset) print(f"Current preset: {self.depth_sensor.get_option_value_description(rs.option.visual_preset, self.depth_sensor.get_option(rs.option.visual_preset))}") # # Set the disparity shift # if disparity_shift != -1: # self.depth_table_control_group = self.advnc_mode.get_depth_table() # self.depth_table_control_group.disparityShift = disparity_shift # self.advnc_mode.set_depth_table(self.depth_table_control_group) # self.depth_table_control_group = self.advnc_mode.get_depth_table() # print("Disparity shift:", self.depth_table_control_group.disparityShift) # Set depth units # self.set_depth_units(depth_units) # Get intrinsics self.stream_profile = self.pipe_profile.get_stream(rs.stream.depth) # Fetch stream profile for depth stream self.intrinsics = self.stream_profile.as_video_stream_profile().get_intrinsics() # Downcast to video_stream_profile and fetch intrinsics # Get depth scale self.depth_scale = self.depth_sensor.get_depth_scale() self.get_depth_units()
def calculate_target_z(target_size): number_of_images = 50 # The required number of frames is 10+ timeout_s = 30 cfg = rs.config() cfg.enable_stream(rs.stream.infrared, 1, 1280, 720, rs.format.y8, 30) q = rs.frame_queue(capacity=number_of_images, keep_frames=True) # Frame queues q2, q3 should be left empty. Provision for future enhancements. q2 = rs.frame_queue(capacity=number_of_images, keep_frames=True) q3 = rs.frame_queue(capacity=number_of_images, keep_frames=True) counter = 0 def cb(frame): nonlocal counter if counter > number_of_images: return for f in frame.as_frameset(): q.enqueue(f) counter += 1 pipe = rs.pipeline(ctx) pp = pipe.start(cfg, cb) dev = pp.get_device() try: stime = time.time() while counter < number_of_images: time.sleep(0.5) if timeout_s < (time.time() - stime): raise RuntimeError( f"Failed to capture {number_of_images} frames in {timeout_s} seconds, got only {counter} frames" ) adev = dev.as_auto_calibrated_device() print('Calculating distance to target...') print(f'\tTarget Size:\t{target_size}') target_z = adev.calculate_target_z(q, q2, q3, target_size[0], target_size[1]) print(f'Calculated distance to target is {target_z}') finally: pipe.stop() return target_z
def __init__(self, rgb_sensor): self._stop = False self.frames = [] self.count_drops = 0 self.frame_drops_info = {} self.prev_hw_timestamp = 0.0 self.prev_fnum = 0 self.first_frame = True self.lrs_queue = rs.frame_queue(capacity=100000, keep_frames=True) self.post_process_queue = Queue(maxsize=1000000) self.rgb_sensor = rgb_sensor
def playback(filename, use_syncer=True): """ One of the two initialization functions: Use playback() to initialize a software device for reading from a rosbag file. This device will NOT generate frames! Only the expect() functions will be available. If you use this function, it replaces init(). This should be followed by start() to actually start "streaming". :param filename: The path to the file to open for playback :param use_syncer: If True, a syncer will be used to attempt frame synchronization -- otherwise a regular queue will be used """ ctx = rs.context() # global device device = rs.playback(ctx.load_device(filename)) device.set_real_time(False) device.set_status_changed_callback(playback_callback) # global depth_sensor, color_sensor sensors = device.query_sensors() depth_sensor = next(s for s in sensors if s.name == "Depth") color_sensor = next(s for s in sensors if s.name == "Color") # global depth_profile, color_profile depth_profile = next(p for p in depth_sensor.profiles if p.stream_type() == rs.stream.depth) color_profile = next(p for p in color_sensor.profiles if p.stream_type() == rs.stream.color) # global syncer if use_syncer: syncer = rs.syncer( 100 ) # We don't want to lose any frames so uses a big queue size (default is 1) else: syncer = rs.frame_queue(100) # global playback_status playback_status = rs.playback_status.unknown
def init(self, device, width, height): self.un_init() self.frame_queue = rs.frame_queue(10) self.device = device self.sensor = self.device.first_depth_sensor() stream_profiles = self.sensor.get_stream_profiles() video_stream_profiles = [] for item in stream_profiles: video_stream_profiles.append(item.as_video_stream_profile()) depth_video_stream_profiles = filter( lambda x: x.stream_type() == rs.stream.depth, video_stream_profiles) depth_video_stream_profiles = list(depth_video_stream_profiles) sorted_depth_video_stream_profiles = sorted( depth_video_stream_profiles, key=lambda x: x.fps(), reverse=True) width_height_sorted_depth_video_stream_profiles = filter( lambda x: x.width() == width and x.height() == height, sorted_depth_video_stream_profiles) width_height_sorted_depth_video_stream_profiles = list( width_height_sorted_depth_video_stream_profiles) self.sensor_profile = width_height_sorted_depth_video_stream_profiles[ 0] self.width = self.sensor_profile.width() self.height = self.sensor_profile.height() self.depth = np.empty(self.height * self.width, dtype=np.uint) self.distance = np.empty(self.width, dtype=np.float) self.sensor.open(self.sensor_profile) self.sensor.start(self.frame_queue)
config.enable_stream(rs.stream.depth, 640, 360, rs.format.z16, 30) # Start streaming to the slow processing function. # This stream will lag, causing the occasional frame drop. print("Slow callback") pipeline.start(config) start = time.time() while time.time() - start < 5: frames = pipeline.wait_for_frames() slow_processing(frames) pipeline.stop() # Start streaming to the the slow processing function by way of a frame queue. # This stream will occasionally hiccup, but the frame_queue will prevent frame loss. print("Slow callback + queue") queue = rs.frame_queue(50) pipeline.start(config, queue) start = time.time() while time.time() - start < 5: frames = queue.wait_for_frame() slow_processing(frames) pipeline.stop() # Start streaming to the the slower processing function by way of a frame queue. # This stream will drop frames because the delays are long enough that the backed up # frames use the entire internal frame pool preventing the SDK from creating more frames. print("Slower callback + queue") queue = rs.frame_queue(50) pipeline.start(config, queue) start = time.time() while time.time() - start < 5:
def init(syncer_matcher=rs.matchers.default): """ One of the two initialization functions: Use init() to initialize a software device that will generate frames (as opposed to playback() which will initialize a software device for reading from a rosbag and will NOT generate frames). This should be followed by start() to actually start "streaming". This sets multiple module variables that are used to generate software frames, and initializes a syncer automatically if needed. :param syncer_matcher: The matcher to use in the syncer (if None, no syncer will be used) -- the default will compare all streams according to timestamp """ global gap_c, gap_d gap_d = 1000 / fps_d gap_c = 1000 / fps_c # global pixels, w, h pixels = bytearray(b'\x00' * (w * h * 2)) # Dummy data # global device device = rs.software_device() if syncer_matcher is not None: device.create_matcher(syncer_matcher) # global depth_sensor, color_sensor depth_sensor = device.add_sensor("Depth") color_sensor = device.add_sensor("Color") # depth_stream = rs.video_stream() depth_stream.type = rs.stream.depth depth_stream.uid = 0 depth_stream.width = 640 depth_stream.height = 480 depth_stream.bpp = 2 depth_stream.fmt = rs.format.z16 depth_stream.fps = fps_d # global depth_profile depth_profile = rs.video_stream_profile( depth_sensor.add_video_stream(depth_stream)) # color_stream = rs.video_stream() color_stream.type = rs.stream.color color_stream.uid = 1 color_stream.width = 640 color_stream.height = 480 color_stream.bpp = 2 color_stream.fmt = rs.format.rgba8 color_stream.fps = fps_c # global color_profile color_profile = rs.video_stream_profile( color_sensor.add_video_stream(color_stream)) # # We don't want to lose any frames so use a big queue size (default is 1) global syncer if syncer_matcher is not None: syncer = rs.syncer(100) else: syncer = rs.frame_queue(100) # global playback_status playback_status = None
ctx = rs.context() sd.add_to(ctx) dev = ctx.query_devices()[0] for d in ctx.query_devices(): if d.get_info(rs.camera_info.name) == name: dev = d images_path = "." if (len(sys.argv) > 1): images_path = str(sys.argv[1]) rec = rs.recorder(images_path + "/1.bag", dev) sensor = rec.query_sensors()[0] q = rs.frame_queue() sensor.open(sensor.get_stream_profiles()) sensor.start(q) files = glob.glob1(images_path, "gt*") index = [] for f in files: idx = (f.split('-')[1]).split('.')[0] index.append(int(idx)) for i in index: left_name = images_path + "/left-" + str(i) + ".png" depth_name = images_path + "/gt-" + str(i) + ".png" result_name = images_path + "/res-" + str(i) + ".png" denoised_name = images_path + "/res_denoised-" + str(i) + ".png"
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, 1280, 720, 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) 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) dt0 = datetime.now() 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() # 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]]) #print("Testing IO for point cloud ...") #pcd = o3d.io.read_point_cloud("../../test_data/fragment.pcd") print(pcd) #o3d.io.write_point_cloud("data/realsense.pcd", pcd) clusteringDepthImage(pcd) if geom_added == False: pass # 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
def main(): app_config = low_res_config # objects = np.empty(NUMBER_OF_OBJECTS, dtype=object) objects = [] old_time = time.time() fourcc = cv2.VideoWriter_fourcc(*'XVID') out = cv2.VideoWriter('./data/output.avi', fourcc, 30.0, (CAMERA_WIDTH, CAMERA_HEIGHT)) pipeline = rs.pipeline() queue = rs.frame_queue(5000, keep_frames=True) config = rs.config() #Visualizer(CAMERA_WIDTH,CAMERA_HEIGHT, "Visualizer", resizable=True) samplegrid = SampleGrid(app_config) samplegrid.initialize() # note: using 640 x 480 depth resolution produces smooth depth boundaries # using rs.format.bgr8 for color image format for OpenCV based image visualization config.enable_stream(rs.stream.depth, app_config["IMAGE_WIDTH"], app_config["IMAGE_HEIGHT"], rs.format.z16, 30) config.enable_stream(rs.stream.color, app_config["IMAGE_WIDTH"], app_config["IMAGE_HEIGHT"], rs.format.rgb8, 30) config.enable_record_to_file("./data/record.bag") #config.enable_device_from_file("./data/realsense.bag", repeat_playback=True) #config.enable_device_from_file("./data/record_homeroom_low.bag", repeat_playback=True) # Start streaming profile = pipeline.start(config, queue) depth_sensor = profile.get_device().first_depth_sensor() #depth_sensor.set_option(rs.option.max_distance, 4) # Getting the depth sensor's depth scale (see rs-align example for explanation) depth_scale = depth_sensor.get_depth_scale() # We will not display the background of objects more than # clipping_distance_in_meters meters away clipping_distance_in_meters = 4 # 3 meter clipping_distance = clipping_distance_in_meters / depth_scale # rs400::advanced_mode # advanced_device(camera.getDevice()); # auto # depth_table = advanced_device.get_depth_table(); # depth_table.depthClampMax = 1300; // 1 # m30 if depth # unit # at # 0.001 # advanced_device.set_depth_table(depth_table); # 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) # 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) transformation_matrix_1 = [[0,0, -1], [0,1, 0], [1, 0,0]]#, [0, 0, 0, 1]] transformation_matrix_2 = [[1,0, 0], [0,0, -1], [0, 1,0]]#, [0, 0, 0, 1]] #transformation_matrix = [[-1, 0, 0, 0], [0, 1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]] #inv_transform = np.linalg.inv(transformation_matrix) # Streaming loop frame_count = 0 intrinsics = profile.get_stream(rs.stream.depth).as_video_stream_profile().get_intrinsics() speedestimation = SpeedEstimation(intrinsics, config) while frame_count < 1000: cycle_time_start = time.time() # Get frameset of color and depth # frames = pipeline.wait_for_frames() start = time.time() frames = queue.wait_for_frame().as_frameset() current_time = frames.get_frame_metadata(rs.frame_metadata_value.time_of_arrival) # Align the depth frame to color frame aligned_frames = align.process(frames) # Get aligned frames depth_frame = aligned_frames.get_depth_frame() color_frame = aligned_frames.get_color_frame() if frame_count < 60: #print(frame_count) frame_count += 1 continue #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) # Validate that both frames are valid if not depth_frame or not color_frame: continue depth_image = np.asanyarray(depth_frame.get_data()) color_image = np.asanyarray(color_frame.get_data()) end = time.time() print("Frame Post Processing took: " + str((end - start) * 1000) + "ms") ############# #### ## Create Point Cloud #### ############# start = time.time() pc = rs.pointcloud() points = pc.calculate(depth_frame) point_array = np.asarray(points.get_vertices(2)) end = time.time() print("Pointcloud generation took: " + str((end - start) * 1000) + "ms") print(point_array.size) # Transformation code # newrow = np.ones((point_array.shape[0],1)) # point_array = np.append(point_array,newrow,axis=1) point_array = np.dot(point_array,transformation_matrix_1) point_array = np.dot(point_array,transformation_matrix_2) # point_array = np.delete(point_array, np.s_[3:4], axis=1) #pcd = o3d.geometry.PointCloud(o3d.utility.Vector3dVector(point_array)) #res = pcd.estimate_normals() #print(res) #o3d.visualization.draw_geometries([pcd]) ## FLood fill start = time.time() #samplegrid.update(np.asarray(pcd.points)) samplegrid.update(point_array) end = time.time() print("update took: " + str((end - start) * 1000) + "ms") start = time.time() floorplane = samplegrid.findFloor() end = time.time() print("Floor detection: " + str((end - start) * 1000) + "ms") # normal = np.array(floorplane.n) # point = np.array(floorplane.p) # d = -point.dot(normal) # xx, yy = np.meshgrid(range(10), range(10)) # # # calculate corresponding z # z = (-normal[0] * xx - normal[1] * yy - d) * 1. / normal[2] # # # plot the surface # plt3d = plt.figure().gca(projection='3d') # plt3d.plot_surface(xx, yy, z) # plt.show() # #plt.savefig("planeimg.png") trans = transformFromGroundPlane(floorplane.n,floorplane.p) indices = [] newrow = np.ones((point_array.shape[0], 1)) point_array = np.append(point_array, newrow, axis=1) point_array = np.dot(point_array, np.array(trans)) point_array = np.delete(point_array, np.s_[3:4], axis=1) trans = [j for sub in trans for j in sub] # for i, arr in enumerate(point_array): # arr[0] = trans[0] * arr[0] + trans[4] * arr[1]+ trans[8] * arr[2] + trans[12] # arr[1] = trans[1] * arr[0] + trans[5] * arr[1] + trans[9] * arr[2] + trans[13] # arr[2] = trans[2] * arr[0] + trans[6] * arr[1] + trans[10] * arr[2] + trans[14] #point_array = np.delete(point_array, indices, axis=0) end = time.time() # print("Removal of clipping: " + str((end - start) * 1000) + "ms") # print(point_array.size) start = time.time() #speedestimation.step(point_array) #speedestimation.step(pcd.points) end = time.time() print("Speed estimation took: " + str((end - start) * 1000) + "ms") # for obj in objects: # if obj.status != "active": # continue # # corners = np.asarray(obj.bounding_box.get_box_points()) # extent = obj.bounding_box.get_extent() # pixels = [] # for point in corners: # pixel = rs.rs2_project_point_to_pixel(intrinsics, point.tolist()) # pixel = [int(coord) for coord in pixel] # pixels.append(pixel) # # cv2.rectangle(color_image, tuple(pixels[1]), tuple(pixels[2]), (0, 255, 0), 2) # # # label = "Obj-ID: "+str(obj.id) +"\n" \ # + "vx: " + str(round(obj.speed_vector[0],2)) + " m/s \n" \ # + "vy: " + str(round(obj.speed_vector[1], 2)) + " m/s \n" \ # + "vz: " + str(round(obj.speed_vector[2], 2))+ " m/s \n" \ # + "speed " + str(round(obj.speed_vector[3],2)) + "m/s \n" # position = tuple(pixels[5]) # y0,dy = position[1], 12 # for i, line in enumerate(label.split('\n')): # y = y0 + i * dy # #cv2.putText(img, line, (50, y), cv2.FONT_HERSHEY_SIMPLEX, 1, 2) # cv2.putText(color_image, line,(position[0]+10,y) , cv2.FONT_HERSHEY_COMPLEX, 0.5, (255, 255, 0), 1) # # # end = time.time() # print("2D projection time: " + str((end - start) * 1000) + "ms") # # cv2.putText(color_image, str(frame_count), (50, 50), cv2.FONT_HERSHEY_COMPLEX, 2, (255, 255, 0), 1) # out_img = cv2.cvtColor(color_image, cv2.COLOR_RGB2BGR) # cv2.imshow('img', out_img) # k = cv2.waitKey(1) # mesh = o3d.geometry.TriangleMesh.create_coordinate_frame() # pcd = o3d.geometry.PointCloud(o3d.utility.Vector3dVector(point_array)) # pcd.transform(trans) # indices = [] # for i, arr in enumerate(np.asarray(pcd.points)): # if arr[2] < 0.05: # indices.append(i) # point_array = np.delete(pcd.points, indices, axis=0) # pcd.points = o3d.utility.Vector3dVector(point_array) # o3d.visualization.draw_geometries([pcd, mesh]) cycle_time_end = time.time() print("Cycle time: " + str((cycle_time_end - cycle_time_start) * 1000) + "ms") out.release() pipeline.stop()
# test scenario: # for each option, set value, wait 10 frames, check value with get_option and get_frame_metadata # values set for each option are min, max, and default values ctx = rs.context() dev = ctx.query_devices()[0] try: color_sensor = dev.first_color_sensor() # Using a profile common to both L500 and D400 color_profile = next(p for p in color_sensor.profiles if p.fps() == 30 and p.stream_type() == rs.stream.color and p.format() == rs.format.yuyv and p.as_video_stream_profile().width() == 640 and p.as_video_stream_profile().height() == 480) color_sensor.open(color_profile) lrs_queue = rs.frame_queue(capacity=10, keep_frames=False) color_sensor.start(lrs_queue) iteration = 0 option_index = -1 # running over options value_index = -1 # 0, 1, 2 for min, max, default # number of frames to wait between set_option and checking metadata # is set as 10 - the expected delay is ~120ms for Win and ~80-90ms for Linux num_of_frames_to_wait = 15 while True: try: lrs_frame = lrs_queue.wait_for_frame(5000) if iteration == 0: option_index = option_index + 1 if option_index == len(color_options): break
def main(): # objects = np.empty(NUMBER_OF_OBJECTS, dtype=object) objects = [] old_time = time.time() fourcc = cv2.VideoWriter_fourcc(*'XVID') out = cv2.VideoWriter('./data/output.avi', fourcc, 30.0, (1280, 720)) pipeline = rs.pipeline() queue = rs.frame_queue(5000, keep_frames=True) config = rs.config() vis = o3d.visualization.Visualizer() vis.create_window('PCD', width=1280, height=720) pointcloud = o3d.geometry.PointCloud() vis.add_geometry(pointcloud) geom_added = False # note: using 640 x 480 depth resolution produces smooth depth boundaries # using rs.format.bgr8 for color image format for OpenCV based image visualization config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30) config.enable_stream(rs.stream.color, 1280, 720, rs.format.rgb8, 30) config.enable_device_from_file("./data/realsense.bag", repeat_playback=True) # Start streaming profile = pipeline.start(config, queue) depth_sensor = profile.get_device().first_depth_sensor() # Getting the depth sensor's depth scale (see rs-align example for explanation) depth_scale = depth_sensor.get_depth_scale() # We will not display the background of objects more than # clipping_distance_in_meters meters away clipping_distance_in_meters = 4 # 3 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) # 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) transformation_matrix = [[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]] inv_transform = np.linalg.inv(transformation_matrix) #grid = Grid(config_perception) #grid.initialize() # Streaming loop frame_count = 0 intrinsics = profile.get_stream( rs.stream.depth).as_video_stream_profile().get_intrinsics() pcd_old = o3d.geometry.PointCloud() while frame_count < 1000: cycle_time_start = time.time() # Get frameset of color and depth # frames = pipeline.wait_for_frames() frames = queue.wait_for_frame().as_frameset() current_time = frames.get_frame_metadata( rs.frame_metadata_value.time_of_arrival) start = time.time() # Align the depth frame to color frame aligned_frames = align.process(frames) # Get aligned frames depth_frame = aligned_frames.get_depth_frame() color_frame = aligned_frames.get_color_frame() # Validate that both frames are valid if not depth_frame or not color_frame: continue depth_image = np.asanyarray(depth_frame.get_data()) color_image = np.asanyarray(color_frame.get_data()) end = time.time() #print("Frame Post Processing took: " + str((end - start) * 1000) + "ms") ############# #### ## Create Point Cloud #### ############# start = time.time() 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 = pcd.remove_non_finite_points(remove_nan=True, remove_infinite=True) pcd = pcd.voxel_down_sample(voxel_size=0.05) points = np.asarray(pcd.points) pcd_tree = o3d.geometry.KDTreeFlann(pcd) start = time.time() [k, idx, _] = pcd_tree.search_knn_vector_3d(pcd.points[1500], 4) end = time.time() print("Neighbour search: " + str((end - start) * 1000) + "ms") print(idx)
if len(devices) is 0: exit(1) # This tutorial will access only a single device, but it is trivial to extend to multiple devices dev = devices[0] print("\nUsing device 0, an %s" % dev.get_camera_info(rs.camera_info.device_name)) print(" Serial number: %s" % dev.get_camera_info(rs.camera_info.device_serial_number)) print(" Firmware version: %s" % dev.get_camera_info(rs.camera_info.camera_firmware_version)) # Configure depth to run at VGA resolution at 30 frames per second dev.open(rs.stream_profile(rs.stream.depth, 640, 480, 30, rs.format.z16)) # Configure frame queue of size one and start streaming into it queue = rs.frame_queue(1) dev.start(queue) # Determine depth value corresponding to one meter one_meter = int(1.0 / dev.get_depth_scale()) while True: # This call waits until a new coherent set of frames is available on a device # Calls to get_frame_data(...) and get_frame_timestamp(...) on a device will return stable values until wait_for_frames(...) is called frame = queue.wait_for_frame() # Retrieve depth data, which was previously configured 640 x 480 image of 16-bit depth values frame_data = frame.get_data() depth_frame = [0] * (len(frame_data) / 2) for i in xrange(len(frame_data) / 2): depth_frame[i] = (frame_data[i * 2] + frame_data[i * 2 + 1] * 256)
# Create pipeline pipeline = rs.pipeline() # Create a config object config = rs.config() # Tell config that we will use a recorded device from file to be used by the pipeline through playback. rs.config.enable_device_from_file(config, args.input) # Configure the pipeline to stream the depth stream # Change this parameters according to the recorded bag file resolution config.enable_stream(rs.stream.depth, rs.format.z16, 30) config.enable_stream(rs.stream.color, rs.format.rgb8, 30) queue = rs.frame_queue(100, keep_frames=True) # Start streaming from file profile = pipeline.start(config) depth_sensor = profile.get_device().first_depth_sensor() depth_scale = depth_sensor.get_depth_scale() print("Depth Scale is: ", depth_scale) clipping_distance_in_meters = 0.7 clipping_distance = clipping_distance_in_meters / depth_scale # Create opencv window to render image in # cv2.namedWindow("Depth Stream", cv2.WINDOW_AUTOSIZE) align_to = rs.stream.color align = rs.align(align_to)