# ****************************************************************************** # Copyright (c) 2023 Orbbec 3D Technology, Inc # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http:# www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. # ****************************************************************************** from pyorbbecsdk import * import cv2 import numpy as np from utils import frame_to_bgr_image import sys import argparse ESC_KEY = 27 def main(argv): pipeline = Pipeline() device = pipeline.get_device() device_info = device.get_device_info() device_pid = device_info.get_pid() config = Config() parser = argparse.ArgumentParser() parser.add_argument("-m", "--mode", help="align mode, HW=hardware mode,SW=software mode,NONE=disable align", type=str, default='HW') parser.add_argument("-s", "--enable_sync", help="enable sync", type=bool, default=True) args = parser.parse_args() align_mode = args.mode enable_sync = args.enable_sync try: profile_list = pipeline.get_stream_profile_list(OBSensorType.COLOR_SENSOR) color_profile = profile_list.get_default_video_stream_profile() config.enable_stream(color_profile) profile_list = pipeline.get_stream_profile_list(OBSensorType.DEPTH_SENSOR) assert profile_list is not None depth_profile = profile_list.get_default_video_stream_profile() assert depth_profile is not None print("color profile : {}x{}@{}_{}".format(color_profile.get_width(), color_profile.get_height(), color_profile.get_fps(), color_profile.get_format())) print("depth profile : {}x{}@{}_{}".format(depth_profile.get_width(), depth_profile.get_height(), depth_profile.get_fps(), depth_profile.get_format())) config.enable_stream(depth_profile) except Exception as e: print(e) return if align_mode == 'HW': if device_pid == 0x066B: # Femto Mega does not support hardware D2C, and it is changed to software D2C config.set_align_mode(OBAlignMode.SW_MODE) else: config.set_align_mode(OBAlignMode.HW_MODE) elif align_mode == 'SW': config.set_align_mode(OBAlignMode.SW_MODE) else: config.set_align_mode(OBAlignMode.DISABLE) if enable_sync: try: pipeline.enable_frame_sync() except Exception as e: print(e) try: pipeline.start(config) except Exception as e: print(e) return while True: try: frames: FrameSet = pipeline.wait_for_frames(100) if frames is None: continue color_frame = frames.get_color_frame() if color_frame is None: continue # covert to RGB format color_image = frame_to_bgr_image(color_frame) if color_image is None: print("failed to convert frame to image") continue depth_frame = frames.get_depth_frame() if depth_frame is None: continue width = depth_frame.get_width() height = depth_frame.get_height() scale = depth_frame.get_depth_scale() depth_data = np.frombuffer(depth_frame.get_data(), dtype=np.uint16) depth_data = depth_data.reshape((height, width)) depth_data = depth_data.astype(np.float32) * scale depth_image = cv2.normalize(depth_data, None, 0, 255, cv2.NORM_MINMAX, dtype=cv2.CV_8U) depth_image = cv2.applyColorMap(depth_image, cv2.COLORMAP_JET) # overlay color image on depth image depth_image = cv2.addWeighted(color_image, 0.5, depth_image, 0.5, 0) cv2.imshow("SyncAlignViewer ", depth_image) key = cv2.waitKey(1) if key == ord('q') or key == ESC_KEY: break except KeyboardInterrupt: break pipeline.stop() if __name__ == "__main__": main(sys.argv[1:])