import pyrealsense2 as rs
import numpy as np
import math

# 初始化T265相机
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.pose)
pipeline.start(config)


def quaternion_to_euler_angle(qx, qy, qz, qw):
    # 计算欧拉角
    roll = np.arctan2(2 * (qw * qx + qy * qz), 1 - 2 * (qx**2 + qy**2))
    pitch = np.arcsin(2 * (qw * qy - qz * qx))
    yaw = np.arctan2(2 * (qw * qz + qx * qy), 1 - 2 * (qy**2 + qz**2))

    # 转换为度数
    roll = np.rad2deg(roll)
    pitch = np.rad2deg(pitch)
    yaw = np.rad2deg(yaw)

    return roll,pitch,yaw



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

        # 获取相机位姿数据
        pose = frames.get_pose_frame()

        if pose:
            data = pose.get_pose_data()
            a=quaternion_to_euler_angle(data.rotation.w,data.rotation.x,data.rotation.y,data.rotation.z)
            roll=a[0]
            yaw=a[1]
            pitch=a[2]
            print(a[0])
finally:
    pipeline.stop()