def capture(pipelines, serial_numbers, prefix, postfix): pc = rs.pointcloud() points = rs.points() for camNo, pipe in enumerate(pipelines): frames = pipe.wait_for_frames() depth_frame = frames.get_depth_frame() color_frame = frames.get_color_frame() pc.map_to(color_frame) points = pc.calculate(depth_frame) points.export_to_ply( prefix + str(serial_numbers[camNo]) + '_' + postfix + '.ply', color_frame) print('Pointcloud saved for camera with serial number', serial_numbers[camNo]) color_image = np.asanyarray(color_frame.get_data()) imsave( prefix + str(serial_numbers[camNo]) + '_' + postfix + 'color_fore.tif', color_image) tex_coor = np.asanyarray(points.get_texture_coordinates_EXT()) imsave( prefix + str(serial_numbers[camNo]) + '_' + postfix + 'texture_fore.tif', tex_coor)
def scan(): # Declare pointcloud object, for calculating pointclouds and texture mappings pc = pyrs.pointcloud() # We want the points object to be persistent so we can display the last cloud when a frame drops points = pyrs.points() # Declare RealSense pipeline, encapsulating the actual device and sensors pipe = pyrs.pipeline() #Start streaming with default recommended configuration profile = pipe.start() # to figgure out the depth scale, normally 0.001 #depth_sensor = profile.get_device().first_depth_sensor() #print depth_sensor.get_depth_scale() try: # Wait for the next set of frames from the camera frames = pipe.wait_for_frames() # Fetch depth frames depth = frames.get_depth_frame() # calculate point cloud points = pc.calculate(depth) # get vertices pts = np.asanyarray(points.get_vertices()) #return the list of vertices to rhino return pts finally: pipe.stop()
def __init__(self): # camera matrix of realsense self.camera_info = CameraInfo() # camera_info.K = [616.3787841796875, 0.0, 434.0303955078125, 0.0, 616.4257202148438, 234.33065795898438, 0.0, 0.0, 1.0] self.camera_info.K = [ 931.6937866210938, 0.0, 624.7894897460938, 0.0, 931.462890625, 360.5186767578125, 0.0, 0.0, 1.0 ] self.camera_info.header.frame_id = 'camera_color_optical_frame' self.camera_info.height = 720 self.camera_info.width = 1280 self.image_size = [640, 480] self.points = rs.points() self.pipeline = rs.pipeline() self.config = rs.config() self.config.enable_stream(rs.stream.color, self.image_size[0], self.image_size[1], rs.format.bgr8, 30) self.config.enable_stream(rs.stream.depth, self.image_size[0], self.image_size[1], rs.format.z16, 30) self.profile = self.pipeline.start(self.config) self.align_to = rs.stream.color self.align = rs.align(self.align_to) self.board_center_x = 0 self.board_center_y = 0 self.board = [[[0, 0], [0, 0], [0, 0]], [[0, 0], [0, 0], [0, 0]], [[0, 0], [0, 0], [0, 0]]] self.h_choice = '' self.R = [0, 0, 255] self.G = [0, 255, 0] self.B = [255, 0, 0]
def capture_RealSense(name): points = rs.points() success, frames = pipeline.try_wait_for_frames(timeout_ms=10) depth_frame = frames.get_depth_frame() other_frame = frames.first(other_stream) color = frames.get_color_frame() depth_frame = decimate.process(depth_frame) if state.postprocessing: for f in filters: depth_frame = f.process(depth_frame) points = pc.calculate(depth_frame) pc.map_to(other_frame) ply = name + ".ply" pcd = name + ".pcd" points.export_to_ply(ply, color) clouding = pcl.load(ply) pcl.save(clouding, pcd) print("Export Reussi")
def __init__(self, camera_configuration_file_path): self._cfg = yaml.load(open(camera_configuration_file_path, 'r'), Loader=yaml.FullLoader) self.width = self._cfg["width"] self.height = self._cfg["height"] self.fps = self._cfg["fps"] self.serial_number = self._cfg["serial_number"] self.points = rs.points() self.pipeline = rs.pipeline() config = rs.config() if self.serial_number != '' and self.serial_number is not None: print('Config for realsense %s' % self.serial_id) # the resolution of color image can be higher than depth image config.enable_stream(rs.stream.infrared, 1, 1280, 720, rs.format.y8, self.fps) config.enable_stream(rs.stream.infrared, 2, 1280, 720, rs.format.y8, self.fps) config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, self.fps) config.enable_stream(rs.stream.color, self.width, self.height, rs.format.bgr8, self.fps) self.profile = self.pipeline.start(config) align_to = rs.stream.color self.align = rs.align(align_to) self.scale = self.get_depth_scale()
def __init__(self, serial_id='', width=1280, height=720, fps=30): self.width = width self.height = height self.fps = fps self.points = rs.points() self.pipeline = rs.pipeline() config = rs.config() if serial_id != '': config.enable_device(serial=serial_id) config.enable_stream(rs.stream.infrared, 1, 1280, 720, rs.format.y8, self.fps) config.enable_stream(rs.stream.infrared, 2, 1280, 720, rs.format.y8, self.fps) config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, self.fps) config.enable_stream(rs.stream.color, self.width, self.height, rs.format.bgr8, self.fps) self.profile = self.pipeline.start(config) align_to = rs.stream.color self.align = rs.align(align_to) for i in range(45): self.getImage()
def get_pointcloud(depth_image, color_frame, img, img_size): point_cloud = rs.pointcloud() points = rs.points() # Obtain point cloud data point_cloud.map_to(color_frame) points = point_cloud.calculate(depth_image) # Convert point cloud to 2d Array points3d = np.asanyarray(points.get_vertices()) points3d = points3d.view(np.float32).reshape(points3d.shape + (-1, )) texture_coords = np.asanyarray(points.get_texture_coordinates()) texture_coords = texture_coords.view( np.float32).reshape(texture_coords.shape + (-1, )) # Remove all invalid data within a certain distance long_distance_mask = points3d[:, 2] < 10 short_distance_mask = points3d[:, 2] > 0.3 distance_mask = np.logical_and(long_distance_mask, short_distance_mask) points3d = points3d[distance_mask] # Sample random points idx = np.random.randint(points3d.shape[0], size=round(points3d.shape[0] / 100)) sampled_points = points3d[idx, :] # Get colours of points point_colors = [] point_colors = np.array(point_colors) return sampled_points, point_colors
def capture_RealSense(name): points = rs.points() global w, h if state.paused: return success, frames = pipeline.try_wait_for_frames(timeout_ms=0) if not success: return depth_frame = frames.get_depth_frame() depth_frame = decimate.process(depth_frame) if state.postprocessing: for f in filters: depth_frame = f.process(depth_frame) # 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 points = pc.calculate(depth_frame) verts = np.array(points.get_vertices(2)) """processes = [] starttime = time.time() for i in verts: p = multiprocessing.Process(target=recup_data_cam, args=(i,)) processes.append(p) p.start() for process in processes: process.join()""" starttime = time.time() pool = multiprocessing.Pool(12) image3d = pool.map(recup_data_cam, verts) pool.close() """for i in verts: for j in i: if 0.5 > j[0] > -0.5: if 0.5 > j[1] > -0.5: if 0.5 > j[2] > -0.5: taille = taille + 1""" print('That took {} seconds'.format(time.time() - starttime)) starttime = time.time() image3d = sup_occurence(image3d) print('That took {} seconds'.format(time.time() - starttime)) print(image3d) """for i in image3d: print("ici") for j in i: print(j) pass""" ttt = input("wait")
def get_pointcloud(depth_image, color_frame, img, img_size): point_cloud = rs.pointcloud() points = rs.points() # Obtain point cloud data point_cloud.map_to(color_frame) points = point_cloud.calculate(depth_image) # Convert point cloud to 2d Array points3d = np.asanyarray(points.get_vertices()) points3d = points3d.view(np.float32).reshape(points3d.shape + (-1, )) texture_coords = np.asanyarray(points.get_texture_coordinates()) texture_coords = texture_coords.view( np.float32).reshape(texture_coords.shape + (-1, )) # Remove all invalid data within a certain distance long_distance_mask = points3d[:, 2] < 10 short_distance_mask = points3d[:, 2] > 0.3 distance_mask = np.logical_and(long_distance_mask, short_distance_mask) points3d = points3d[distance_mask] texture_coords = texture_coords[distance_mask] # Get colours u_coords = ((texture_coords[:, 0]) * img_size[0]) u_coords = np.round(np.clip(u_coords, a_min=0, a_max=img_size[0] - 1)) v_coords = ((texture_coords[:, 1]) * img_size[1]) v_coords = np.round(np.clip(v_coords, a_min=0, a_max=img_size[1] - 1)) uv_coords = np.vstack((u_coords, v_coords)).T.astype(np.uint16) # Sample random points idx = np.random.randint(points3d.shape[0], size=round(points3d.shape[0] / 500)) sampled_points = points3d[idx, :] uv_coords = uv_coords[idx, :] # Add extra column of 0's to 3d points o = np.ones((sampled_points.shape[0], 1)) sampled_points = np.hstack((sampled_points, o)) # Get colours of points point_colors = [] for i, coord in enumerate(uv_coords): cols = img[coord[0], coord[1], :] point_colors.append(cols) point_colors = np.array(point_colors) return sampled_points, point_colors
def __init__(self, size=[640, 480]): self.pipe = rs.pipeline() config = rs.config() config.enable_stream(rs.stream.depth, size[0], size[1], rs.format.z16, 30) config.enable_stream(rs.stream.color, size[0], size[1], rs.format.bgr8, 30) # Start streaming self.profile = self.pipe.start(config) # Declare pointcloud object, for calculating pointclouds and texture mappings self.pc = rs.pointcloud() # We want the points object to be persistent so we can display the last cloud when a frame drops self.points = rs.points() self.init_sensor() print('Sensor Initilization Done!') self.extract_instrics() self.saliency_cut = SaliencyCut()
def export_to_ply(): # Declare pointcloud object, for calculating pointclouds and texture mappings pc = rs2.pointcloud() # We want the points object to be persistent so we can display the last cloud when a frame drops points = rs2.points() # Declare RealSense pipeline, encapsulating the actual device and sensors pipe = rs2.pipeline() config = rs2.config() # Enable depth stream # config.enable_stream(rs2.stream.depth) config.enable_stream(rs2.stream.depth, 640, 480, rs2.format.z16, 30) config.enable_stream(rs2.stream.color, 640, 480, rs2.format.rgb8, 30) # Start streaming with chosen configuration pipe.start(config) # We'll use the colorizer to generate texture for our PLY # (alternatively, texture can be obtained from color or infrared stream) colorizer = rs2.colorizer() colorizer.set_option(rs2.option.color_scheme, 0) try: # Wait for the next set of frames from the camera frames = pipe.wait_for_frames() colorized = colorizer.process(frames) # Create save_to_ply object ply = rs2.save_to_ply("1.ply") # Set options to the desired values # In this example we'll generate a textual PLY with normals (mesh is already created by default) ply.set_option(rs2.save_to_ply.option_ply_binary, True) ply.set_option(rs2.save_to_ply.option_ply_normals, False) print("Saving to 1.ply...") # Apply the processing block to the frameset which contains the depth frame and the texture ply.process(colorized) print("Done") finally: pipe.stop()
def capture(isBack, pipelines, serial_numbers, isFirst, prefix, postfix): if isBack: ground = 'back' else: ground = 'fore' pc = rs.pointcloud() points = rs.points() for camNo, pipe in enumerate(pipelines): frames = pipe.wait_for_frames() depth_frame = frames.get_depth_frame() color_frame = frames.get_color_frame() color_image = np.asanyarray(color_frame.get_data()) if not isBack: pc.map_to(color_frame) points = pc.calculate(depth_frame) points.export_to_ply( prefix + str(serial_numbers[camNo]) + '_' + postfix + ground + '.ply', color_frame) tex_coor = np.asanyarray(points.get_texture_coordinates_EXT()) imsave( prefix + str(serial_numbers[camNo]) + '_' + postfix + 'texture_' + ground + '.tif', tex_coor) print( 'Foreground pointcloud and texture coordinates saved for camera with serial number ', serial_numbers[camNo]) imsave( prefix + str(serial_numbers[camNo]) + '_' + postfix + 'color_' + ground + '.tif', color_image) print( ground + 'ground color image saved for camera with serial number ', serial_numbers[camNo]) if isFirst: input("Place balls in the box and press enter")
def __init__(self, width=1280, hight=720, fps=30): self.width = width self.hight = hight self.fps = fps self.points = rs.points() self.pipeline = rs.pipeline() config = rs.config() config.enable_stream(rs.stream.infrared, 1, self.width, self.hight, rs.format.y8, self.fps) config.enable_stream(rs.stream.infrared, 2, self.width, self.hight, rs.format.y8, self.fps) config.enable_stream(rs.stream.depth, self.width, self.hight, rs.format.z16, self.fps) config.enable_stream(rs.stream.color, self.width, self.hight, rs.format.bgr8, self.fps) profile = self.pipeline.start(config) align_to = rs.stream.color self.align = rs.align(align_to) time.sleep(1)
def scangrid(): # Declare pointcloud object, for calculating pointclouds and texture mappings pc = pyrs.pointcloud() # We want the points object to be persistent so we can display the last cloud when a frame drops points = pyrs.points() # Declare RealSense pipeline, encapsulating the actual device and sensors pipe = pyrs.pipeline() #Start streaming with default recommended configuration profile = pipe.start() # to figgure out the depth scale, normally 0.001 #depth_sensor = profile.get_device().first_depth_sensor() #print depth_sensor.get_depth_scale() try: # Wait for the next set of frames from the camera frames = pipe.wait_for_frames() # Fetch depth frames depth = frames.get_depth_frame() points = pc.calculate(depth) pts = np.asanyarray(points.get_vertices()) # use: from scipy.interpolate import griddata # https://earthscience.stackexchage.com/questions/12057/how-to-interpolate-scattered-data-to-a-regular-grid-in-python/ # use a mask to mask-out the outline.. # values to return to rhino return pts # convert to numpy array #npy_vtx = np.zeros((len(vtx), 3), float) #for i in range(len(vtx)): # npy_vtx[i][0] = np.float(vtx[i][0]) # npy_vtx[i][1] = np.float(vtx[i][1]) # npy_vtx[i][2] = np.float(vtx[i][2]) finally: pipe.stop()
def extract_frames(pipe, cfg, save_path, post_processing=False, save_colorize=True, save_pc=False, visualize=True): """Helper function to align and extract rgb-d image from bagfile. Check more saving options in arguments. Args: pipe: pyrealsense2 pipeline. cfg: pyrealsense2 pipeline configuration. save_path: Path to save the extracted frames. save_colorize: Save colorized depth maps visualization. save_pc: Save point cloud data. visualize: Visualize the video frames during saving. """ # Configurations if save_colorize: colorizer = rs.colorizer() if save_pc: pc = rs.pointcloud() points = rs.points() # Save path if not os.path.exists(save_path): os.makedirs(save_path) # Start the pipe i = 0 profile = pipe.start(cfg) device = profile.get_device() playback = device.as_playback() playback.set_real_time( False) # Make sure this is False or frames get dropped while True: try: # Wait for a conherent pairs of frames: (rgb, depth) pairs = pipe.wait_for_frames() # Align depth image to rgb image first align = rs.align(rs.stream.color) pairs = align.process(pairs) color_frame = pairs.get_color_frame() depth_frame = pairs.get_depth_frame() if not depth_frame or not color_frame: continue # Post-processing if post_processing: depth_frame = post_processing(depth_frame) # Get rgb-d images color_img = np.asanyarray(color_frame.get_data()) color_img = cv2.cvtColor(color_img, cv2.COLOR_RGB2BGR) depth_img = np.asanyarray(depth_frame.get_data()) print('Frame {}, Depth Image {}, Color Image {}'.format( i + 1, depth_img.shape, color_img.shape)) # Save as loseless formats cv2.imwrite(os.path.join(save_path, '{}_rgb.png'.format(i)), color_img, [cv2.IMWRITE_PNG_COMPRESSION, 0]) np.save(os.path.join(save_path, '{}_depth.npy'.format(i)), depth_img) if save_colorize: # Save colorized depth map depth_img_colorized = np.asanyarray( colorizer.colorize(depth_frame).get_data()) cv2.imwrite( os.path.join(save_path, '{}_depth_colorized.jpg'.format(i)), depth_img_colorized) # No need for lossless here if save_pc: # NOTE: Point cloud calculation takes longer time. pc.map_to(color_frame) points = pc.calculate(depth_frame) points.export_to_ply( os.path.join(save_path, '{}_pc.ply'.format(i)), color_frame) i += 1 if visualize: # Stack both images horizontally images = np.vstack((color_img, depth_img_colorized)) # Show images cv2.namedWindow('RealSense', cv2.WINDOW_NORMAL) cv2.imshow('RealSense', images) cv2.waitKey(1) except Exception as e: print(e) break # Clean pipeline pipe.stop() print('{} frames saved in total.'.format(i)) return
def get_pointcloud_frame(bagfile_dir, filter = False): # -------------------------------------------- # # Read the bag file and save point cloud to .ply file # n_img: number of matrix you want to save # interval: number of frames between saved matrix # -------------------------------------------- # input = bagfile_dir try: # Declare pointcloud object, for calculating pointclouds and texture mappings pc = rs.pointcloud() # We want the points object to be persistent so we can display the last cloud when a frame drops points = rs.points() # Create pipeline pipeline = rs.pipeline() # Create a config object config = rs.config() # Tell config that we will use a recorded device from filem to be used by the pipeline through playback. rs.config.enable_device_from_file(config, input) # Configure the pipeline to stream the depth and color stream config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30) config.enable_stream(rs.stream.color, 1280, 720, rs.format.rgb8, 30) # Declare filters dec_filter = rs.decimation_filter() # Decimation - reduces depth frame density spat_filter = rs.spatial_filter() # Spatial - edge-preserving spatial smoothing temp_filter = rs.temporal_filter() # Temporal - reduces temporal noise fill_filter = rs.hole_filling_filter() # Hole Filling filter disp_filter = rs.disparity_transform() # Disparity Transform filter # 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) while(True): # Get frameset of depth frames = pipeline.wait_for_frames() # Get depth frame depth_frame = frames.get_depth_frame() color_frame = frames.get_color_frame() if (filter): # Perform filtering filtered = dec_filter.process(depth_frame) filtered = spat_filter.process(filtered) filtered = fill_filter.process(filtered) depth_frame = temp_filter.process(filtered) color_frame = dec_filter.process(color_frame) # Tell pointcloud object to map to this color frame pc.map_to(color_frame) # Generate the pointcloud and texture mappings points = pc.calculate(depth_frame) # yield points_array = np.asanyarray(points.get_vertices()) yield np.asanyarray(depth_frame.get_data()), points_array finally: pass return
def __init__(self, camera_configuration_file_path): self._cfg = yaml.load(open(camera_configuration_file_path, 'r'), Loader=yaml.FullLoader) self.serial_number = self._cfg["serial_number"] self.points = rs.points() self.pipeline = rs.pipeline() config = rs.config() if self.serial_number != '' and self.serial_number != 'None': print('Config for realsense %s' % self.serial_number) config.enable_device(self.serial_number) # the resolution of color image can be higher than depth image if self._cfg["name"] == 'L515': config.enable_stream(rs.stream.infrared, self._cfg['depth_infrared']['width'], self._cfg['depth_infrared']['height'], rs.format.y8, self._cfg['depth_infrared']['fps']) config.enable_stream(rs.stream.depth, self._cfg['depth_infrared']['width'], self._cfg['depth_infrared']['height'], rs.format.z16, self._cfg['depth_infrared']['fps']) config.enable_stream(rs.stream.color, self._cfg['color']['width'], self._cfg['color']['height'], rs.format.bgr8, self._cfg['color']['fps']) else: config.enable_stream(rs.stream.infrared, 1, self._cfg['depth_infrared']['width'], self._cfg['depth_infrared']['height'], rs.format.y8, self._cfg['depth_infrared']['fps']) config.enable_stream(rs.stream.infrared, 2, self._cfg['depth_infrared']['width'], self._cfg['depth_infrared']['height'], rs.format.y8, self._cfg['depth_infrared']['fps']) config.enable_stream(rs.stream.depth, self._cfg['depth_infrared']['width'], self._cfg['depth_infrared']['height'], rs.format.z16, self._cfg['depth_infrared']['fps']) config.enable_stream(rs.stream.color, self._cfg['color']['width'], self._cfg['color']['height'], rs.format.bgr8, self._cfg['color']['fps']) self.profile = self.pipeline.start(config) if "exposure" in self._cfg['color'].keys( ) and self._cfg['color']['exposure'] != 'None': # set camera parameters for i in range(len(self.profile.get_device().sensors)): if self.profile.get_device().sensors[i].is_color_sensor(): # set color camera color_sensor = self.profile.get_device().sensors[i] color_sensor.set_option(rs.option.enable_auto_exposure, 0) color_sensor.set_option(rs.option.exposure, self._cfg['color']['exposure']) # color_sensor.set_option(rs.option.gain, 100) print("camera exposure: ", color_sensor.get_option(rs.option.exposure) * 0.1, 'ms') ''' The align class used in librealsense demos maps between depth and some other stream and vice versa. Realsense do not offer other forms of stream alignments. https://github.com/IntelRealSense/librealsense/issues/1556 ''' self.basic_img = self._cfg["align_image"] if self._cfg["align_image"] == 'color': align_to = rs.stream.color elif self._cfg["align_image"] == 'depth': align_to = rs.stream.depth else: print("Error align image!") print("Align to color!") align_to = rs.stream.color self.align = rs.align(align_to) self.scale = self.get_depth_scale()
def main(): #initialize node rospy.init_node('image_save') rospy.Subscriber("chatter", Point, callback_pos) rospy.Subscriber("chatter_ori", Point, callback_ori) rospy.Subscriber("/camera/color/image_raw", Image, realsense_callback) rospy.Subscriber("/camera/aligned_depth_to_color/image_raw", Image, realsense_callback_depth) # Declare pointcloud object, for calculating pointclouds and texture mappings pc = rs.pointcloud() # We want the points object to be persistent so we can display the last cloud when a frame drops points = rs.points() # Declare RealSense pipeline, encapsulating the actual device and sensors pipe = rs.pipeline() config = rs.config() config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 15) config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 15) # 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) #Start streaming with default recommended configuration pipe.start(config) try: # Wait for the next set of frames from the camera frames = pipe.wait_for_frames() # unaligned #depth_frame = frames.get_depth_frame() is a 640x360 depth image #color_frame = frames.get_color_frame() # 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() if not aligned_depth_frame or not color_frame: pass # Convert images to numpy arrays depth_image = np.asanyarray(aligned_depth_frame.get_data()) color_image = np.asanyarray(color_frame.get_data()) # Apply colormap on depth image (image must be converted to 8-bit per pixel first) #depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET) points = pc.calculate(aligned_depth_frame) images = np.hstack((color_image, depth_image)) cv2.namedWindow('Align Example', cv2.WINDOW_AUTOSIZE) cv2.imshow('Align Example', images) key = cv2.waitKey(1) # Press esc or 'q' to close the image window if key & 0xFF == ord('q') or key == 27: cv2.destroyAllWindows() #name = "1" #print("Saving to 1.ply...") #points.export_to_ply(name + ".ply", color_frame) #cv2.imwrite(name + ".png",depth_image) #cv2.imwrite(name + ".jpg",color_image) #print("Done") finally: pipe.stop() processFlag = False rospy.spin()
def main(): # Declare pointcloud object, for calculating pointclouds and texture mappings pc = rs.pointcloud() # We want the points object to be persistent so we can display the last cloud when a frame drops points = rs.points() pipe = rs.pipeline() cfg = rs.config() rs.config.enable_device_from_file(cfg, './20191211_160154.bag', repeat_playback=False) cfg.enable_stream(rs.stream.color, 424, 240, rs.format.rgb8, 6) # color camera cfg.enable_stream(rs.stream.depth, 424, 240, rs.format.z16, 6) # depth camera # pipe.start(cfg) profile = pipe.start(cfg) playback = profile.get_device().as_playback() playback.set_real_time(False) # define Filters thres_filter = rs.threshold_filter() depth_to_disparity = rs.disparity_transform(True) spat_filter = rs.spatial_filter() temp_filter = rs.temporal_filter() # hole_fill_filter = rs.hole_filling_filter() disparity_to_depth = rs.disparity_transform(False) i = 0 while True: try: frames = pipe.wait_for_frames() except: break print(i) depth_frame = frames.get_depth_frame() color_frame = frames.get_color_frame() # process depth image if True: depth_frame = thres_filter.process(depth_frame) depth_frame = depth_to_disparity.process(depth_frame) depth_frame = spat_filter.process(depth_frame) depth_frame = temp_filter.process(depth_frame) # depth_frame = hole_fill_filter.process(depth_frame) depth_frame = disparity_to_depth.process(depth_frame) depth_image = np.asanyarray(depth_frame.get_data()) color_image = np.asanyarray(color_frame.get_data()) depth_colormap = cv2.applyColorMap( cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET) points = pc.calculate(depth_frame) cloud = points_to_pcl(points) cloud_normals, cluster_indices = region_growing_segmentation(cloud) idx = choose_flat_plane(cluster_indices, cloud_normals) chosen_plane_indices = cluster_indices[idx] arr = cloud.to_array() plane_points = arr[chosen_plane_indices] plane_coef = fit_plane(plane_points) plane_filtered_indices = find_close_points(cloud, plane_coef) processed = mark_pixels(depth_colormap, 424, plane_filtered_indices) # Dilation img_processed = image_post_process(processed) cv2.imwrite("./images/proc_" + str(i) + ".png", img_processed) i += 1
# imports import pyrealsense2 as rs2 import openmesh as om from os import remove # get the file name of the stl file (and make sure it ends with '.stl') filename = input("Enter the file name of the stl file: ") if filename[-4:] != ".stl": filename += ".stl" print("Taking a depth frame") # declare pointcloud object, for calculating pointclouds and texture mappings pc: rs2.pointcloud = rs2.pointcloud() # we want the points object to be persistent so we can display the last cloud when a frame drops points: rs2.points = rs2.points() # declare RealSense pipeline, encapsulating the actual device and sensors pipe: rs2.pipeline = rs2.pipeline() config: rs2.config = rs2.config() # enable depth stream config.enable_stream(rs2.stream.depth) # start streaming with chosen configuration pipe.start(config) # we'll use the colorizer to generate texture for our PLY # (alternatively, texture can be obtained from color or infrared stream) colorizer: rs2.colorizer = rs2.colorizer() # create a thresh filter with max distance of 0.7 meter
import pyrealsense2 as rs import numpy as np rs.pipeline() rs.pipeline.try_wait_for_frames() rs.pipeline.wait_for_frames() rs.pipeline_profile() rs.pipeline_profile.get_device() rs.pipeline_profile.get_stream() rs.pipeline_profile.get_streams() rs.pipeline_wrapper() rs.playback() rs.playback_status() rs.points() rs.pose() rs.pose_frame() rs.pose_stream_profile() rs.pipeline rs.pipeline.get_active_profile() rs.pipeline.poll_for_frames() rs.pipeline.start() rs.pipeline.stop() rs.pipeline.wait_for_frames() rs.pipeline.try_wait_for_frames() pipe = rs.pipeline() config = rs.config() config.enable_stream config.enable_all_streams config.enable_device
def run(dt): points = rs.points() global w, h window.set_caption("RealSense (%dx%d) %dFPS (%.2fms) %s" % (w, h, 0 if dt == 0 else 1.0 / dt, dt * 1000, "PAUSED" if state.paused else "")) if state.paused: return success, frames = pipeline.try_wait_for_frames(timeout_ms=0) if not success: return depth_frame = frames.get_depth_frame() other_frame = frames.first(other_stream) color = frames.get_color_frame() depth_frame = decimate.process(depth_frame) if state.postprocessing: for f in filters: depth_frame = f.process(depth_frame) # 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 color_image = np.asanyarray(other_frame.get_data()) colorized_depth = colorizer.colorize(depth_frame) depth_colormap = np.asanyarray(colorized_depth.get_data()) if state.color: mapped_frame, color_source = other_frame, color_image else: mapped_frame, color_source = colorized_depth, depth_colormap points = pc.calculate(depth_frame) pc.map_to(mapped_frame) # handle color source or size change fmt = convert_fmt(mapped_frame.profile.format()) global image_data if (image_data.format, image_data.pitch) != (fmt, color_source.strides[0]): empty = (gl.GLubyte * (w * h * 3))() image_data = pyglet.image.ImageData(w, h, fmt, empty) # copy image deta to pyglet image_data.set_data(fmt, color_source.strides[0], color_source.ctypes.data) verts = np.asarray(points.get_vertices(2)).reshape(h, w, 3) texcoords = np.asarray(points.get_texture_coordinates(2)) if len(vertex_list.vertices) != verts.size: vertex_list.resize(verts.size // 3) # need to reassign after resizing vertex_list.vertices = verts.ravel() vertex_list.tex_coords = texcoords.ravel() copy(vertex_list.vertices, verts) copy(vertex_list.tex_coords, texcoords)
def get_frame(): # Create a pipeline pipeline = rs.pipeline() print('pipeline started') # Create a config and configure the pipeline to stream # different resolutions of color and depth streams config = rs.config() print("Setting up config of running in live data") config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30) config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30) # Getting the depth sensor's depth scale (see rs-align example for explanation) 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) # We will be removing the background of objects more than # clipping_distance_in_meters meters away #clipping_distance_in_meters = 15 #10 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) while True: # Declare pointcloud object, for calculating pointclouds and texture mappings pc = rs.pointcloud() # We want the points object to be persistent so we can display the last cloud when a frame drops points = rs.points() # 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() # Tell pointcloud object to map to this color frame pc.map_to(color_frame) # Generate the pointcloud and texture mappings points = pc.calculate(aligned_depth_frame) points_array = np.asanyarray(points.get_vertices()) # Validate that both frames are valid if not aligned_depth_frame or not color_frame: continue depth_image = np.asanyarray(aligned_depth_frame.get_data()) color_image = np.asanyarray(color_frame.get_data()) # Remove background - Set pixels further than clipping_distance to grey grey_color = 153 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) # Render images depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET) #images = np.hstack((bg_removed, depth_colormap)) yield color_image, depth_image, points_array
def camera_setup(): dir_path = os.path.dirname(os.path.realpath(__file__)) # Append to syspath the path to openpose python build sys.path.append('/home/naoskin/openpose140/build/python') try: from openpose import * except: raise Exception( 'Error: OpenPose library could not be found. Did you enable `BUILD_PYTHON` in CMake and have this Python script in the right folder?' ) # Parameters for OpenPose. # Take a look at C++ OpenPose example for meaning of components. params = dict() params["logging_level"] = 4 params["output_resolution"] = "-1x-1" params["net_resolution"] = "-1x480" params["model_pose"] = "COCO" params["alpha_pose"] = 0.6 params["scale_gap"] = 0.3 params["scale_number"] = 1 params["render_threshold"] = 0.05 params["render_pose"] = True params["num_gpu_start"] = 0 params["disable_blending"] = False params["number_people_max"] = 1 params["download_heatmaps"] = False params["heatmaps_add_parts"] = True params["heatmaps_add_PAFs"] = True # params["tracking"] = 5 # params["number_people_max"] = 1 # Ensure you point to the correct path where models are located params["default_model_folder"] = "/home/naoskin/openpose140/models/" params["display"] = 0 # Construct OpenPose object allocates GPU memory openpose = OpenPose(params) depth_fps = 90 # setup_advanced_camera() # Declare pointcloud object, for calculating pointclouds and texture mappings pc = rs.pointcloud() # We want the points object to be persistent so we can display the last cloud when a frame drops points = rs.points() # 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() # This is the minimal recommended resolution config.enable_stream(rs.stream.depth, 848, 480, rs.format.z16, 90) config.enable_stream(rs.stream.color, 848, 480, rs.format.bgr8, 30) # Start streaming profile = pipeline.start(config) # Getting the depth sensor's depth scale (see rs-align example for explanation) depth_sensor = profile.get_device().first_depth_sensor() depth_scale = depth_sensor.get_depth_scale() # Create an align object # rs.align allows us to perform alignment of depth frames to others frames # The "align_to" is the stream type to which we plan to align depth frames. # align_to = rs.stream.depth align_to = rs.stream.color align = rs.align(align_to) print("Camera setup ready.") return align, depth_scale, openpose, pc, points, pipeline, profile
## License: Apache 2.0. See LICENSE file in root directory. ## Copyright(c) 2017 Intel Corporation. All Rights Reserved. ##################################################### ## Export to PLY ## ##################################################### # First import the library import pyrealsense2 as rs # Declare pointcloud object, for calculating pointclouds and texture mappings pc = rs.pointcloud() # We want the points object to be persistent so we can display the last cloud when a frame drops points = rs.points() # Declare RealSense pipeline, encapsulating the actual device and sensors pipe = rs.pipeline() #Start streaming with default recommended configuration pipe.start(); try: # Wait for the next set of frames from the camera frames = pipe.wait_for_frames() # Fetch color and depth frames depth = frames.get_depth_frame() color = frames.get_color_frame() # Tell pointcloud object to map to this color frame pc.map_to(color)
def getDepthFrames(nImages=10, resolution=(1280, 720), fps=30, sleepTime=5, laser=False, outputFolder='./'): if nImages < 1: raise ValueError() if sleepTime < 0: raise ValueError() if not os.path.isdir(outputFolder): os.makedirs(outputFolder) pointcloud = rs.pointcloud() # type: rs.pointcloud points = rs.points() # type: rs.points config = rs.config() # type: rs.config config.enable_stream(rs.stream.depth, resolution[0], resolution[1], rs.format.z16, fps) config.enable_stream(rs.stream.color, resolution[0], resolution[1], rs.format.rgb8, fps) align = rs.align(rs.stream.depth) # type: rs.align pipeline = rs.pipeline() # type: rs.pipeline profile = pipeline.start(config) # type: rs.pipeline_profile device = profile.get_device() # type: rs.device depthSensor = device.first_depth_sensor() # type: rs.depth_sensor if depthSensor.supports(rs.option.emitter_enabled): depthSensor.set_option(rs.option.emitter_enabled, 1.0 if laser else 0.0) elif laser: raise EnvironmentError('Device does not support laser') np.savez( os.path.join(outputFolder, 'depth.scale.npz'), depthscale=depthSensor.get_depth_scale(), ) counter = 0 print('Writing depth intrinsics') print('') while True: frames = pipeline.wait_for_frames() # type: rs.frameset alignedFrames = align.process(frames) # type: rs.frameset depth = alignedFrames.get_depth_frame() # type: rs.depth_frame if depth: depthIntrinsics = intrinsics_to_dict( depth.get_profile().as_video_stream_profile().get_intrinsics()) with open(os.path.join(outputFolder, 'depth.intrinsics.pkl'), 'wb') as f: pickle.dump(depthIntrinsics, f) break while True: print('Waiting for frame {:d}'.format(counter)) frames = pipeline.wait_for_frames() # type: rs.frameset alignedFrames = align.process(frames) # type: rs.frameset depth = alignedFrames.get_depth_frame() # type: rs.depth_frame color = alignedFrames.get_color_frame() # type: rs.video_frame if not depth: print('No depth data, retry...') continue if not color: print('No color data, retry...') continue print('Depth data available') depthImage = np.asanyarray(depth.get_data()) print('Writing raw numpy file') np.savez(os.path.join(outputFolder, '{:d}.depth.image.npz'.format(counter)), depthImage=depthImage) print('Generating point cloud') pointcloud.map_to(color) points = pointcloud.calculate(depth) # type: rs.points print('Writing point cloud to PLY file') points.export_to_ply( os.path.join(outputFolder, '{:d}.pointcloud.ply'.format(counter)), color) counter = counter + 1 if counter >= nImages: break print('Sleeping before next frame') print('') time.sleep(sleepTime) pipeline.stop()
def main(): # ROS node initialization rospy.init_node('perception', anonymous=True) moveit_commander.roscpp_initialize(sys.argv) robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() group = moveit_commander.MoveGroupCommander("manipulator") group.set_planner_id('RRTConnectkConfigDefault') group.set_num_planning_attempts(5) group.set_planning_time(5) group.set_max_velocity_scaling_factor(0.5) # get the transformation between robot base and camera tfBuffer = tf2_ros.Buffer(rospy.Duration(1200.0)) listener = tf2_ros.TransformListener(tfBuffer) try: trans = tfBuffer.lookup_transform('world', 'camera_color_optical_frame', rospy.Time(0), rospy.Duration(1.0)) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): print "Find transform failed" # define and go to the home pose waiting for picking instruction home_joint_position = [-80.25/180*3.14, 2.0/180*3.14, 125.8/180*3.14, 34.1/180*3.14, 96.1/180*3.14, 12.7/180*3.14] group.set_joint_value_target(home_joint_position) plan = group.plan() # raw_input('Press enter to go the home position: ') group.execute(plan) time.sleep(1) # initiate game game = TicTacToe() # initiate realsense points = rs.points() pipeline= rs.pipeline() config = rs.config() config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30) config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30) profile = pipeline.start(config) depth_sensor = profile.get_device().first_depth_sensor() depth_scale = depth_sensor.get_depth_scale() align_to = rs.stream.color align = rs.align(align_to) # capture the initial image, time.sleep(3) frames = pipeline.wait_for_frames() aligned_frames = align.process(frames) aligned_depth_frame = aligned_frames.get_depth_frame() color_frame = aligned_frames.get_color_frame() depth_image_background = np.asanyarray(aligned_depth_frame.get_data()) color_image_background = np.asanyarray(color_frame.get_data()) # TODO: detect the tic tac toe game board, compute all the 9 positions of placing X and O # TODO: define a function to detect the human's move from color image def detect_person_move(color_image, game): pass # start the game game.init_game() while len(game.empty_cells(game.board)) > 0 and game.gameOver() == False: # TODO: Ask the human to place her piece X, detect the move and update the state of the game board if game.first == 'N': game.ai_turn(self.c_choice, self.h_choice) game.first = '' game.human_turn(self.c_choice, self.h_choice, detector) game.ai_turn(game.c_choice, game.h_choice) if game.gameOver() == True: break # TODO: calculate robot's move and execute it print("Game Over. " + game.whoWon() + " Wins")
def main(): # Create object for parsing command-line options parser = argparse.ArgumentParser(description="Read recorded bag file and display depth stream in jet colormap.\ Remember to change the stream resolution, fps and format to match the recorded.") # Add argument which takes path to a bag file as an input parser.add_argument("-b", "--bag", type=str, help="Path to the bag file", default="/home/cel/Umich/EECS545/FinalProject/data/RS/A/A_9.bag") parser.add_argument("-d", "--depth_map_path", type=str, help="Path to the output depth map h5 file", default="/home/cel/Umich/EECS545/FinalProject/data/RS/A/rs_a_9_depth_map.h5") parser.add_argument("-df", "--depth_folder", type=str, help="Path to the output depth image folder", default="/home/cel/Umich/EECS545/FinalProject/data/RS/A/rs_a_9_depth_npy/") parser.add_argument("-p", "--point_cloud_path", type=str, help="Path to the output point cloud h5 file", default="/home/cel/Umich/EECS545/FinalProject/data/RS/A/rs_a_9_point_cloud.h5") parser.add_argument("-pf", "--point_cloud_folder", type=str, help="Path to the output point cloud h5 file", default="/home/cel/Umich/EECS545/FinalProject/data/RS/A/rs_a_9_point_clouds/") parser.add_argument("-if", "--image_folder", type=str, help="Path to the output image folder", default="/home/cel/Umich/EECS545/FinalProject/data/RS/A/rs_a_9_test_images/") parser.add_argument("-dif", "--depth_image_folder", type=str, help="Path to the output image folder", default="/home/cel/Umich/EECS545/FinalProject/data/RS/A/rs_a_9_depth_images/") # Parse the command line arguments to an object args = parser.parse_args() # Safety if no parameter have been given if not args.bag: print("No input paramater have been given.") print("For help type --help") exit() # Check if the given file have bag extension if os.path.splitext(args.bag)[1] != ".bag": print("The given file is not of correct file format.") print("Only .bag files are accepted") exit() # remove existing hdf5 file try: os.remove(args.depth_map_path) os.remove(args.point_cloud_path) except: print('no previous h5 file') # creat path for saving images try: os.mkdir(args.image_folder) except: print('image folder already exist') try: os.mkdir(args.depth_folder) except: print('depth folder already exist') try: os.mkdir(args.point_cloud_folder) except: print('point cloud folder already exist') try: os.mkdir(args.depth_image_folder) except: print('depth image folder already exist') W = 424 H = 240 # write our own h5 file from realsense bag f_rs_depth = h5py.File(args.depth_map_path, "a") depth_map_id = f_rs_depth.create_dataset('id', (1,), maxshape=(None,), dtype='uint8', chunks=(1,)) depth_map_from_bag = f_rs_depth.create_dataset('data', (1,H,W), maxshape=(None,H,W), dtype='float16', chunks=(1,H,W)) depth_map_id[0] = 0 depth_map_from_bag[0, :, :] = np.zeros((H, W)) f_rs_point_cloud = h5py.File(args.point_cloud_path, "a") pcd_id = f_rs_point_cloud.create_dataset('id', (1,), maxshape=(None,), dtype='uint8', chunks=(1,)) pcd_from_bag = f_rs_point_cloud.create_dataset('data', (1,W*H,3), maxshape=(None,W*H,3), dtype='float16', chunks=(1,W*H,3)) pcd_id[0] = 0 pcd_from_bag[0, :, :] = np.zeros((W*H, 3)) # initialize index_list = [] try: # Create pipeline pipeline = rs.pipeline() # Create a config object config = rs.config() # Tell config that we will use a recorded device from filem to be used by the pipeline through playback. rs.config.enable_device_from_file(config, args.bag, repeat_playback=False) # Configure the pipeline to stream the depth stream & color stream config.enable_stream(rs.stream.depth, W, H, rs.format.z16, 30) config.enable_stream(rs.stream.color, W, H, rs.format.rgb8, 30) # Start streaming from file pipeline.start(config) index = 0 # Create colorizer object colorizer = rs.colorizer() # Streaming loop while True: # Get frameset of depth frames = pipeline.wait_for_frames() # Get depth frame depth_frame = frames.get_depth_frame() # Get color frame color_frame = frames.get_color_frame() # save to list index_list.append(index) # save depth image depth_image = np.asanyarray(depth_frame.get_data()) / 1000 np.save(args.depth_folder+str(index)+".npy", depth_image) # Colorize depth frame to jet colormap depth_color_frame = colorizer.colorize(depth_frame) # Convert depth_frame to numpy array to render image in opencv depth_color_image = np.asanyarray(depth_color_frame.get_data()) depth_color_image_flipped = cv2.flip(depth_color_image, 1) cv2.imwrite(args.depth_image_folder+str(index)+".png", depth_color_image_flipped) # save color image color_image = np.asanyarray(color_frame.get_data()) color_image = cv2.cvtColor(color_image, cv2.COLOR_BGR2RGB) cv2.imwrite(args.image_folder+str(index)+".png", color_image) # save point cloud pc = rs.pointcloud() points = rs.points() pc.map_to(color_frame) points = pc.calculate(depth_frame) coordinates = np.ndarray(buffer=points.get_vertices(), dtype=np.float16, shape=(W*H, 3)) np.save(args.point_cloud_folder+str(index)+".npy", coordinates) index += 1 except RuntimeError: print("No more frames arrived, reached end of BAG file!") finally: print('total', len(index_list), 'frames read') for index in index_list: depth_npy = np.load(args.depth_folder+str(index)+".npy") point_cloud_npy = np.load(args.point_cloud_folder+str(index)+".npy") # save depth map if index == 0: # depth depth_map_from_bag[-1:,:,:] = depth_npy # point cloud pcd_from_bag[-1:,:,:] = point_cloud_npy else: # depth depth_map_id.resize(depth_map_id.shape[0]+1, axis=0) depth_map_id[-1:] = index depth_map_from_bag.resize((depth_map_from_bag.shape[0]+1, depth_map_from_bag.shape[1], depth_map_from_bag.shape[2])) depth_map_from_bag[-1:,:,:] = depth_npy # point cloud pcd_id.resize(pcd_id.shape[0]+1, axis=0) pcd_id[-1:] = index pcd_from_bag.resize((pcd_from_bag.shape[0]+1, pcd_from_bag.shape[1], pcd_from_bag.shape[2])) pcd_from_bag[-1:,:,:] = point_cloud_npy print('depth map h5 file saved:', depth_map_from_bag.shape) print('point cloud h5 file saved:', pcd_from_bag.shape) f_rs_depth.close() f_rs_point_cloud.close()
def save_pointcloud_to_ply(self, bagfile_dir, n_img=50, interval=1, filter=False): # -------------------------------------------- # # Read the bag file and save point cloud to .ply file # n_img: number of matrix you want to save # interval: number of frames between saved matrix # -------------------------------------------- # import pyrealsense2 as rs try: # Declare pointcloud object, for calculating pointclouds and texture mappings pc = rs.pointcloud() # We want the points object to be persistent so we can display the last cloud when a frame drops points = rs.points() # Create pipeline pipeline = rs.pipeline() # Create a config object config = rs.config() # Tell config that we will use a recorded device from filem to be used by the pipeline through playback. rs.config.enable_device_from_file(config, bagfile_dir) # Configure the pipeline to stream the depth and color stream #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_stream(rs.stream.depth, 640, 480, rs.format.z16, 30) config.enable_stream(rs.stream.color, 640, 480, rs.format.rgb8, 30) # Start streaming from file profile = pipeline.start(config) # Declare filters #dec_filter = rs.decimation_filter() # Decimation - reduces depth frame density spat_filter = rs.spatial_filter( ) # Spatial - edge-preserving spatial smoothing temp_filter = rs.temporal_filter( ) # Temporal - reduces temporal noise fill_filter = rs.hole_filling_filter() # Hole Filling filter disp_filter = rs.disparity_transform( ) # Disparity Transform filter depth_sensor = profile.get_device().first_depth_sensor() depth_scale = depth_sensor.get_depth_scale() print("Depth Scale is: ", depth_scale) count = 0 for i in range((n_img - 1) * interval + 1): if (i % interval != 0): # Get frameset of depth frames = pipeline.wait_for_frames() continue else: # Get frameset of depth frames = pipeline.wait_for_frames() # Get depth frame depth_frame = frames.get_depth_frame() color_frame = frames.get_color_frame() if (filter): # Perform filtering #filtered = dec_filter.process(depth_frame) filtered = spat_filter.process(depth_frame) #filtered = fill_filter.process(filtered) depth_frame = temp_filter.process(filtered) #color_frame = dec_filter.process(color_frame) # Tell pointcloud object to map to this color frame pc.map_to(color_frame) # Generate the pointcloud and texture mappings points = pc.calculate(depth_frame) # generate filename filename = self.save_dir + "pc" + "%04d" % count + ".ply" # Save raw data into numpy matrix file points.export_to_ply(filename, color_frame) count += 1 print("save %d th point cloud from %d th frame to" % (count, i) + filename) finally: pass return
uStep = MICRO_STEPS.ustep_8 mechGain = MECH_GAIN.ballscrew_10mm_turn mm.configAxis(axis, uStep, mechGain) print("Axis " + str(axis) + " configured with " + str(uStep) + " microstepping and " + str(mechGain) + "mm/turn mechanical gain") # Configure the axis direction ax_direction = DIRECTION.POSITIVE mm.configAxisDirection(axis, ax_direction) print("Axis direction set to " + str(ax_direction)) #New variable declaration # Declare pointcloud object, for calculating pointclouds and texture mappings pc = rs.pointcloud() # We want the points object to be persistent so we can display the last cloud when a frame drops points = rs.points() ### real sense initialization #### pipeline = rs.pipeline() config = rs.config() config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30) config.enable_stream(rs.stream.color, 1920, 1080, rs.format.rgb8, 30) print('Real sense depth and rgb streams initialized') # Home mm at the beginning of each new rep and treatment if mm.getCurrentPositions()[2] != 0.0: mm.configHomingSpeed(axis, 5) mm.emitHome(axis) print('mm is going home')
def detect(save_img=False): # Declare pointcloud object, for calculating pointclouds and texture mappings pc = rs.pointcloud() # We want the points object to be persistent so we can display the last cloud when a frame drops points = rs.points() 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.bgr8, 30) # Start streaming pipe_profile = pipeline.start(config) # 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) frames = pipeline.wait_for_frames() aligned_frames = align.process(frames) depth_frame = aligned_frames.get_depth_frame() color_frame = aligned_frames.get_color_frame() #time.sleep(8) img_color = np.array(color_frame.get_data()) img_depth = np.array(depth_frame.get_data()) cv2.imwrite('/home/xcy/workspace/read.jpg', img_color) #detection section img_size = ( 320, 192 ) if ONNX_EXPORT else opt.img_size # (320, 192) or (416, 256) or (608, 352) for (height, width) out, source, weights, half, view_img, save_txt = opt.output, opt.source, opt.weights, opt.half, opt.view_img, opt.save_txt webcam = source == '0' or source.startswith('rtsp') or source.startswith( 'http') or source.endswith('.txt') # Initialize device = torch_utils.select_device( device='cpu' if ONNX_EXPORT else opt.device) if os.path.exists(out): shutil.rmtree(out) # delete output folder os.makedirs(out) # make new output folder # Initialize model model = Darknet(opt.cfg, img_size) # Load weights attempt_download(weights) if weights.endswith('.pt'): # pytorch format model.load_state_dict( torch.load(weights, map_location=device)['model']) else: # darknet format load_darknet_weights(model, weights) # Second-stage classifier classify = False if classify: modelc = torch_utils.load_classifier(name='resnet101', n=2) # initialize modelc.load_state_dict( torch.load('weights/resnet101.pt', map_location=device)['model']) # load weights modelc.to(device).eval() # Eval mode model.to(device).eval() # Fuse Conv2d + BatchNorm2d layers # model.fuse() # Export mode if ONNX_EXPORT: model.fuse() img = torch.zeros((1, 3) + img_size) # (1, 3, 320, 192) f = opt.weights.replace(opt.weights.split('.')[-1], 'onnx') # *.onnx filename torch.onnx.export(model, img, f, verbose=False, opset_version=11) # Validate exported model import onnx model = onnx.load(f) # Load the ONNX model onnx.checker.check_model(model) # Check that the IR is well formed print(onnx.helper.printable_graph( model.graph)) # Print a human readable representation of the graph return # Half precision half = half and device.type != 'cpu' # half precision only supported on CUDA if half: model.half() # Set Dataloader vid_path, vid_writer = None, None if webcam: view_img = True torch.backends.cudnn.benchmark = True # set True to speed up constant image size inference dataset = LoadStreams(source, img_size=img_size) else: save_img = True dataset = LoadImages(source, img_size=img_size) # Get names and colors names = load_classes(opt.names) colors = [[random.randint(0, 255) for _ in range(3)] for _ in range(len(names))] # Run inference t0 = time.time() _ = model(torch.zeros( (1, 3, img_size, img_size), device=device)) if device.type != 'cpu' else None # run once for path, img, im0s, vid_cap in dataset: img = torch.from_numpy(img).to(device) img = img.half() if half else img.float() # uint8 to fp16/32 img /= 255.0 # 0 - 255 to 0.0 - 1.0 if img.ndimension() == 3: img = img.unsqueeze(0) # Inference t1 = torch_utils.time_synchronized() pred = model(img, augment=opt.augment)[0] t2 = torch_utils.time_synchronized() # to float if half: pred = pred.float() # Apply NMS pred = non_max_suppression(pred, opt.conf_thres, opt.iou_thres, multi_label=False, classes=opt.classes, agnostic=opt.agnostic_nms) # Apply Classifier if classify: pred = apply_classifier(pred, modelc, img, im0s) # Process detections for i, det in enumerate(pred): # detections per image if webcam: # batch_size >= 1 p, s, im0 = path[i], '%g: ' % i, im0s[i] else: p, s, im0 = path, '', im0s save_path = str(Path(out) / Path(p).name) s += '%gx%g ' % img.shape[2:] # print string if det is not None and len(det): # Rescale boxes from img_size to im0 size det[:, :4] = scale_coords(img.shape[2:], det[:, :4], im0.shape).round() # Print results for c in det[:, -1].unique(): n = (det[:, -1] == c).sum() # detections per class s += '%g %ss, ' % (n, names[int(c)]) # add to string stop_num = 0 #print('c = ',len(det)) # Write results for *xyxy, conf, cls in det: c1, c2 = (int(xyxy[0]), int(xyxy[1])), (int(xyxy[2]), int(xyxy[3])) c0 = ((int(xyxy[0]) + int(xyxy[2])) / 2, (int(xyxy[1]) + int(xyxy[3])) / 2) if save_txt: # Write to file with open(save_path + '.txt', 'a') as file: file.write(('%g ' * 6 + '\n') % (*xyxy, cls, conf)) if save_img or view_img: # Add bbox to image label = '%s %.2f' % (names[int(cls)], conf) plot_one_box(xyxy, im0, label=label, color=colors[int(cls)]) #print(c0) if int(cls) == opt.targetclass: x_center = int((int(xyxy[0]) + int(xyxy[2])) / 2) y_center = int((int(xyxy[1]) + int(xyxy[3])) / 2) # Intrinsics and Extrinsics depth_intrin = depth_frame.profile.as_video_stream_profile( ).intrinsics color_intrin = color_frame.profile.as_video_stream_profile( ).intrinsics depth_to_color_extrin = depth_frame.profile.get_extrinsics_to( color_frame.profile) # Depth scale: units of the values inside a depth frame, how to convert the value to units of 1 meter depth_sensor = pipe_profile.get_device( ).first_depth_sensor() depth_scale = depth_sensor.get_depth_scale() # Map depth to color depth_pixel = [240, 320] # Random pixel depth_point = rs.rs2_deproject_pixel_to_point( depth_intrin, depth_pixel, depth_scale) color_point = rs.rs2_transform_point_to_point( depth_to_color_extrin, depth_point) color_pixel = rs.rs2_project_point_to_pixel( color_intrin, color_point) pc.map_to(color_frame) points = pc.calculate(depth_frame) vtx = np.array(points.get_vertices()) tex = np.array(points.get_texture_coordinates()) pix_num = 1280 * y_center + x_center point_cloud_value = [ np.float(vtx[pix_num][0]), np.float(vtx[pix_num][1]), np.float(vtx[pix_num][2]) ] print('point_cloud_value:', point_cloud_value, names[int(cls)], int(cls)) #if not np.isnan(np.float(vtx[pix_num][0])) and not np.isinf(np.float(vtx[pix_num][0])): # os.system('rostopic pub -1 /goal_pose geometry_msgs/PointStamped [0,[0,0],zed_left_camera_optical_frame] [%s,%s,%s]' %(np.float(vtx[pix_num][0]),np.float(vtx[pix_num][1]),np.float(vtx[pix_num][2])) #os.system('rostopic pub -1 /start_plan std_msgs/Bool 1') #else: #os.system('rostopic pub -1 /start_plan std_msgs/Bool 0') #print("Can't estimate point cloud at this position.") break #only the target class #stop_num += 1 #if stop_num >= len(det): # os.system('rostopic pub -1 /start_plan std_msgs/Bool 0') # Print time (inference + NMS) print('%sDone. (%.3fs)' % (s, t2 - t1)) # Stream results if view_img: cv2.imshow(p, im0) if cv2.waitKey(1) == ord('q'): # q to quit raise StopIteration # Save results (image with detections) if save_img: if dataset.mode == 'images': cv2.imwrite(save_path, im0) else: if vid_path != save_path: # new video vid_path = save_path if isinstance(vid_writer, cv2.VideoWriter): vid_writer.release( ) # release previous video writer fps = vid_cap.get(cv2.CAP_PROP_FPS) w = int(vid_cap.get(cv2.CAP_PROP_FRAME_WIDTH)) h = int(vid_cap.get(cv2.CAP_PROP_FRAME_HEIGHT)) vid_writer = cv2.VideoWriter( save_path, cv2.VideoWriter_fourcc(*opt.fourcc), fps, (w, h)) vid_writer.write(im0) if save_txt or save_img: print('Results saved to %s' % os.getcwd() + os.sep + out) if platform == 'darwin': # MacOS os.system('open ' + out + ' ' + save_path) print('Done. (%.3fs)' % (time.time() - t0)) # Close the camera pipeline.stop()