11 import pyrealsense2
as rs
19 cfg.enable_stream(rs.stream.pose)
27 frames = pipe.wait_for_frames()
30 pose = frames.get_pose_frame()
33 data = pose.get_pose_data()
44 pitch = -m.asin(2.0 * (x*z - w*y)) * 180.0 / m.pi;
45 roll = m.atan2(2.0 * (w*x + y*z), w*w - x*x - y*y + z*z) * 180.0 / m.pi;
46 yaw = m.atan2(2.0 * (w*z + x*y), w*w + x*x - y*y - z*z) * 180.0 / m.pi;
48 print(
"Frame #{}".format(pose.frame_number))
49 print(
"RPY [deg]: Roll: {0:.7f}, Pitch: {1:.7f}, Yaw: {2:.7f}".format(roll, pitch, yaw))
static std::string print(const transformation &tf)