def create_pipeline(config: dict): """Sets up the pipeline to extract depth and rgb frames Arguments: config {dict} -- A dictionary mapping for configuration. see default.yaml Returns: tuple -- pipeline, process modules, filters, t265 device (optional) """ # Create pipeline and config for D4XX,L5XX pipeline = rs.pipeline() rs_config = rs.config() # IF t265 is enabled, need to handle seperately t265_dev = None t265_sensor = None t265_pipeline = rs.pipeline() t265_config = rs.config() if config['playback']['enabled']: # Load recorded bag file rs.config.enable_device_from_file( rs_config, config['playback']['file'], config['playback'].get('repeat', False)) # This code is only activated if the user points to a T265 Recorded Bag File if config['tracking']['enabled']: rs.config.enable_device_from_file( t265_config, config['tracking']['playback']['file'], config['playback'].get('repeat', False)) t265_config.enable_stream(rs.stream.pose) t265_pipeline.start(t265_config) profile_temp = t265_pipeline.get_active_profile() t265_dev = profile_temp.get_device() t265_playback = t265_dev.as_playback() t265_playback.set_real_time(False) else: # Ensure device is connected ctx = rs.context() devices = ctx.query_devices() if len(devices) == 0: logging.error("No connected Intel Realsense Device!") sys.exit(1) if config['advanced']: logging.info( "Attempting to enter advanced mode and upload JSON settings file" ) load_setting_file(ctx, devices, config['advanced']) if config['tracking']['enabled']: # Cycle through connected devices and print them for dev in devices: dev_name = dev.get_info(rs.camera_info.name) print("Found {}".format(dev_name)) if "Intel RealSense D4" in dev_name: pass elif "Intel RealSense T265" in dev_name: t265_dev = dev elif "Intel RealSense L515" in dev_name: pass if config['tracking']['enabled']: if len(devices) != 2: logging.error("Need 2 connected Intel Realsense Devices!") sys.exit(1) if t265_dev is None: logging.error("Need Intel Realsense T265 Device!") sys.exit(1) if t265_dev: # Unable to open as a pipeline, must use sensors t265_sensor = t265_dev.query_sensors()[0] profiles = t265_sensor.get_stream_profiles() pose_profile = [ profile for profile in profiles if profile.stream_name() == 'Pose' ][0] t265_sensor.open(pose_profile) t265_sensor.start(callback_pose) logging.info("Started streaming Pose") rs_config.enable_stream(rs.stream.depth, config['depth']['width'], config['depth']['height'], rs.format.z16, config['depth']['framerate']) # other_stream, other_format = rs.stream.infrared, rs.format.y8 rs_config.enable_stream(rs.stream.color, config['color']['width'], config['color']['height'], rs.format.rgb8, config['color']['framerate']) # Start streaming pipeline.start(rs_config) profile = pipeline.get_active_profile() depth_sensor = profile.get_device().first_depth_sensor() color_sensor = profile.get_device().first_color_sensor() depth_scale = depth_sensor.get_depth_scale() # depth_sensor.set_option(rs.option.global_time_enabled, 1.0) # color_sensor.set_option(rs.option.global_time_enabled, 1.0) if config['playback']['enabled']: dev = profile.get_device() playback = dev.as_playback() playback.set_real_time(False) # Processing blocks filters = [] decimate = None align = rs.align(rs.stream.color) depth_to_disparity = rs.disparity_transform(True) disparity_to_depth = rs.disparity_transform(False) # Decimation if config.get("filters").get("decimation"): filt = config.get("filters").get("decimation") if filt.get('active', True): filt.pop('active', None) # Remove active key before passing params decimate = rs.decimation_filter(**filt) # Spatial if config.get("filters").get("spatial"): filt = config.get("filters").get("spatial") if filt.get('active', True): filt.pop('active', None) # Remove active key before passing params my_filter = rs.spatial_filter(**filt) filters.append(my_filter) # Temporal if config.get("filters").get("temporal"): filt = config.get("filters").get("temporal") if filt.get('active', True): filt.pop('active', None) # Remove active key before passing params my_filter = rs.temporal_filter(**filt) filters.append(my_filter) process_modules = (align, depth_to_disparity, disparity_to_depth, decimate) intrinsics = get_intrinsics(pipeline, rs.stream.color) proj_mat = create_projection_matrix(intrinsics) sensor_meta = dict(depth_scale=depth_scale) config['sensor_meta'] = sensor_meta # Note that sensor must be saved so that it is not garbage collected t265_device = dict(pipeline=t265_pipeline, sensor=t265_sensor) return pipeline, process_modules, filters, proj_mat, t265_device
if args.record_rosbag or args.record_imgs: depth_sensor.set_option(rs.option.visual_preset, Preset.HighAccuracy) # 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 = 3 # 3 meter clipping_distance = clipping_distance_in_meters / depth_scale # Create an align object # rs.align allows us to perform alignment of depth frames to others frames # The "align_to" is the stream type to which we plan to align depth frames. align_to = rs.stream.color align = rs.align(align_to) # Streaming loop frame_count = 0 try: while True: # Get frameset of color and depth frames = pipeline.wait_for_frames() # Align the depth frame to color frame aligned_frames = align.process(frames) # Get aligned frames aligned_depth_frame = aligned_frames.get_depth_frame() color_frame = aligned_frames.get_color_frame()
def main(args): if args.t: # read tracking datas Pubs = JointPub() tracking_datas = [] with open("/home/pku-hr6/yyf_ws/data/arm_running.txt", 'r') as rf: lines = rf.readlines() for line in lines: datas = line.split(" ") datas = [float(data) for data in datas] print(datas) tracking_datas.append(datas) else: rospy.init_node('data_saver') data_nums = args.n file_names = args.file datas = [] save_files = os.path.join("./data", file_names + '.txt') # Configure depth and color streams pipeline = rs.pipeline() config = rs.config() pc = rs.pointcloud() # Get device product line for setting a supporting resolution pipeline_wrapper = rs.pipeline_wrapper(pipeline) pipeline_profile = config.resolve(pipeline_wrapper) device = pipeline_profile.get_device() device_product_line = str(device.get_info(rs.camera_info.product_line)) # device_product_line is SR300 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 scale of depth sensor depth_sensor = pipeline_profile.get_device().first_depth_sensor() depth_scale = depth_sensor.get_depth_scale() # clipping_distance_in_meters meters away clipping_distance_in_meters = 0.5 # 0.5 meter clipping_distance = clipping_distance_in_meters / depth_scale # creat align map align_to = rs.stream.color align = rs.align(align_to) l_h = 170 l_s = 42 l_v = 41 u_h = 185 u_s = 255 u_v = 255 l_b = np.array([l_h, l_s, l_v]) u_b = np.array([u_h, u_s, u_v]) count = 0 with open(save_files, "w") as wf: # while count < data_nums: while len(datas) < data_nums: # while True: # Wait for a coherent pair of frames: depth and color if args.t: if count >= len(tracking_datas) - 1: break frames = pipeline.wait_for_frames() aligned_frame = align.process(frames) depth_frame = aligned_frame.get_depth_frame() color_frame = aligned_frame.get_color_frame() 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()) # remove background grey_color = 153 depth_image_3d = np.dstack( (depth_image, depth_image, depth_image)) # depth img is 1 channel, color is 3 channels bg_rmvd = np.where( (depth_image_3d > clipping_distance) | (depth_image_3d <= 0), grey_color, color_image) # get final img depth_colormap = cv2.applyColorMap( cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET) hsv_map = cv2.cvtColor(bg_rmvd, cv2.COLOR_BGR2HSV) mask = cv2.inRange(hsv_map, l_b, u_b) res = cv2.bitwise_and(bg_rmvd, bg_rmvd, mask=mask) img = np.hstack((bg_rmvd, depth_colormap)) # get object from mask map and calculate position mask_index = np.nonzero(mask) valid_p = False depth_point = np.array([]) if not mask_index[0].shape[0] == 0: valid_p = True x_index = int(np.median(mask_index[1])) y_index = int(np.median(mask_index[0])) x_min = x_index - 20 x_max = x_index + 20 y_min = y_index - 20 y_max = y_index + 20 # Intrinsics & Extrinsics depth_intrin = depth_frame.profile.as_video_stream_profile( ).intrinsics # print(depth_intrin) # 640x480 p[314.696 243.657] f[615.932 615.932] depth_pixel = [x_index, y_index] dist2obj = depth_frame.get_distance(x_index, y_index) depth_point = rs.rs2_deproject_pixel_to_point( depth_intrin, depth_pixel, dist2obj) depth_point = [float(dep) * 100 for dep in depth_point] # using cm depth_point = [ depth_point[2], -depth_point[0], -depth_point[1] ] #[x,y,z](in base) = [z,-x,-y](in camera) depth_point = [ depth_point[0] + 1.8, depth_point[1], depth_point[2] + 5.3 ] txt = "({:.2f},{:.2f},{:.2f})".format(depth_point[0], depth_point[1], depth_point[2]) # print(txt) cv2.rectangle(res, (x_min, y_min), (x_max, y_max), (255, 0, 0), 2) cv2.putText(res, txt, (x_index, y_index), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 1) cv2.imshow('Align Example', img) cv2.imshow("mask", mask) cv2.imshow("res", res) # print(depth_point) # msg = rospy.wait_for_message('/motor_states/pan_tilt_port',MotorStateList,timeout=10) # # print(len(msg.motor_states)) # joints,valid_q = get_joint_angle(msg) # if valid_p and valid_q: # depth_point = np.array(depth_point) # # print(joints) # # print(depth_point) # if args.only_q: # data = joints # else: # data = np.append(depth_point, joints) # print(data) # datas.append(data) # if args.t: # pubulish joint # data = tracking_datas[count] # Pubs.publish_jointsD(data) # count = count + 1 if cv2.waitKey(100) & 0xFF == ord('q'): # quit break for data in datas: for i in range(len(data)): wf.write(str(round(data[i], 2))) if i != len(data) - 1: wf.write(" ") else: wf.write("\n")
def deviceInitial(self): self.deviceDetect() self.pipeline = rs.pipeline() self.config = rs.config() # Configure depth and color streams if self.options.enableColor is True: self.config.enable_stream( rs.stream.color, self.options.resColor[0], self.options.resColor[1], rs.format.bgr8, 30, ) if self.options.enableInfrared is True: self.config.enable_stream( rs.stream.infrared, self.options.resInfrared[0], self.options.resInfrared[1], rs.format.bgr8, 30, ) if self.options.enableDepth is True: self.config.enable_stream( rs.stream.depth, self.options.resDepth[0], self.options.resDepth[1], rs.format.z16, 30, ) # Start streaming cfg = self.pipeline.start(self.config) # Alignment if self.options.enableAlign is True: if self.options.enableColor is True: align_to = rs.stream.color elif self.options.enableInfrared is True: align_to = rs.stream.infrared self.align = rs.align(align_to) # Advanced settings dev = cfg.get_device() depth_sensor = dev.first_depth_sensor() if self.options.enableDepth is True and self.options.enablePost is True: depth_sensor.set_option(rs.option.visual_preset, 4) if self.options.enableIntrin is True: self.scale = depth_sensor.get_depth_scale() # Get intrinsics if self.options.enableColor is True: stream = cfg.get_stream(rs.stream.color) profile = stream.as_video_stream_profile() elif self.options.enableInfrared is True: stream = cfg.get_stream(rs.stream.infrared) profile = stream.as_video_stream_profile() self.intrin = profile.get_intrinsics()
def main(): default_model_dir = '../all_models' default_model = 'mobilenet_ssd_v2_coco_quant_postprocess_edgetpu.tflite' default_labels = 'coco_labels.txt' parser = argparse.ArgumentParser() parser.add_argument('--model', help='.tflite model path', default=os.path.join(default_model_dir, default_model)) parser.add_argument('--labels', help='label file path', default=os.path.join(default_model_dir, default_labels)) parser.add_argument( '--top_k', type=int, default=3, help='number of categories with highest score to display') parser.add_argument('--camera_idx', type=str, help='Index of which video source to use. ', default=0) parser.add_argument('--threshold', type=float, default=0.1, help='classifier score threshold') args = parser.parse_args() print('Loading {} with {} labels.'.format(args.model, args.labels)) interpreter = common.make_interpreter(args.model) interpreter.allocate_tensors() labels = load_labels(args.labels) # Configure depth and color streams pipeline = rs.pipeline() 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 profile = pipeline.start(config) try: while True: # Wait for a coherent pair of frames: depth and color frames = pipeline.wait_for_frames() depth_frame = frames.get_depth_frame() color_frame = frames.get_color_frame() if not depth_frame or not color_frame: continue # Convert images to numpy arrays color = np.asanyarray(color_frame.get_data()) colorizer = rs.colorizer() colorized_depth = np.asanyarray( colorizer.colorize(depth_frame).get_data()) # Create alignment primitive with color as its target stream: align = rs.align(rs.stream.color) frames = align.process(frames) # Update color and depth frames: aligned_depth_frame = frames.get_depth_frame() colorized_depth = np.asanyarray( colorizer.colorize(aligned_depth_frame).get_data()) cv2_im = color cv2_im_rgb = cv2.cvtColor(cv2_im, cv2.COLOR_BGR2RGB) pil_im = Image.fromarray(cv2_im_rgb) start = time.monotonic() common.set_input(interpreter, pil_im) interpreter.invoke() objs = get_output(interpreter, score_threshold=args.threshold, top_k=args.top_k) inference_time = time.monotonic() - start inference_time = 'Inference time: %.2f ms (%.2f fps)' % ( inference_time * 1000, 1.0 / inference_time) color = append_objs_to_img(color, objs, labels, inference_time) # Calculate the distance depth_scale = profile.get_device().first_depth_sensor( ).get_depth_scale() color = calculate_distance(color, aligned_depth_frame, objs, labels, depth_scale) # Stack both images horizontally #images = np.hstack((color, colorized_depth)) #cv2.imwrite("image.jpg", images) cv2.imshow('frame', color) if cv2.waitKey(1) & 0xFF == ord('q'): break finally: # Stop streaming pipeline.stop()
def run_live(ingredient_detector): data_pusher = DataPusher() pipeline = rs.pipeline() config = rs.config() config.enable_stream(rs.stream.depth, REALSENSE_WIDTH, REALSENSE_HEIGHT, rs.format.z16, 30) config.enable_stream(rs.stream.color, REALSENSE_WIDTH, REALSENSE_HEIGHT, rs.format.bgr8, 30) align_to = rs.stream.color align = rs.align(align_to) # Start streaming pipeline.start(config) colorizer = rs.colorizer() hole_filler = rs.hole_filling_filter() # Skip 5 first frames to give the Auto-Exposure time to adjust for x in range(5): pipeline.wait_for_frames() while True: frames = pipeline.wait_for_frames(timeout_ms=5000) aligned_frames = align.process(frames) color = aligned_frames.get_color_frame() depth = aligned_frames.get_depth_frame() # depth = hole_filler.process(depth) # depth2 = depth.get_distance(100, 100) # print(depth2) if not depth or not color: continue depth_frame_data = np.asanyarray(depth.get_data()) color_frame_data = np.asanyarray(color.get_data()) depth_image = np.asanyarray(colorizer.colorize(depth).get_data(), dtype=np.uint8) # depth_image = np.asanyarray(depth.get_data(), dtype=np.uint8) color_image = np.asanyarray(color.get_data(), dtype=np.uint8) color_image_with_overlay = np.copy(color_image) # cv.imwrite("rgb.jpg", color_image) # cv.imwrite("depth.jpg", depth_image) process_colour_image(color_image_with_overlay, ingredient_detector) circle_detector.draw_segmented_circle(color_image_with_overlay) if cv.waitKey(1) & 0xFF == ord(' '): # np.save("colour.txt", color_image) # np.save("depth.txt", np.asanyarray(depth.get_data(), dtype=np.uint8)) # r = color_image[:, :, 0] # g = color_image[:, :, 1] # b = color_image[:, :, 2] # np.savetxt("colour-r.txt", r) # np.savetxt("colour-g.txt", g) # np.savetxt("colour-b.txt", b) # np.savetxt("depth.txt", np.asanyarray(depth.get_data(), dtype=np.uint8)) scan_request = ScanRequest(color_frame_data, depth_frame_data, 1, 1) data_pusher.push_scan(scan_request) check_for_key() cv.imshow("Depth", depth_image) cv.imshow("RGB", color_image_with_overlay) sleep(0.2)
def run(self): try: if self.motor_control is not None: ofile = open(self.prefix_filename + 'alphamap.csv', 'w') writer = csv.writer(ofile) print('Real sense thread is starting up') advanced.set_adv() pipeline = rs.pipeline() cnt = rs.context() devs = cnt.query_devices() d = devs.front() print(devs.size()) serial_no = d.get_info(rs.camera_info(1)) print(serial_no) config = rs.config() config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 15) config.enable_stream(rs.stream.color, 1280, 720, rs.format.rgb8, 15) #config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 10) #config.enable_stream(rs.stream.color, 640, 480, rs.format.rgb8, 10) time.sleep(1) pipeline.start(config) print('Camera is warming up') time.sleep(6) pointcloud = rs.pointcloud() #Make an align object. Allows us to align depth to color align_to = rs.stream.color align = rs.align(align_to) print('Done initializing real sense pipeline') if self.motor_control is not None: print('Initializing Arduino board') self.motor_control.pipeline = pipeline self.motor_control.setup() print('Done initializing Arduino board') count = 1 while True: if self.motor_control is not None: pos = self.motor_control.nextPos() for (m_nr, p) in enumerate(pos): print('Motor', m_nr, ': ', p) self.motor_filename = str(count) + '_' + serial_no writer.writerow([count] + pos) frames = pipeline.wait_for_frames() depth = frames.get_depth_frame() color = frames.get_color_frame() pointcloud.map_to(color) points = pointcloud.calculate(depth) width = color.get_width() height = color.get_height() external_format = GL_RGB if color.get_profile().format() is rs.format.y8: external_format = GL_LUMINANCE external_type = GL_UNSIGNED_BYTE pixels = np.asanyarray(color.get_data()) # 2018-01-14 Kenny: This is the old librealsense2 interface #coordinates = np.asanyarray(points.get_vertices()) #uvs = np.asanyarray(points.get_texture_coordinates()) coords = np.asanyarray(points.get_vertices_EXT(), dtype=np.float32) texs = np.asanyarray(points.get_texture_coordinates_EXT(), dtype=np.float32) vertex_array = np.hstack((coords, texs)) filename = self.prefix_filename + 'frame' + str( count) + self.postfix_filename if self.motor_control is not None: filename = self.prefix_filename + self.motor_filename + self.postfix_filename if self.save_color: imsave(filename + 'color.tif', pixels) if self.save_texture: texture = np.asanyarray( points.get_texture_coordinates_EXT()) imsave(filename + 'texture.tif', texture) if self.save_ply: points.export_to_ply(filename + '.ply', color) if self.save_depth: is_aligned = False while not is_aligned: aligned_frames = align.process(frames) try: aligned_depth_frame = aligned_frames.get_depth_frame( ) depth_image = np.asanyarray( aligned_depth_frame.get_data()) is_aligned = True except: pass imsave(filename + 'depth.tif', depth_image) if self.render is not None: self.render.copy_data(vertex_array, width, height, external_format, external_type, pixels) print('frame', count, 'done') count = count + 1 if not threading.main_thread().is_alive(): print('Main thread is dead, closing down sensor') ofile.close() pipeline.stop() if self.bot is not None: self.bot.end('') return except Exception as e: print(e) pipeline.stop() ofile.close() if self.bot is not None: self.bot.end(e) return
def kedalaman(self, frames): self.align = rs.align(rs.stream.color) frames = self.align.process(frames) aligned_depth_frame = frames.get_depth_frame() depth_real = np.asanyarray(aligned_depth_frame.get_data()) return depth_real
def Realsense(self): #Realsense global color_image #이미지 가져옴 pipeline = rs.pipeline() #설정 파일 생성 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) #설정을 적용하여 이미지 취득 시작, 프로파일 얻음 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) #1 meter, 클리핑할 영역을 1m로 설정 clipping_distance_in_meters = 1 clipping_distance = clipping_distance_in_meters / depth_scale #스케일에 따른 클리핑 거리 #depth 이미지를 맞추기 위한 이미지, 컬러 이미지 align_to = rs.stream.color #depth 이미지와 맞추기 위해 align 생성 align = rs.align(align_to) #try: while True: #color와 depth의 프레임셋을 기다림 frames = pipeline.wait_for_frames() #frames.get_depth_frame() 은 640x360 depth 이미지이다. #모든(depth 포함) 프레임을 컬러 프레임에 맞추어 반환 aligned_frames = align.process(frames) #aligned depth 프레임은 640x480 의 depth 이미지이다. aligned_depth_frame = aligned_frames.get_depth_frame() #컬러 프레임을 얻음 color_frame = aligned_frames.get_color_frame() #프레임이 없으면, 건너 뜀 if not aligned_depth_frame or not color_frame: continue #depth이미지를 배열로 depth_image = np.asanyarray(aligned_depth_frame.get_data()) #color 이미지를 배열로 color_image = np.asanyarray(color_frame.get_data()) #글자 표시 color = (0, 255, 255) thickness = 2 location = (250, 30) font = cv2.FONT_HERSHEY_COMPLEX fontScale = 1.0 cv2.putText(color_image, 'q : Quit', location, font, fontScale, color, thickness) #이미지 윈도우 정의 cv2.namedWindow('Align Example', cv2.WINDOW_AUTOSIZE) #이미지를 넣어 윈도우에 보임 cv2.imshow('Align Example', color_image) key = cv2.waitKey(1) #if key & 0xFF == ord('s'): # cv2.imwrite('./calibration_data/{0}.jpg'.format(i), color_image) # cv2.destroyAllWindows() #q를 누르면, 나간다. if key & 0xFF == ord('q') or key == 27: #윈도우 제거 cv2.destroyAllWindows() break
def start_pipeline(): save_path = "./out" 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) pipeline.start(config) print('[INFO] Starting pipeline') profile = pipeline.get_active_profile() # profile = pipeline.start(config) 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 print("[INFO] Width: ", w, "Height: ", h) fx, fy, = depth_intrinsics.fx, depth_intrinsics.fy print("[INFO] Focal length X: ", fx) print("[INFO] Focal length Y: ", fy) ppx, ppy = depth_intrinsics.ppx, depth_intrinsics.ppy print("[INFO] Principal point X: ", ppx) print("[INFO] Principal point Y: ", ppy) # Identifying depth scaling factor depth_sensor = profile.get_device().first_depth_sensor() depth_scale = depth_sensor.get_depth_scale() pc = rs.pointcloud() decimate = rs.decimation_filter() spatial = rs.spatial_filter hole = rs.hole_filling_filter temporal = rs.temporal_filter() colorizer = rs.colorizer() align_to = rs.stream.color align = rs.align(align_to) frame_counter = 0 try: print("[INFO] Starting pipeline stream with frame {:d}".format( frame_counter)) while True: frames = pipeline.wait_for_frames() aligned_frames = align.process(frames) aligned_depth_frame = aligned_frames.get_depth_frame() aligned_color_frame = aligned_frames.get_color_frame() depth_colormap = np.asanyarray( colorizer.colorize(aligned_depth_frame).get_data()) color_image = np.asanyarray(aligned_color_frame.get_data()) pts = pc.calculate(aligned_depth_frame) pc.map_to(aligned_color_frame) if not aligned_depth_frame or not aligned_color_frame: print("No depth or color frame, try again...") continue depth_image = np.asanyarray(aligned_depth_frame.get_data()) images = np.hstack( (cv2.cvtColor(color_image, cv2.COLOR_BGR2RGB), depth_colormap)) cv2.imshow(str(frame_counter), images) key = cv2.waitKey(1) if key == ord("d"): landmarks = landmark_detector(color_image) for n, px in landmarks.items(): z = aligned_depth_frame.get_distance(px[0], px[1]) # Test Landmarks px = [width, height] pt = rs.rs2_deproject_pixel_to_point( depth_intrinsics, [px[0], px[1]], z) w, h = rs.rs2_project_point_to_pixel(depth_intrinsics, pt) print("Original Pixel:", px) print("Deprojected Pixel:", [w, h]) return px, w, h except Exception as ex: print(ex) finally: pipeline.stop()
def main(): if not os.path.exists(args.output): os.mkdir(args.output) data_id = len(os.listdir(args.output)) data_path = os.path.join(args.output, '{:04d}'.format(data_id)) color_path = os.path.join(data_path, 'color') depth_path = os.path.join(data_path, 'depth') height_map_color_path = os.path.join(data_path, 'height_map_color') height_map_depth_path = os.path.join(data_path, 'height_map_depth') label_path = os.path.join(data_path, 'label') os.mkdir(data_path) os.mkdir(color_path) os.mkdir(depth_path) os.mkdir(height_map_color_path) os.mkdir(height_map_depth_path) os.mkdir(label_path) # initialize urx rob = urx.Robot(args.ip) rob.set_tcp((0, 0, 0, 0, 0, 0)) rob.set_payload(0.5, (0, 0, 0)) gripper = urx.RobotiqGripper(port=args.port) gripper.activation_request() gripper.set_speed(0x40) gripper.set_force(0x80) # initialize realsense pipeline = rs.pipeline() config = rs.config() config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30) config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30) pipeline.start(config) align = rs.align(rs.stream.color) i = 57 while i: _ = pipeline.wait_for_frames() i -= 1 # load configuration camera_location = np.loadtxt(args.camera_location) rob.movel(camera_location, acc=args.a, vel=args.v) init_angle = rob.getj()[-1] origin = np.loadtxt(args.origin) mtx = np.loadtxt(args.projection_matrix) b_depth_height_map = cv2.imread(args.depth_dir, cv2.IMREAD_ANYDEPTH) cv2.imwrite(data_path + '/background_depth_height_map.png', b_depth_height_map) b_depth_height_map = b_depth_height_map.astype(np.float32) f = open(os.path.join(data_path, 'file_name.txt'), 'w') k = 0 num_success = 0 num_fail = 0 try: for i in range(0, 10): print('--------i= %s' % i + '. Attempt to grasp without knowledge--------') # align the depth frame to color frame frames = pipeline.wait_for_frames() aligned_frames = align.process(frames) color_frame = aligned_frames.first(rs.stream.color) depth_frame = aligned_frames.get_depth_frame() color_img = np.asanyarray(color_frame.get_data()) depth_img = (np.asanyarray(depth_frame.get_data()) * args.z_scale).astype(np.uint16) warp_color_img = cv2.warpPerspective(color_img, mtx, (args.width, args.height)) warp_depth_img = cv2.warpPerspective( depth_img, mtx, (args.width, args.height)).astype(np.float32) # get center point of object chosen among objects seen by the camera center_point, rect, object_pts, z = get_center_point( warp_color_img, warp_depth_img, b_depth_height_map) episode = [] # approach to the target pose pose1 = copy.deepcopy(origin) pose1[0] += center_point[1] * args.ratio # height pose1[1] += center_point[0] * args.ratio # width pose1[2] += args.hover_distance rob.movel(pose1, acc=args.a, vel=args.v) episode.append(pose1) # rotate end effector angle joint_angle = rob.getj() delta = np.random.uniform(0, np.pi) angle = init_angle + delta joint_angle[-1] = angle rob.movej(joint_angle, args.a, args.v) pose2 = rob.getl() episode.append(pose2) # reach to target pose with end effector rotated pose3 = rob.getl() pose3[2] = pose3[2] - args.hover_distance + z rob.movel(pose3, acc=args.a, vel=args.v) episode.append(pose3) # close gripper and obtain gripper status print('Try grasp:') gripper.gripper_close() rob.movel(pose2, acc=args.a, vel=args.v) status = gripper.get_object_detection_status() position = gripper.get_gripper_pos() if (status == 1 or status == 2) and position < 217: print('Grasp success!') rob.movel(pose3, acc=args.a, vel=args.v) gripper.gripper_open() rob.movel(pose2, acc=args.a, vel=args.v) rob.movel(camera_location, acc=args.a, vel=args.v) for j in range(1): frames = pipeline.wait_for_frames() aligned_frames = align.process(frames) color_frame = aligned_frames.first(rs.stream.color) depth_frame = aligned_frames.get_depth_frame() color_img = np.asanyarray(color_frame.get_data()) depth_img = (np.asanyarray(depth_frame.get_data()) * args.z_scale).astype(np.uint16) warp_color_img = cv2.warpPerspective( color_img, mtx, (args.width, args.height)) warp_depth_img = cv2.warpPerspective( depth_img, mtx, (args.width, args.height)) cv2.imwrite(color_path + '/{:06d}.png'.format(k), color_img) cv2.imwrite(depth_path + '/{:06d}.png'.format(k), depth_img) cv2.imwrite( height_map_color_path + '/{:06d}.png'.format(k), warp_color_img) cv2.imwrite( height_map_depth_path + '/{:06d}.png'.format(k), warp_depth_img) _, rect, object_pts, _ = get_center_point( warp_color_img, warp_depth_img, b_depth_height_map) points = get_label(center_point, -delta, 256 - position) label = draw_label(points, args.width, args.height) cv2.imwrite(label_path + '/{:06d}.png'.format(k), label) np.savetxt(label_path + '/{:06d}.good.txt'.format(k), points) np.savetxt(label_path + '/{:06d}.rectangle.txt'.format(k), np.asanyarray(rect)) np.savetxt( label_path + '/{:06d}.object_points.txt'.format(k), np.asanyarray(object_pts)) f.write('{:06d}\n'.format(k)) k += 1 print('i=%s, j=%s' % (i, j) + ', all good grasp labels written.') num_success = num_success + 1 print( 'Now, let\'s grasp it again same to last pose to move to another place.' ) rob.movel(episode[0], acc=args.a, vel=args.v) rob.movel(episode[1], acc=args.a, vel=args.v) rob.movel(episode[2], acc=args.a, vel=args.v) gripper.gripper_close() pose4 = copy.deepcopy(origin) # generate a new center_point randomly. center_point = [ np.random.randint(57, args.width - 57), np.random.randint(57, args.height - 57) ] pose4[0] += center_point[1] * args.ratio pose4[1] += center_point[0] * args.ratio pose4[2] += args.hover_distance rob.movel(pose4, acc=args.a, vel=args.v) episode[0] = pose4 joint_angle = rob.getj() delta = np.random.uniform(0, np.pi) angle = init_angle + delta joint_angle[-1] = angle rob.movej(joint_angle, args.a, args.v) pose5 = rob.getl() episode[1] = pose5 pose6 = rob.getl() pose6[2] = pose6[2] - args.hover_distance + z rob.movel(pose6, acc=args.a, vel=args.v) episode[2] = pose6 gripper.gripper_open() print( 'OK,move to another place.Notice that we remember the route and angle unless j=1.' ) rob.movel(episode[1], acc=args.a, vel=args.v) rob.movel(camera_location, acc=args.a, vel=args.v) elif position > 217: print('Grasp fails!') cv2.imwrite(color_path + '/{:06d}.png'.format(k), color_img) cv2.imwrite(depth_path + '/{:06d}.png'.format(k), depth_img) cv2.imwrite(height_map_color_path + '/{:06d}.png'.format(k), warp_color_img) cv2.imwrite(height_map_depth_path + '/{:06d}.png'.format(k), warp_depth_img) points = get_label(center_point, -delta, 256) label = draw_label(points, args.width, args.height, (0, 0, 255)) cv2.imwrite(label_path + '/{:06d}.png'.format(k), label) np.savetxt(label_path + '/{:06d}.bad.txt'.format(k), points) np.savetxt(label_path + '/{:06d}.rectangle.txt'.format(k), np.asanyarray(rect)) np.savetxt(label_path + '/{:06d}.object_points.txt'.format(k), np.asanyarray(object_pts)) f.write('{:06d}\n'.format(k)) k += 1 print('i=%s' % i + ', all bad grasp labels written.') num_fail = num_fail + 1 gripper.gripper_open() rob.movel(camera_location, acc=args.a, vel=args.v) else: gripper.gripper_open() rob.movel(camera_location, acc=args.a, vel=args.v) finally: rob.close() pipeline.stop() f.close() print("Success: " + str(num_success) + ", fail: " + str(num_fail))
import torch import skvideo.io import os # import png #!===================================================================================================================== pipeline = 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) # 设定需要对齐的方式(这里是深度对齐彩色,彩色图不变,深度图变换) align_to = rs.stream.color # 设定需要对齐的方式(这里是彩色对齐深度,深度图不变,彩色图变换) # align_to = rs.stream.depth alignedFs = rs.align(align_to) profile = pipeline.start(cfg) size = (640, 480) #save as avi out = cv2.VideoWriter('color_video.avi', cv2.VideoWriter_fourcc(*'XVID'), 30, size) outdepth = cv2.VideoWriter('depth_video.avi', cv2.VideoWriter_fourcc(*'XVID'), 30, size) #or save as mp4 #out = cv2.VideoWriter('color_video.mp4',cv2.VideoWriter_fourcc('m', 'p', '4', 'v'), 30, size) #outdepth = cv2.VideoWriter('depth_video.mp4',cv2.VideoWriter_fourcc('m', 'p', '4', 'v'), 30, size) depth_numpy = [] color_numpy = [] all_depth_numpy = [] all_color_numpy = [] depth_fpath = "./numpy/depth.npy" color_fpath = "./numpy/color.npy" if os.path.exists(depth_fpath):
def main(): global handin global currentState # img_rows, img_cols, maxFrames = 32, 32, 100 img_rows, img_cols, maxFrames = 50, 50, 55 depthFrame = 0 cameraHeightR = 480 cameraWidthR = 848 # cameraWidthR = 640 frameRateR = 60 # frameRateR = 30 # crop parameter xupam = 350 yupam = 200 xdpam = 250 ydpam = 300 #1,3,4,5,8,9,12,13,14,15 classGest = ['1', '12', '13', '14', '15', '3', '4', '5', '8', '9'] nameGest = [ 'call', 'scroll up', 'scroll down', 'right', 'leftback', "like", 'play/pause', 'close', 'click', 'down back' ] #classGest = ['11', '12', '13','4','8'] #nameGest = ['back', 'scroll up', 'scroll down', 'close 4', 'click 8'] delayGest = 5 delayBol = False backgroundRemove = True boxCounter = True # load the model and weight json_file = open('3dcnnresult/33/3dcnnmodel.json', 'r') loaded_model_json = json_file.read() json_file.close() loaded_model = model_from_json(loaded_model_json) # load weights into new model loaded_model.load_weights("3dcnnresult/33/3dcnnmodel.hd5") print("Loaded model from disk") loaded_model.compile(loss=categorical_crossentropy, optimizer='rmsprop', metrics=['accuracy']) conf = loaded_model.model.get_config() shapeInput, ap = loaded_model.model.get_layer(name="dense_2").input_shape shapeOutput, sp = loaded_model.model.get_layer(name="dense_2").output_shape weightDense2 = loaded_model.model.get_layer( name="dense_2").get_weights()[0] weightDense22I = loaded_model.model.get_layer( name="dense_2").get_weights()[1] weightDense22A = loaded_model.model.get_layer( name="dense_2").get_weights()[1] # load the FSM ytfsmfile = "youtube_fsm.txt" ytFsmTable = loadFSM(ytfsmfile) updatedWeight = False updatedWeight2 = False '''' print("========================New weight I ==================================") print(weightDense22I) print("========================New weight A ==================================") print(weightDense22A) ''' #print(newWeight[0]) #NweightDense2 = loaded_model.model.get_layer(name="dense_2").get_weights()[0] #modelConfig = loaded_model.get_config() #print(modelConfig) # setup cv face detection face_cascade = cv2.CascadeClassifier( 'haarcascades/haarcascade_frontalface_alt2.xml') # setup Realsense # Configure depth and color streams pipeline = rs.pipeline() config = rs.config() config.enable_stream(rs.stream.depth, cameraWidthR, cameraHeightR, rs.format.z16, frameRateR) config.enable_stream(rs.stream.color, cameraWidthR, cameraHeightR, rs.format.bgr8, frameRateR) # if using background removal # Start streaming profile = pipeline.start(config) # Getting the depth sensor's depth scale (see rs-align example for explanation) depth_sensor = profile.get_device().first_depth_sensor() depth_scale = depth_sensor.get_depth_scale() # print "Depth Scale is: " , depth_scale # We will be removing the background of objects more than # clipping_distance_in_meters meters away clipping_distance_in_meters = 1 # 1 meter clipping_distance = clipping_distance_in_meters / depth_scale # for text purpose imgTxt = np.zeros((480, 400, 3), np.uint8) txt = "OpenCV" txtLoad = "[" txtDelay = "[" txtRecord = "Capture" txtDel = "Delay" txtProbability = "0%" font = cv2.FONT_HERSHEY_SIMPLEX framearray = [] ctrDelay = 0 channel = 1 gestCatch = False gestStart = False startTime = 0 endTime = 0 x, y, w, h = 0, 0, 0, 0 align_to = rs.stream.color align = rs.align(align_to) num_frames = 0 im_width, im_height = (848, 480) # max number of hands we want to detect/track num_hands_detect = 1 score_thresh = 0.37 try: while True: dataImg = [] # Wait for a coherent pair of frames: depth and color frames = pipeline.wait_for_frames() # Align the depth frame to color frame aligned_frames = align.process(frames) # Get aligned frames aligned_depth_frame = aligned_frames.get_depth_frame() color_frame = aligned_frames.get_color_frame() depth_image = np.asanyarray(aligned_depth_frame.get_data()) color_image = np.asanyarray(color_frame.get_data()) # Validate that both frames are valid if not aligned_depth_frame or not color_frame: continue #print(scores) num_frames += 1 image_np = color_image try: image_np = cv2.cvtColor(image_np, cv2.COLOR_BGR2RGB) except: print("Error converting to RGB") boxes, scores = detector_utils.detect_objects( image_np, detection_graph, sess) for i in range(num_hands_detect): if (scores[i] > score_thresh): handin = True #print("xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx") break if handin == True: gestStart = True else: gestStart = False if (backgroundRemove == True): # Remove background - Set pixels further than clipping_distance to grey # grey_color = 153 grey_color = 0 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) color_image = bg_removed draw_image = color_image else: draw_image = color_image # face detection here gray = cv2.cvtColor(color_image, cv2.COLOR_BGR2GRAY) if gestCatch == False: faces = face_cascade.detectMultiScale(gray, 1.1, 2) #print("face: ",len(faces)) ctr = 0 idxFace = -1 minDist = 9999 if len(faces) > 0: for f in faces: xh, yh, wh, hh = f farea = wh * hh midxf = int(xh + (wh * 0.5)) midyf = int(yh + (hh * 0.5)) depth_imageS = depth_image * depth_scale deptRef = depth_imageS.item(midyf, midxf) if deptRef <= minDist: idxFace = ctr minDist = deptRef ctr = ctr + 1 #print("id face", idxFace) if idxFace >= 0: x, y, w, h = faces[idxFace] cv2.rectangle(draw_image, (x, y), (x + w, y + h), (255, 0, 0), 2) # crop the face then pass to resize midx = int(x + (w * 0.5)) midy = int(y + (h * 0.5)) xUp = (x - (w * 3)) yUp = (y - (h * 1.5)) xDn = ((x + w) + (w * 1)) yDn = ((y + h) + (h * 2)) if xUp < 1: xUp = 0 if xDn >= cameraWidthR: xDn = cameraWidthR if yUp < 1: yUp = 0 if yDn >= cameraHeightR: yDn = cameraHeightR if handin == False: cv2.rectangle(draw_image, (xUp.__int__(), yUp.__int__()), (xDn.__int__(), yDn.__int__()), (0, 0, 255), 2) else: cv2.rectangle(draw_image, (xUp.__int__(), yUp.__int__()), (xDn.__int__(), yDn.__int__()), (0, 255, 0), 2) cv2.circle(draw_image, (midx.__int__(), midy.__int__()), 10, (255, 0, 0)) # region of interest roi_gray = gray[yUp.__int__():yDn.__int__(), xUp.__int__():xDn.__int__()] #print(cv2.useOptimized()) ''''' stateText = "State " + str(currentState) cv2.putText(imgTxt, stateText, (10, 200), font, 1, (255, 255, 255), 2, cv2.LINE_AA) avtext = "Available Gesture" cv2.putText(imgTxt, avtext, (10, 230), font, 1, (255, 255, 255), 2, cv2.LINE_AA) for i in range(0, len(availableGest)): availGestText = "G" + availableGest[i] + ", " cv2.putText(imgTxt, availGestText, (10 + (i * 80), 260), font, 1, (255, 255, 255), 2, cv2.LINE_AA) ''' # if handin == True and depthFrame==10: # depth_imageS = depth_image * depth_scale # deptRef = depth_imageS.item(midy, midx) # boxColor = color_image.copy() # checkhandIn2(boxCounter, deptRef, midx, midy, w, h, depth_imageS, boxColor, draw_image,handin) # find the depth of middle point of face if backgroundRemove == True and gestCatch == False: #if backgroundRemove == True: e1 = cv2.getTickCount() depth_imageS = depth_image * depth_scale deptRef = depth_imageS.item(midy, midx) # print(clipping_distance) clipping_distance = (deptRef + 0.2) / depth_scale boxColor = color_image.copy() #handin = checkhandIn(boxCounter, deptRef, xUp, yUp, xDn, yDn, depth_imageS, draw_image) ##handin = checkhandIn2(boxCounter, deptRef, midx, midy, w, h, depth_imageS, boxColor, draw_image) #print(handin) e2 = cv2.getTickCount() times = (e2 - e1) / cv2.getTickFrequency() #print(times) #handin = False ##if handin == True: ## gestStart = True ##else: ## gestStart = False # print("gest start = ", gestStart) if delayBol == False and gestStart == True: # your code execution if depthFrame < maxFrames: frame = cv2.resize(roi_gray, (img_rows, img_cols)) framearray.append(frame) depthFrame = depthFrame + 1 txtLoad = txtLoad + "[" gestCatch = True # print(depthFrame) if depthFrame == maxFrames: dataImg.append(framearray) xx = np.array(dataImg).transpose((0, 2, 3, 1)) X = xx.reshape( (xx.shape[0], img_rows, img_cols, maxFrames, channel)) X = X.astype('float32') print('X_shape:{}'.format(X.shape)) #==================== Update the Weight ======================================= newWeightI = [] newWeightA = [] # find the available gesture in the current state availableGest = getGesture(currentState, ytFsmTable) print(availableGest) availG, ignoreG = translateAvailableGest( availableGest, classGest) print(availG) print(ignoreG) if updatedWeight: weightI = manipWeight(weightDense22I, ignoreG, 1000) newWeightI.append(weightDense2) newWeightI.append(weightI) if updatedWeight2: maxClass = 10 weightA = manipWeight(weightDense22A, availG, 1000) newWeightA.append(weightDense2) newWeightA.append(weightA) #================================================================================= if updatedWeight == False and updatedWeight2 == False: newWeightI.append(weightDense2) newWeightI.append(weightDense22A) loaded_model.model.get_layer( name="dense_2").set_weights(newWeightI) if handin == True: # do prediction resc = loaded_model.predict_classes(X)[0] res = loaded_model.predict_proba(X)[0] # find the result of prediction gesture resultC = classGest[resc] nameResultG = nameGest[resc] for a in range(0, len(res)): print("Gesture {}: {} ".format( str(nameGest[a]), str(res[a] * 100))) else: resultC = 0 nameResultG = "not enough frame recorded" print( "===============================================================" ) if updatedWeight2: loaded_model.model.get_layer( name="dense_2").set_weights(newWeightA) # do prediction resc2 = loaded_model.predict_classes(X)[0] res2 = loaded_model.predict_proba(X)[0] # find the result of prediction gesture resultC2 = classGest[resc2] nameResultG2 = nameGest[resc2] for a in range(0, len(res2)): print("Gesture {}: {} ".format( str(nameGest[a]), str(res2[a] * 100))) # show text of gesture result imgTxt = np.zeros((480, 400, 3), np.uint8) #txt = "Gesture-" + str(resultFsm) if updatedWeight2: if res2[resc2] > res[resc]: txt = "ignored gesture" act = -1 else: txt = nameResultG act = resultC else: txt = nameResultG act = resultC print(act) # check with FSM for finding the next state currentState = getNextState(currentState, act, ytFsmTable) print(currentState) framearray = [] #dataImg = [] txtLoad = "" depthFrame = 0 gestCatch = False handin = False delayBol = True cv2.putText(imgTxt, txtLoad, (10, 20), font, 0.1, (255, 255, 255), 2, cv2.LINE_AA) cv2.putText(imgTxt, txtRecord, (10, 50), font, 1, (255, 255, 255), 2, cv2.LINE_AA) cv2.putText(imgTxt, txt, (10, 160), font, 2, (255, 255, 255), 2, cv2.LINE_AA) # 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) # print(delayBol) if delayBol == True: ctrDelay = ctrDelay + 1 txtDelay = txtDelay + "[" txtDel = "Delay" cv2.putText(imgTxt, txtDelay, (10, 70), font, 0.1, (255, 255, 255), 2, cv2.LINE_AA) cv2.putText(imgTxt, txtDel, (10, 100), font, 1, (255, 255, 255), 2, cv2.LINE_AA) if ctrDelay == delayGest: ctrDelay = 0 txtDelay = "" delayBol = False gestCatch = False handin = False gestStart = False draw_image = cv2.flip(draw_image, 1) # Stack both images horizontally images = np.hstack((draw_image, imgTxt)) # Show images cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE) cv2.imshow('RealSense', images) cv2.waitKey(1) finally: # Stop streaming pipeline.stop()
def run_loop(bag_path, seg_model, seg_opts, save_images=False, output_mode=0): if save_images: create_folders() # 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.input) # Start streaming from file Pipe = pipeline.start(config) # Getting the depth sensor's depth scale (see rs-align example for explanation) depth_sensor = Pipe.get_device().first_depth_sensor() depth_scale = depth_sensor.get_depth_scale() print("Depth Scale is: ", depth_scale) # can be commented out if output_mode == 0 or output_mode == 1: # Create opencv window to render image in cv2.namedWindow("Full Stream", cv2.WINDOW_NORMAL) flip_transform = [[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]] # Create colorizer object colorizer = rs.colorizer() idx = 0 # initial frame delay idx_limit = 30 # pre_seg_mask_sum = None # previous frame path segmentation area - isn't being used right now # Streaming loop try: while True: idx += 1 # Get frameset of depth frames = pipeline.wait_for_frames() # ignore first idx frames if idx < idx_limit: continue else: pass align = rs.align(rs.stream.color) frames = align.process(frames) # Get color frame color_frame = frames.get_color_frame() # Get intrinsic in Open3d format for mode 2 and 3 for point cloud output if output_mode == 1 or output_mode == 2: intrinsic = o3d.camera.PinholeCameraIntrinsic( get_intrinsic_matrix(color_frame)) # Get depth frame depth_frame = frames.get_depth_frame() # Print intrinsics and extrinsics - not necessary : can be commented out if idx == idx_limit: camera_intrinsics(color_frame, depth_frame, Pipe) color_image = np.asanyarray(color_frame.get_data()) ### Add SEGMENTATION part here ### pred = test(color_image, seg_model, seg_opts) # pavement, floor, road, earth/ground, field, path, dirt/track - chosen classes for the model selected (we'd like an oversegmentation of the path) seg_mask = (pred == 11) | (pred == 3) | (pred == 6) | ( pred == 13) | (pred == 29) | (pred == 52) | ( pred == 91) #.astype(np.uint8) if idx == idx_limit: # 1st frame detection needs to be robust pre_seg_mask_sum = np.sum(seg_mask) # checking for bad detection new_seg_sum = np.sum(seg_mask) diff = abs(new_seg_sum - pre_seg_mask_sum) # if diff > pre_seg_mask_sum/15: # smoothening between segmentation outputs - seems like a bad idea since the model inputs are not connected between timesteps # seg_mask = np.ones_like(pred).astype(np.uint8) # need to add depth (5mt) criterea for calculation for robustness # del new_seg_sum # else: pre_seg_mask_sum = new_seg_sum ### mask Hole filling seg_mask = nd.binary_fill_holes(seg_mask).astype(int) seg_mask = seg_mask.astype(np.uint8) ##### seg_mask_3d = np.dstack((seg_mask, seg_mask, seg_mask)) pred_color = colorEncode( pred, loadmat(os.path.join(model_folder, 'color150.mat'))['colors']) ################################## depth_frame = depth_filter(depth_frame) depth_array = np.asarray(depth_frame.get_data()) # 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()) ############ Plane Detection ## need to add smoothening between frames - by plane weights' variance? try: ### need to add multithreading here (and maybe other methods?) planes_mask_binary = plane_detection(color_image*seg_mask_3d, depth_array*seg_mask,\ loop=5) except TypeError as e: try: print("plane mask 1st error") planes_mask, planes_normal, list_plane_params = test_PlaneDetector_send( img_color=color_image * seg_mask_3d, img_depth=depth_array * seg_mask) except TypeError as e: print("plane mask not detected-skipping frame") continue ## removed this part planes_mask = np.ones_like(depth_array).astype(np.uint8) planes_mask = np.dstack( (planes_mask, planes_mask, planes_mask)) ############################################## ## Hole filling for plane_mask (plane mask isn't binary - fixed that!) planes_mask_binary = nd.binary_fill_holes(planes_mask_binary) planes_mask_binary = planes_mask_binary.astype(np.uint8) # Clean plane mask object detection by seg_mask planes_mask_binary *= seg_mask planes_mask_binary_3d = np.dstack( (planes_mask_binary, planes_mask_binary, planes_mask_binary)) edges = planes_mask_binary - nd.morphology.binary_dilation( planes_mask_binary ) # edges calculation - between travesable and non-traversable path ############################################# if output_mode == 1 or output_mode == 2: odepth_image = o3d.geometry.Image(depth_array * edges) ocolor_image = o3d.geometry.Image(color_image * np.dstack( (edges, edges, edges))) rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth( ocolor_image, odepth_image, depth_scale=1.0 / depth_scale, depth_trunc=10, # set to 10 metres convert_rgb_to_intensity=False) temp = o3d.geometry.PointCloud.create_from_rgbd_image( rgbd_image, intrinsic) temp.transform(flip_transform) # temp = temp.voxel_down_sample(0.03) # Point cloud output of frame is appended to the list if output_mode == 1 or output_mode == 2: output_list.append(temp) # image format conversion for cv2 visualization/output if output_mode == 0 or output_mode == 1 or save_images == True: pred_color = cv2.cvtColor(pred_color, cv2.COLOR_RGB2BGR) color_image = cv2.cvtColor(color_image, cv2.COLOR_RGB2BGR) edges = cv2.cvtColor(edges, cv2.COLOR_GRAY2BGR) ## for displaying seg_mask seg_mask = (np.array(seg_mask) * 255).astype(np.uint8) seg_mask = cv2.cvtColor( seg_mask, cv2.COLOR_GRAY2BGR) # segmentation binary mask final_output_mask = cv2.cvtColor( planes_mask_binary, cv2.COLOR_GRAY2BGR) # final traversable path mask if output_mode == 0 or output_mode == 1: # # Blending rgb and depth images for display - can check alignment with this as well alpha = 0.2 beta = (1.0 - alpha) dst = cv2.addWeighted( color_image, alpha, pred_color, beta, 0.0 ) # color and segmentation output from ADE20K model blended dst2 = cv2.addWeighted(depth_color_image, alpha, color_image, beta, 0.0) # color and depth blended together ################################## ### delete later if needed - color image masked by final traversable path final_output = color_image * planes_mask_binary_3d mask = (final_output[:, :, 0] == 0) & ( final_output[:, :, 1] == 0) & (final_output[:, :, 2] == 0) final_output[:, :, :3][mask] = [255, 255, 255] ###### ### Select outputs for visualization - we've chosen some as default image_set1 = np.vstack((dst, dst2)) # image_set2 = np.vstack((planes_mask_binary_3d*255, seg_mask)) # image_set2 = np.vstack((dst, final_output)) image_set2 = np.vstack((edges, final_output)) ### Choose which images you want to display from above combined_images = np.concatenate((image_set1, image_set2), axis=1) if save_images == True: # Outputs saved - you can modify this cv2.imwrite("output/visualization/frame%d.png" % idx, combined_images) cv2.imwrite("data/color/frame%d.png" % idx, color_image) # save frame as JPEG file cv2.imwrite("data/depth/frame%d.png" % idx, depth_array) # save frame as JPEG file cv2.imwrite("output/edges/frame%d.png" % idx, edges) # save frame as JPEG file cv2.imwrite("output/segmentation_mask/frame%d.png" % idx, seg_mask) # save frame as JPEG file cv2.imwrite("output/output_mask/frame%d.png" % idx, final_output_mask) # save frame as JPEG file if output_mode == 0 or output_mode == 1: try: cv2.imshow('Full Stream', combined_images) except TypeError as e: print(idx, e) key = cv2.waitKey(1) # if pressed escape exit program if key == 27: cv2.destroyAllWindows() break finally: pipeline.stop() if output_mode == 0 or output_mode == 1: cv2.destroyAllWindows() return
def gesture(): #return('gesture') #global testDir #global img_rows, img_cols, maxFrames #global depthFrame #crop parameter #global xupam #global yupam #global xdpam #global ydpam #global classGest #global delayGest #global delayBol #load the model and weight #global json_file #global loaded_model_json #global loaded_model #setup cv face detection #global face_cascade # setup Realsense # Configure depth and color streams # for text purpose global handin global facex global facey global txt global txtLoad global txtDelay global txtRecord global txtDel global txtProbability global font #global framearray #global ctrDelay #global channel #global gestCatch #global gestStart #global x,y,w,h #global vc #global rval , firstFrame #global heightc, widthc, depthcol #global imgTxt #global resultC #global count #global stat pipeline = rs.pipeline() K.clear_session() testDir = "videoTest/" img_rows, img_cols, maxFrames = 32, 32, 55 depthFrame = 0 #crop parameter xupam = 350 yupam = 200 xdpam = 250 ydpam = 300 depthFrame = 0 cameraHeightR = 480 #cameraWidthR = 848 cameraWidthR = 848 frameRateR = 60 classGest = ['1','11','12','13','4','5','7','8'] delayGest = 20 delayBol = False framearray = [] ctrDelay = 0 channel = 1 gestCatch = False gestStart = False backgroundRemove = True x,y,w,h = 0,0,0,0 count=0 boxCounter = True #load the model and weight json_file = open('3dcnnresult/24/3dcnnmodel.json', 'r') loaded_model_json = json_file.read() json_file.close() # load weights into new model loaded_model = model_from_json(loaded_model_json) #loaded_model.load_weights("3dcnnresult/3dcnnmodel.hd5") loaded_model.load_weights("3dcnnresult/24/3dcnnmodel.hd5") #setup cv face detection face_cascade = cv2.CascadeClassifier('haarcascades/haarcascade_frontalface_alt2.xml') config = rs.config() config.enable_stream(rs.stream.depth, cameraWidthR, cameraHeightR, rs.format.z16, frameRateR) config.enable_stream(rs.stream.color, cameraWidthR, cameraHeightR, rs.format.bgr8, frameRateR) '''vc = cv2.VideoCapture(0) rval , firstFrame = vc.read() heightc, widthc, depthcol = firstFrame.shape imgTxt = np.zeros((heightc, 400, 3), np.uint8) #print('tryyyyy1')''' stat =0 # Start streaming profile = pipeline.start(config) # Getting the depth sensor's depth scale (see rs-align example for explanation) depth_sensor = profile.get_device().first_depth_sensor() depth_scale = depth_sensor.get_depth_scale() # print "Depth Scale is: " , depth_scale # We will be removing the background of objects more than # clipping_distance_in_meters meters away clipping_distance_in_meters = 2 # 1 meter clipping_distance = clipping_distance_in_meters / depth_scale #print("tes====================") # for text purpose imgTxt = np.zeros((480, 400, 3), np.uint8) txt = "OpenCV" txtLoad = "[" txtDelay = "[" txtRecord = "Capture" txtDel = "Delay" txtProbability = "0%" font = cv2.FONT_HERSHEY_SIMPLEX align_to = rs.stream.color align = rs.align(align_to) #print("Loaded model from disk") count=0 loaded_model.compile(loss=categorical_crossentropy, optimizer='rmsprop', metrics=['accuracy']) while True: if True: dataImg = [] # Wait for a coherent pair of frames: depth and color frames = pipeline.wait_for_frames() # Align the depth frame to color frame aligned_frames = align.process(frames) # Get aligned frames aligned_depth_frame = aligned_frames.get_depth_frame() color_frame = aligned_frames.get_color_frame() # 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()) if (backgroundRemove == True): # Remove background - Set pixels further than clipping_distance to grey # grey_color = 153 grey_color = 0 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) color_image = bg_removed draw_image = color_image else: draw_image = color_image #face detection here gray = cv2.cvtColor(color_image, cv2.COLOR_BGR2GRAY) #cv2.imshow('gray', gray) #cv2.waitKey(5000) faces = face_cascade.detectMultiScale(gray, 1.1, 2) if gestCatch == False: faces = face_cascade.detectMultiScale(gray, 1.1, 2) if len(faces) > 0: '''x, y, w, h = faces[0] ''' for f in faces: xh, yh, wh, hh = f farea = wh * hh if farea > 5000 and farea < 18000: x, y, w, h = f fArea = w*h ''' midx = int(x + (w * 0.5)) midy = int(y + (h * 0.5)) xUp = (x - (w * 3)) yUp = (y - (h * 1.5)) xDn = ((x + w) + (w * 1)) yDn = ((y + h) + (h * 2)) if xUp < 1: xUp = 0 if xDn >= cameraWidthR: xDn = cameraWidthR if yUp < 1: yUp = 0 if yDn >= cameraHeightR: yDn = cameraHeightR if handin == False: cv2.rectangle(draw_image, (xUp.__int__(), yUp.__int__()), (xDn.__int__(), yDn.__int__()), (0, 0, 255), 2) else: cv2.rectangle(draw_image, (xUp.__int__(), yUp.__int__()), (xDn.__int__(), yDn.__int__()), (0, 255, 0), 2) cv2.circle(draw_image, (midx.__int__(), midy.__int__()), 10, (255, 0, 0)) # region of interest roi_gray = gray[yUp.__int__():yDn.__int__(), xUp.__int__():xDn.__int__()] ''' midx = int(x + (w * 0.5)) midy = int(y + (h * 0.5)) xUp = (x - (w * 3)) yUp = (y - (h * 1.5)) xDn = ((x + w) + (w * 1)) yDn = ((y + h) + (h * 2)) if xUp < 1: xUp = 0 if xDn >= cameraWidthR: xDn = cameraWidthR if yUp < 1: yUp = 0 if yDn >= cameraHeightR: yDn = cameraHeightR if handin == False: cv2.rectangle(draw_image, (xUp.__int__(), yUp.__int__()), (xDn.__int__(), yDn.__int__()), (0, 0, 255), 2) else: cv2.rectangle(draw_image, (xUp.__int__(), yUp.__int__()), (xDn.__int__(), yDn.__int__()), (0, 255, 0), 2) cv2.circle(draw_image, (midx.__int__(), midy.__int__()), 10, (255, 0, 0)) roi_color = color_image[yUp.__int__():yDn.__int__(), xUp.__int__():xDn.__int__()] #find the depth of middle point of face if backgroundRemove == True and gestCatch == False: #e1 = cv2.getTickCount() depth_imageS = depth_image * depth_scale deptRef = depth_imageS.item(midy, midx) # print(clipping_distance) clipping_distance = (deptRef + 0.2) / depth_scale boxColor = color_image.copy() handin = checkhandIn(boxCounter, deptRef, midx, midy, w, h, depth_imageS, boxColor, draw_image) e2 = cv2.getTickCount() #times = (e2 - e1) / cv2.getTickFrequency() #print(times) if handin == True: gestStart = True else: gestStart = False if delayBol == False and gestStart == True: if depthFrame < maxFrames: frame = cv2.resize(roi_color, (img_rows, img_cols)) framearray.append(cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)) depthFrame = depthFrame+1 txtLoad = txtLoad+"[" count=count+1 gestCatch = True #print(depthFrame) if depthFrame == maxFrames: dataImg.append(framearray) xx = np.array(dataImg).transpose((0, 2, 3, 1)) X = xx.reshape((xx.shape[0], img_rows, img_cols, maxFrames, channel)) X = X.astype('float32') #print('X_shape:{}'.format(X.shape)) #do prediction resc = loaded_model.predict_classes(X)[0] res = loaded_model.predict_proba(X)[0] resultC = classGest[resc] #print("X=%s, Probability=%s" % (resultC, res[resc]*100)) for r in range(0,8): print("prob: " + str(res[r]*100)) #show text #imgTxt = np.zeros((480, 400, 3), np.uint8) txt = "Gesture-" + str(resultC) txtProbability = str(res[resc]*100)+"%" framearray = [] #dataImg = [] txtLoad = "" depthFrame = 0 handin = False gestCatch = False delayBol = True #cv2.putText(imgTxt, txtLoad, (10, 20), font, 0.1, (255, 255, 255), 2, cv2.LINE_AA) #.putText(imgTxt, txtRecord, (10, 50), font, 1, (255, 255, 255), 2, cv2.LINE_AA) #cv2.putText(imgTxt, txt, (10, 200), font, 2, (255, 255, 255), 2, cv2.LINE_AA) #cv2.putText(imgTxt, txtProbability, (10, 250), font, 1, (255, 255, 255), 2, cv2.LINE_AA) # 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) #print(delayBol) if delayBol == True: ctrDelay = ctrDelay+1 txtDelay = txtDelay + "[" #txtDel = "Delay" #cv2.putText(imgTxt, txtDelay, (10, 70), font, 0.1, (255, 255, 255), 2, cv2.LINE_AA) #cv2.putText(imgTxt, txtDel, (10, 100), font, 1, (255, 255, 255), 2, cv2.LINE_AA) if ctrDelay == delayGest: ctrDelay = 0 txtDelay = "" delayBol = False # Stack both images horizontally #images = np.hstack((draw_image, imgTxt)) # put the text here # Show images #cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE) #cv2.imshow('RealSense', images) #cv2.waitKey(1) if count==maxFrames: #vc.release() K.clear_session() pipeline.stop() return resultC '''
def run_loop(bag_path, seg_model, seg_opts, save_images=False): # 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.input) # Start streaming from file Pipe = pipeline.start(config) # Getting the depth sensor's depth scale (see rs-align example for explanation) depth_sensor = Pipe.get_device().first_depth_sensor() depth_scale = depth_sensor.get_depth_scale() print("Depth Scale is: ", depth_scale) # Create opencv window to render image in cv2.namedWindow("Full Stream", cv2.WINDOW_NORMAL) # Create colorizer object colorizer = rs.colorizer() idx = 0 # initial frame delay idx_limit = 30 pre_seg_mask_sum = None # previous frame path segmentation area # Streaming loop try: while True: idx += 1 # Get frameset of depth frames = pipeline.wait_for_frames() # ignore first idx frames if idx < idx_limit: continue else: pass align = rs.align(rs.stream.color) frames = align.process(frames) # Get color frame color_frame = frames.get_color_frame() # Get depth frame depth_frame = frames.get_depth_frame() # Get intrinsics and extrinsics if idx == idx_limit: camera_intrinsics(color_frame, depth_frame, Pipe) color_image = np.asanyarray(color_frame.get_data()) ### Add Segmentation part here ### pred = test(color_image, seg_model, seg_opts) # pavement, floor, road, earth/ground, field, path, dirt/track seg_mask = (pred == 11) | (pred == 3) | (pred == 6) | ( pred == 13) | (pred == 29) | (pred == 52) | ( pred == 91) #.astype(np.uint8) if idx == idx_limit: # 1st frame detection needs to be robust pre_seg_mask_sum = np.sum(seg_mask) # checking for bad detection new_seg_sum = np.sum(seg_mask) diff = abs(new_seg_sum - pre_seg_mask_sum) # if diff > pre_seg_mask_sum/15: # smoothening between segmentation outputs - seems like a bad idea since the model inputs are not connected between timesteps # seg_mask = np.ones_like(pred).astype(np.uint8) # need to add depth (5mt) criterea for calculation for robustness # del new_seg_sum # else: pre_seg_mask_sum = new_seg_sum ### mask Hole filling seg_mask = nd.binary_fill_holes(seg_mask).astype(int) seg_mask = seg_mask.astype(np.uint8) ##### seg_mask_3d = np.dstack((seg_mask, seg_mask, seg_mask)) pred_color = colorEncode( pred, loadmat(os.path.join(model_folder, 'color150.mat'))['colors']) ################################## depth_frame = depth_filter(depth_frame) depth_array = np.asarray(depth_frame.get_data()) # 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()) ############ Plane Detection ## need to add smoothening between frames - by plane weights' variance? try: ### need to add multithreading here (and maybe other methods?) planes_mask_binary = plane_detection(color_image*seg_mask_3d, depth_array*seg_mask,\ loop=5) except TypeError as e: try: print("plane mask 1st error") planes_mask, planes_normal, list_plane_params = test_PlaneDetector_send( img_color=color_image * seg_mask_3d, img_depth=depth_array * seg_mask) except TypeError as e: print("plane mask not detected-skipping frame") continue ## removed this part planes_mask = np.ones_like(depth_array).astype(np.uint8) planes_mask = np.dstack( (planes_mask, planes_mask, planes_mask)) ############################################## ## Hole filling for plane_mask (plane mask isn't binary - fixed that!) planes_mask_binary = nd.binary_fill_holes(planes_mask_binary) planes_mask_binary = planes_mask_binary.astype(np.uint8) # Clean plane mask object detection by seg_mask planes_mask_binary *= seg_mask planes_mask_binary_3d = np.dstack( (planes_mask_binary, planes_mask_binary, planes_mask_binary)) edges = planes_mask_binary - nd.morphology.binary_dilation( planes_mask_binary) # edges calculation edges = cv2.cvtColor(edges, cv2.COLOR_GRAY2BGR) ############################################# # for cv2 output pred_color = cv2.cvtColor(pred_color, cv2.COLOR_RGB2BGR) color_image = cv2.cvtColor(color_image, cv2.COLOR_RGB2BGR) # if save_images == True: # cv2.imwrite("data_/color/frame%d.png" % idx, color_image) # save frame as JPEG file # cv2.imwrite("data_/depth/frame%d.png" % idx, depth_array) # save frame as JPEG file # cv2.imwrite("data_/color_depth/frame%d.png" % idx, depth_color_image) # save frame as JPEG file # cv2.imwrite("data_/thresholded_color/frame%d.png" % idx, thresholded_color_image) # save frame as JPEG file # # cv2.imwrite("data_/thresholded_depth/frame%d.png" % idx, thresholded_depth_image) # save frame as JPEG file # # Blending images alpha = 0.2 beta = (1.0 - alpha) dst = cv2.addWeighted(color_image, alpha, pred_color, beta, 0.0) # kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE,(11,11)) # res = cv2.morphologyEx(planes_mask,cv2.MORPH_OPEN,kernel) dst2 = cv2.addWeighted(depth_color_image, alpha, color_image, beta, 0.0) ## for displaying seg_mask seg_mask = (np.array(seg_mask) * 255).astype(np.uint8) seg_mask = cv2.cvtColor(seg_mask, cv2.COLOR_GRAY2BGR) ################################## ### delete later final_output = color_image * planes_mask_binary_3d mask = (final_output[:, :, 0] == 0) & ( final_output[:, :, 1] == 0) & (final_output[:, :, 2] == 0) final_output[:, :, :3][mask] = [255, 255, 255] ###### # if np.sum(planes_mask) == depth_array.shape[0]*depth_array.shape[1]: # image_set1 = np.vstack((dst, color_image)) # else: image_set1 = np.vstack((color_image, depth_color_image)) # image_set2 = np.vstack((planes_mask_binary_3d*255, seg_mask)) image_set2 = np.vstack((dst, final_output)) # image_set2 = np.vstack((edges, final_output)) combined_images = np.concatenate((image_set1, image_set2), axis=1) if save_images == True: cv2.imwrite("./meeting_example/frame%d.png" % idx, combined_images) try: cv2.imshow('Full Stream', combined_images) except TypeError as e: print(idx, e) key = cv2.waitKey(1) # if pressed escape exit program if key == 27: cv2.destroyAllWindows() break finally: pipeline.stop() cv2.destroyAllWindows() # if save_images == True: # pkl.dump( threshold_mask, open( "data_/depth_threshold.pkl", "wb" ) ) # print("Mask pickle saved") return
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 aligned(self, frames): self.align = rs.align(rs.stream.color) frames = self.align.process(frames) aligned_depth_frame = frames.get_depth_frame() return aligned_depth_frame
import numpy as np import cv2 W = 848 H = 480 # Configure depth and color streams pipeline = rs.pipeline() config = rs.config() config.enable_stream(rs.stream.depth, W, H, rs.format.z16, 30) config.enable_stream(rs.stream.color, W, H, rs.format.bgr8, 30) print("[INFO] start streaming...") pipeline.start(config) aligned_stream = rs.align(rs.stream.color) # alignment between color and depth point_cloud = rs.pointcloud() print("[INFO] loading model...") # download model from: https://github.com/opencv/opencv/wiki/TensorFlow-Object-Detection-API#run-network-in-opencv net = cv2.dnn.readNetFromTensorflow( "frozen_inference_graph.pb", "faster_rcnn_inception_v2_coco_2018_01_28.pbtxt") while True: frames = pipeline.wait_for_frames() frames = aligned_stream.process(frames) color_frame = frames.get_color_frame() depth_frame = frames.get_depth_frame().as_depth_frame() points = point_cloud.calculate(depth_frame) verts = np.asanyarray(points.get_vertices()).view(np.float32).reshape(
def start(self): self.pipeline.start(self.config) self.alignment = rs.align(rs.stream.color)
def __init__(self, scanDuration, rootDir, sharpening, configFile, fps, width, height, visualPreset, laserPower, exposure, gain) : self.scanDuration = scanDuration self.fps = fps self.rootDir = rootDir self.sharpening = sharpening self.width = width self.height = height self.configFile = configFile self.captureName = None self.closed = False if self.configFile : try : with open(self.configFile) as file: configString = json.load(file) configDict = configString configString = json.dumps(configString) self.fps = int(configDict["stream-fps"]) self.width = int(configDict["stream-width"]) self.height = int(configDict["stream-height"]) Logger.printSuccess("Parameters successfully loaded !") except : Logger.printError("Could not read the RealSense configuration file") self.window = cv2.namedWindow("Capture", cv2.WINDOW_AUTOSIZE) Logger.printInfo("Initializing scanner ...") try : #Initialization self.pipeline = rs2.pipeline() self.config = rs2.config() #Enable streams self.config.enable_stream(rs2.stream.depth, self.width, self.height, rs2.format.z16, self.fps) self.config.enable_stream(rs2.stream.color, self.width, self.height, rs2.format.bgr8, self.fps) #Start streaming self.profile = self.pipeline.start(self.config) except Exception as e : Logger.printError("Unable to start the stream, gonna quit.\nException -> " + str(e)) exit() self.device = self.profile.get_device() self.depthSensor = self.device.first_depth_sensor() try : #Set parameters if configFile : advancedMode = rs2.rs400_advanced_mode(self.device) print(configString) advancedMode.load_json(json_content=configString) else : """ 0 -> Custom 1 -> Default 2 -> Hand 3 -> High Accuracy 4 -> High Density 5 -> Medium Density """ self.depthSensor.set_option(rs2.option.visual_preset, visualPreset) self.depthSensor.set_option(rs2.option.laser_power, laserPower) self.depthSensor.set_option(rs2.option.gain, gain) self.depthSensor.set_option(rs2.option.exposure, exposure) #self.depthSensor.set_option(rs2.option.max_distance, 4.0) except Exception as e : Logger.printError("Unable to set one parameter on the RealSense, gonna continue to run.\nException -> " + str(e)) pass self.intrinsics = self.profile.get_stream(rs2.stream.depth).as_video_stream_profile().get_intrinsics() #Create an align object -> aligning depth frames to color frames self.aligner = rs2.align(rs2.stream.color) Logger.printSuccess("Scanner successfully initialized !") self.depthDataset = []
import pyrealsense2 as rs WIDTH = int(1280) HEIGHT = int(720) # Configure depth and color streams pipeline = rs.pipeline() realsense_config = rs.config() realsense_config.enable_stream(rs.stream.depth, WIDTH, HEIGHT, rs.format.z16, 30) realsense_config.enable_stream(rs.stream.color, WIDTH, HEIGHT, rs.format.bgr8, 30) # Record stream (color and depth) to file realsense_config.enable_record_to_file('lego_detection_test.bag') # Create alignment primitive with color as its target stream: alignment_stream = rs.align(rs.stream.color) # Start streaming profile = pipeline.start(realsense_config) # Getting the depth sensor's depth scale depth_sensor = profile.get_device().first_depth_sensor() depth_scale = depth_sensor.get_depth_scale() # Display barcode and QR code location def display(frame, decoded_objects): # Loop over all decoded objects for decoded_object in decoded_objects: points = decoded_object.polygon
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 __init__(self, lower=(96, 59, 53,), upper=(160, 255, 130), callback=None) -> None: self.lower, self.upper = lower, upper # os.system('rosnode kill image_server') self.pipeline = rs.pipeline() 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) # set dep-th units, this should give us better resolution sub 5m? device: rs.device = rs.context().query_devices()[0] advnc_mode = rs.rs400_advanced_mode(device) depth_table_control_group: STDepthTableControl = advnc_mode.get_depth_table() depth_table_control_group.depthUnits = 500 advnc_mode.set_depth_table(depth_table_control_group) # Start streaming profile = self.pipeline.start(config) # Getting the depth sensor's depth scale (see rs-align example for explanation) depth_sensor: DepthSensor = profile.get_device().first_depth_sensor() depth_scale = depth_sensor.get_depth_scale() print("Depth Scale is: ", depth_scale) align_to = rs.stream.color self.align = rs.align(align_to) for i in range(10): try: sensors = sum([dev.query_sensors() for dev in rs.context().query_devices()], []) color_sensor = next( sensor for sensor in sensors if sensor.get_info(rs.camera_info.name) == "RGB Camera") break except Exception as e: print(e) sleep(0.3) print("Setting RGB camera sensor settings") # exposure: 166.0 # white balance: 4600.0 # gain: 64.0 # auto exposure enabled: 1.0 # exposure: 166.0 # white balance: 4600.0 # gain: 64.0 color_sensor.set_option(rs.option.saturation, 60) color_sensor.set_option(rs.option.exposure, 166) color_sensor.set_option(rs.option.enable_auto_exposure, 0) color_sensor.set_option(rs.option.white_balance, 4600) color_sensor.set_option(rs.option.enable_auto_white_balance, 0) color_sensor.set_option(rs.option.gain, 64) align = rs.align(align_to) # color_sensor.set_option(rs.option.enable_auto_exposure, 0) print("auto exposure enabled: {}".format(color_sensor.get_option(rs.option.enable_auto_exposure))) print("exposure: {}".format(color_sensor.get_option(rs.option.exposure))) # 166 print("white balance: {}".format(color_sensor.get_option(rs.option.white_balance))) print("gain: {}".format(color_sensor.get_option(rs.option.gain))) # 64 self.average = StreamingMovingAverage(20) self.average_raw = StreamingMovingAverage(20) self.average_fps = StreamingMovingAverage(20) self.average_area = StreamingMovingAverage(20) self.color = None self.distance = None self.fps = 0 self.area = 0 sleep(2)
def main(): # if not os.path.exists(args.directory): # os.mkdir(args.directory) try: for s in range(15,16): read_path ="/home/user/python_projects/utils-GJ/realsense_bag/kist_scene/"+str(s)+".bag" save_path = "/home/user/python_projects/DenseFusion_yolact_base/living_lab_video/"+str(s)+"/" begin = time.time() index = 0 config = rs.config() config.enable_stream(rs.stream.color) config.enable_stream(rs.stream.depth) pipeline = rs.pipeline() rs.config.enable_device_from_file(config, read_path) profile = pipeline.start(config) align_to = rs.stream.color align = rs.align(align_to) depth_sensor = profile.get_device().first_depth_sensor() depth_scale = depth_sensor.get_depth_scale() print("scale : " + str(depth_scale)) profile = pipeline.get_active_profile() color_stream = profile.get_stream(rs.stream.color) color_profile = rs.video_stream_profile(color_stream) color_intrinsics = color_profile.get_intrinsics() depth_profile = rs.video_stream_profile(profile.get_stream(rs.stream.depth)) depth_intrinsics = depth_profile.get_intrinsics() print("*** color intrinsics ***") print(color_intrinsics) print("*** depth intrinsics ***") print(depth_intrinsics) while True: if time.time() - begin > 22: break time.sleep(0.02) frames = pipeline.wait_for_frames() # align the deph to color frame aligned_frames = align.process(frames) # get aligned frames aligned_depth_frame = aligned_frames.get_depth_frame() aligned_depth_image = np.asanyarray(aligned_depth_frame.get_data()) # scaled_depth_image = depth_image * depth_scale # convert color image to BGR for OpenCV color_frame = frames.get_color_frame() color_image = np.asanyarray(color_frame.get_data()) # r, g, b = cv2.split(color_image) # color_image = cv2.merge((b, g, r)) # cv2.imshow("color", color_image) # cv2.imshow("aligned_depth_image", aligned_depth_image) # cv2.waitKey(0) print("index : " + str(index)) if not os.path.exists(save_path): os.mkdir(save_path) cv2.imwrite(save_path+"depth_image" + str(index) + ".png", aligned_depth_image) cv2.imwrite(save_path+"color_image" + str(index) + ".png", color_image) # cv2.imwrite("/home/user/kist_scene/scene" + str(i) + "/depth_image" + str(index) + ".png", aligned_depth_image) # cv2.imwrite("/home/user/kist_scene/scene" + str(i) + "/color_image" + str(index) + ".png", color_image) index += 1 finally: pass
def camThread(LABELS, results, frameBuffer, camera_mode, camera_width, camera_height, background_transparent_mode, background_img): global fps global detectfps global lastresults global framecount global detectframecount global time1 global time2 global cam global window_name global depth_scale global align_to global align # Configure depth and color streams # Or # Open USB Camera streams if camera_mode == 0: pipeline = rs.pipeline() 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) 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) window_name = "RealSense" elif camera_mode == 1: cam = cv2.VideoCapture(0) if cam.isOpened() != True: print("USB Camera Open Error!!!") sys.exit(0) cam.set(cv2.CAP_PROP_FPS, 30) cam.set(cv2.CAP_PROP_FRAME_WIDTH, camera_width) cam.set(cv2.CAP_PROP_FRAME_HEIGHT, camera_height) window_name = "USB Camera" cv2.namedWindow(window_name, cv2.WINDOW_AUTOSIZE) while True: t1 = time.perf_counter() # 0:= RealSense Mode # 1:= USB Camera Mode if camera_mode == 0: # Wait for a coherent pair of frames: depth and color frames = pipeline.wait_for_frames() depth_frame = frames.get_depth_frame() color_frame = frames.get_color_frame() if not depth_frame or not color_frame: continue if frameBuffer.full(): frameBuffer.get() color_image = np.asanyarray(color_frame.get_data()) elif camera_mode == 1: # USB Camera Stream Read s, color_image = cam.read() if not s: continue if frameBuffer.full(): frameBuffer.get() frames = color_image height = color_image.shape[0] width = color_image.shape[1] frameBuffer.put(color_image.copy()) res = None if not results.empty(): res = results.get(False) detectframecount += 1 imdraw = overlay_on_image(frames, res, LABELS, camera_mode, background_transparent_mode, background_img, depth_scale=depth_scale, align=align) lastresults = res else: imdraw = overlay_on_image(frames, lastresults, LABELS, camera_mode, background_transparent_mode, background_img, depth_scale=depth_scale, align=align) cv2.imshow(window_name, cv2.resize(imdraw, (width, height))) if cv2.waitKey(1)&0xFF == ord('q'): # Stop streaming if pipeline != None: pipeline.stop() sys.exit(0) ## Print FPS framecount += 1 if framecount >= 15: fps = "(Playback) {:.1f} FPS".format(time1/15) detectfps = "(Detection) {:.1f} FPS".format(detectframecount/time2) framecount = 0 detectframecount = 0 time1 = 0 time2 = 0 t2 = time.perf_counter() elapsedTime = t2-t1 time1 += 1/elapsedTime time2 += elapsedTime
print("Depth2 Scale is: ", depth_scale3) # We will be removing the background of objects more than # clipping_distance_in_meters meters away clipping_distance_in_meters = 1 #1 meter clipping_distance = clipping_distance_in_meters / depth_scale clipping_distance2 = clipping_distance_in_meters / depth_scale2 clipping_distance3 = clipping_distance_in_meters / depth_scale3 # 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) align2 = rs.align(align_to) align3 = rs.align(align_to) imgforder_name = 'imgs_front/' depforder_name = 'depths_front/' imgfile_name = 'image_front' depfile_name = 'depth_front' imgforder_name2 = 'imgs_left/' depforder_name2 = 'depths_left/' imgfile_name2 = 'image_left' depfile_name2 = 'depth_left' imgforder_name3 = 'imgs_right/' depforder_name3 = 'depths_right/'
frameset = pipe.wait_for_frames() color_frame = frameset.get_color_frame() depth_frame = frameset.get_depth_frame() # Cleanup: pipe.stop() print("Frames Captured") color = np.asanyarray(color_frame.get_data()) plt.rcParams["axes.grid"] = False plt.rcParams['figure.figsize'] = [12, 6] colorizer = rs.colorizer() # Create alignment primitive with color as its target stream: align = rs.align(rs.stream.color) frameset = align.process(frameset) # Update color and depth frames: aligned_depth_frame = frameset.get_depth_frame() colorized_depth = np.asanyarray( colorizer.colorize(aligned_depth_frame).get_data()) # Show the two frames together: images = np.hstack((color, colorized_depth)) plt.imshow(images) depth = np.asanyarray(aligned_depth_frame.get_data()) # Get data scale from the device and convert to meters depth_scale = profile.get_device().first_depth_sensor().get_depth_scale()
def detect_video_realtime_mp(): # Configure depth and color streams p1 = Process(target=predict_bbox_mp, args=(original_frames, predicted_data)) p2 = Process(target=postprocess_mp, args=(boundingBoxes, original_frames, processed_frames)) p3 = Process(target=Show_Image_mp, args=(processed_frames, original_frames)) p4 = Process(target=socketVideoStream, args=("10.0.0.36", 8080, processed_frames)) p1.start() p2.start() p3.start() p4.start() while True: try: frames = pipeline.wait_for_frames() color_frame = frames.get_color_frame() depth_frame = frames.get_depth_frame() if not depth_frame or not color_frame: continue violate = set() color_image = color_frame.get_data() color_image = np.asanyarray(color_image) # align images align = rs.align(rs.stream.color) frameset = align.process(frames) # Update color and depth frames: aligned_depth_frame = frameset.get_depth_frame() depth = np.asanyarray(aligned_depth_frame.get_data()) depthFrames.put(depth) original_frames.put(color_image) if not predicted_data.empty(): pred_bbox = predicted_data.get() numberOfPeople = len(pred_bbox) bboxes = [] vectors = [] if numberOfPeople >= 1: for bbox in pred_bbox: (sx, sy, ex, ey) = bbox bboxes.append(bbox) w = sx + (ex - sx) / 2 h = sy + (ey - sy) / 2 vectors.append(get3d(int(w), int(h), frames)) if (len(bboxes) > 0): boundingBoxes.put((bboxes, vectors)) except Exception as e: print('Error is ', str(e))
# Getting the depth sensor's depth scale (see rs-align example for explanation) depth_sensor = profile.get_device().first_depth_sensor() depth_scale = depth_sensor.get_depth_scale() print "Depth Scale is: " , depth_scale # We will be removing the background of objects more than # clipping_distance_in_meters meters away clipping_distance_in_meters = 1 #1 meter clipping_distance = clipping_distance_in_meters / depth_scale # Create an align object # rs.align allows us to perform alignment of depth frames to others frames # The "align_to" is the stream type to which we plan to align depth frames. align_to = rs.stream.color align = rs.align(align_to) # Streaming loop try: while True: # Get frameset of color and depth frames = pipeline.wait_for_frames() # frames.get_depth_frame() is a 640x360 depth image # Align the depth frame to color frame aligned_frames = align.process(frames) # Get aligned frames aligned_depth_frame = aligned_frames.get_depth_frame() # aligned_depth_frame is a 640x480 depth image color_frame = aligned_frames.get_color_frame()
config_2.enable_device(sn_list[1]) config_2.enable_stream(rs.stream.depth, WIDTH, HEIGHT, rs.format.z16, 30) config_2.enable_stream(rs.stream.color, WIDTH, HEIGHT, rs.format.bgr8, 30) # Start streaming from both cameras cfg1 = pipeline_1.start(config_1) cfg2 = pipeline_2.start(config_2) profile = cfg1.get_stream(rs.stream.depth) intr1 = profile.as_video_stream_profile().get_intrinsics() print('intr1:', intr1) profile = cfg2.get_stream(rs.stream.depth) intr2 = profile.as_video_stream_profile().get_intrinsics() print('intr2:', intr2) align1 = rs.align(rs.stream.color) align2 = rs.align(rs.stream.color) if not os.path.exists('tmp'): os.makedirs('tmp') cam_dir_1 = 'tmp/depth_cam1' cam_dir_2 = 'tmp/depth_cam2' if not os.path.exists(cam_dir_1): os.makedirs(os.path.join(cam_dir_1, 'depth')) os.makedirs(os.path.join(cam_dir_1, 'color')) if not os.path.exists(cam_dir_2): os.makedirs(os.path.join(cam_dir_2, 'depth')) os.makedirs(os.path.join(cam_dir_2, 'color')) with open(os.path.join(cam_dir_1, 'sn.txt'), 'w') as fout: fout.write(sn_list[0] + '\n') with open(os.path.join(cam_dir_1, 'intr.txt'), 'w') as fout: