import pyrealsense2 as rs
import numpy as np
import cv2

def run_realsense():
    # 初始化RealSense
    pipeline = rs.pipeline()
    config = rs.config()
    config.enable_stream(rs.stream.depth, 848, 480, rs.format.z16, 30)
    config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30)

    # 启动相机
    profile = pipeline.start(config)

    try:
        while True:
            # 等待一帧
            frames = pipeline.wait_for_frames()

            # 对齐深度和彩色图
            align = rs.align(rs.stream.color)
            aligned_frames = align.process(frames)

            depth_frame = aligned_frames.get_depth_frame()
            color_frame = aligned_frames.get_color_frame()

            if not depth_frame or not color_frame:
                continue

            # 转 numpy
            depth_image = np.asanyarray(depth_frame.get_data())
            color_image = np.asanyarray(color_frame.get_data())

            # 深度图可视化（转为伪彩色）
            depth_colormap = cv2.convertScaleAbs(depth_image, alpha=0.03)
            depth_colormap = cv2.applyColorMap(depth_colormap, cv2.COLORMAP_JET)

            # 显示
            cv2.imshow("RGB Image", color_image)
            cv2.imshow("Depth Image", depth_colormap)

            # 按 q 退出
            if cv2.waitKey(1) & 0xFF == ord('q'):
                break

    finally:
        pipeline.stop()
        cv2.destroyAllWindows()

if __name__ == "__main__":
    run_realsense()