[sample] roll-pitch-yaw from T265

wrappers/python/examples/t265_rpy.py

@@ -0,0 +1,51 @@
+#!/usr/bin/python
+# -*- coding: utf-8 -*-
+## License: Apache 2.0. See LICENSE file in root directory.
+## Copyright(c) 2019 Intel Corporation. All Rights Reserved.
+
+#####################################################
+##           librealsense T265 rpy example         ##
+#####################################################
+
+# First import the library
+import pyrealsense2 as rs
+import numpy as np
+import transformations as tf
+import math as m
+
+H_aeroRef_T265Ref = np.array([[0,0,-1,0],[1,0,0,0],[0,-1,0,0],[0,0,0,1]])
+H_T265body_aeroBody = np.linalg.inv(H_aeroRef_T265Ref)
+
+# Declare RealSense pipeline, encapsulating the actual device and sensors
+pipe = rs.pipeline()
+
+# Build config object and request pose data
+cfg = rs.config()
+cfg.enable_stream(rs.stream.pose)
+
+# Start streaming with requested config
+pipe.start(cfg)
+
+try:
+    for _ in range(5000):
+        # Wait for the next set of frames from the camera
+        frames = pipe.wait_for_frames()
+
+        # Fetch pose frame
+        pose = frames.get_pose_frame()
+        if pose:
+            # Print some of the pose data to the terminal
+            data = pose.get_pose_data()
+
+            H_T265Ref_T265body = tf.quaternion_matrix([data.rotation.w, data.rotation.x,data.rotation.y,data.rotation.z]) # in transformations, Quaternions w+ix+jy+kz are represented as [w, x, y, z]!
+
+            # transform to aeronautic coordinates (body AND reference frame!)
+            H_aeroRef_aeroBody = H_aeroRef_T265Ref.dot( H_T265Ref_T265body.dot( H_T265body_aeroBody ))
+
+            rpy_rad = np.array( tf.euler_from_matrix(H_aeroRef_aeroBody, 'rxyz') )
+
+            print("Frame #{}".format(pose.frame_number))
+            print("RPY [deg]: {}".format(rpy_rad*180/m.pi))
+
+finally:
+    pipe.stop()