Alternative, possibly simpler approach for obtaining pitch, roll and …

doc/installation_osx.md

@@ -6,21 +6,21 @@
 
 ## Building from Source
 
-1. Install CommantLineTools `sudo xcode-select` or download XCode 6.0+ via the AppStore
+1. Install CommantLineTools `sudo xcode-select --install` or download XCode 6.0+ via the AppStore
 2. Install the Homebrew package manager via terminal - [link](http://brew.sh/)
 3. Install the following packages via brew:
   * `brew install cmake libusb pkg-config`
-  * `brew cask install vulkan-sdk`
+  * `brew cask install apenngrace/vulkan/vulkan-sdk`
 
 **Note** *librealsense* requires CMake version 3.8+ that can also be obtained via the [official CMake site](https://cmake.org/download/).  
 
 
 4. Generate XCode project:
   * `mkdir build && cd build`
   * `sudo xcode-select --reset`
-  * `cmake .. -G Xcode -DBUILD_EXAMPLES=true -DBUILD_WITH_OPENMP=false -DHWM_OVER_XU=false`
+  * `cmake .. -DBUILD_EXAMPLES=true -DBUILD_WITH_OPENMP=false -DHWM_OVER_XU=false`
 5. Build the Project
-  * `make -j`
+  * `make -j2`
 
 > **Note:** On some Mac systems you might encounter `ld: library not found for -lusb-1.0` error (either in the terminal during make or in XCode) This can be worked-around by setting environment variable: `/bin/launchctl setenv LIBRARY_PATH /usr/local/lib`
 

wrappers/python/examples/t265_rpy.py

@@ -9,13 +9,8 @@
 
 # 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()
 
@@ -27,7 +22,7 @@
 pipe.start(cfg)
 
 try:
-    for _ in range(5000):
+    while (True):
         # Wait for the next set of frames from the camera
         frames = pipe.wait_for_frames()
 
@@ -37,15 +32,22 @@
             # 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 ))
+            # Euler angles from pose quaternion
+            # See also https://github.com/IntelRealSense/librealsense/issues/5178#issuecomment-549795232
+            # and https://github.com/IntelRealSense/librealsense/issues/5178#issuecomment-550217609
 
-            rpy_rad = np.array( tf.euler_from_matrix(H_aeroRef_aeroBody, 'sxyz') ) # Rz(yaw)*Ry(pitch)*Rx(roll) body w.r.t. reference frame
+            w = data.rotation.w
+            x = data.rotation.z
+            y = data.rotation.x
+            z = data.rotation.y
 
+            pitch =  -m.asin(2.0 * (x*z - w*y)) * 180.0 / m.pi;
+            roll  =  m.atan2(2.0 * (w*x + y*z), w*w - x*x - y*y + z*z) * 180.0 / m.pi;
+            yaw   =  m.atan2(2.0 * (w*z + x*y), w*w + x*x - y*y - z*z) * 180.0 / m.pi;
+
             print("Frame #{}".format(pose.frame_number))
-            print("RPY [deg]: {}".format(rpy_rad*180/m.pi))
+            print("RPY [deg]: Roll: {0:.7f}, Pitch: {1:.7f}, Yaw: {2:.7f}".format(roll, pitch, yaw))
+
 
 finally:
     pipe.stop()