6DoF tracking support

hellocardboard-android/src/main/jni/util.cc

@@ -430,7 +430,7 @@ static constexpr uint64_t kNanosInSeconds = 1000000000;
 
 long GetMonotonicTimeNano() {
   struct timespec res;
-  clock_gettime(CLOCK_MONOTONIC, &res);
+  clock_gettime(CLOCK_BOOTTIME, &res);
   return (res.tv_sec * kNanosInSeconds) + res.tv_nsec;
 }
 

sdk/CMakeLists.txt

@@ -48,6 +48,8 @@ file(GLOB screen_params_srcs "screen_params/android/*.cc")
 file(GLOB device_params_srcs "device_params/android/*.cc")
 # Rendering Sources
 file(GLOB rendering_srcs "rendering/*.cc")
+# 6DoF sources
+file(GLOB sixdof_srcs "sixdof/*.cc")
 
 # === Cardboard Unity JNI ===
 file(GLOB cardboard_unity_jni_srcs "unity/android/*.cc")
@@ -71,6 +73,7 @@ add_library(cardboard_api SHARED
     ${screen_params_srcs}
     ${device_params_srcs}
     ${rendering_srcs}
+    ${sixdof_srcs}
     # Cardboard Unity JNI sources
     ${cardboard_unity_jni_srcs}
     # Cardboard Unity Wrapper sources

sdk/cardboard.cc

@@ -321,6 +321,18 @@ void CardboardHeadTracker_getPose(CardboardHeadTracker* head_tracker,
   std::memcpy(orientation, &out_orientation[0], 4 * sizeof(float));
 }
 
+// Aryzon 6DoF
+void CardboardHeadTracker_addSixDoFData(CardboardHeadTracker* head_tracker,
+                                        int64_t timestamp_ns,
+                                        float* position,
+                                        float* orientation) {
+  if (CARDBOARD_IS_NOT_INITIALIZED() || CARDBOARD_IS_ARG_NULL(head_tracker)) {
+    return;
+  }
+
+  static_cast<cardboard::HeadTracker*>(head_tracker)->AddSixDoFData(timestamp_ns, position, orientation);
+}
+
 void CardboardQrCode_getSavedDeviceParams(uint8_t** encoded_device_params,
                                           int* size) {
   if (CARDBOARD_IS_NOT_INITIALIZED() ||

sdk/head_tracker.cc

@@ -23,19 +23,38 @@
 
 namespace cardboard {
 
+// Aryzon 6DoF
+const int rotation_samples = 10;
+const int position_samples = 3;
+const int64_t max6DoFTimeDifference = 200000000; // Maximum time difference between last pose state timestamp and last 6DoF timestamp, if it takes longer than this the last known location of sixdof will be used
+
 HeadTracker::HeadTracker()
     : is_tracking_(false),
       sensor_fusion_(new SensorFusionEkf()),
       latest_gyroscope_data_({0, 0, Vector3::Zero()}),
       accel_sensor_(new SensorEventProducer<AccelerometerData>()),
-      gyro_sensor_(new SensorEventProducer<GyroscopeData>()) {
+      gyro_sensor_(new SensorEventProducer<GyroscopeData>()),
+      // Aryzon 6DoF
+      rotation_data_(new RotationData(rotation_samples)),
+      position_data_(new PositionData(position_samples)),
+      start_orientation_(screen_params::getScreenOrientation()) { // Aryzon multiple orientations
   sensor_fusion_->SetBiasEstimationEnabled(/*kGyroBiasEstimationEnabled*/ true);
   on_accel_callback_ = [&](const AccelerometerData& event) {
     OnAccelerometerData(event);
   };
   on_gyro_callback_ = [&](const GyroscopeData& event) {
     OnGyroscopeData(event);
   };
+
+  // Aryzon multiple orientations
+  if (start_orientation_ == screen_params::LandscapeLeft) {
+      ekf_to_head_tracker = Rotation::FromYawPitchRoll(-M_PI / 2.0, 0, -M_PI / 2.0);
+  } else if (start_orientation_ == screen_params::LandscapeRight) {
+      ekf_to_head_tracker = Rotation::FromYawPitchRoll(M_PI / 2.0, 0, M_PI / 2.0);
+  } else {
+      // Portrait
+      ekf_to_head_tracker = Rotation::FromYawPitchRoll(M_PI / 2.0, M_PI / 2.0, M_PI / 2.0);
+  }
 }
 
 HeadTracker::~HeadTracker() { UnregisterCallbacks(); }
@@ -67,36 +86,65 @@ void HeadTracker::GetPose(int64_t timestamp_ns,
                           std::array<float, 4>& out_orientation) const {
   Rotation predicted_rotation;
   const PoseState pose_state = sensor_fusion_->GetLatestPoseState();
-  if (!sensor_fusion_->IsFullyInitialized()) {
-    CARDBOARD_LOGI(
-        "Head Tracker not fully initialized yet. Using pose prediction only.");
-    predicted_rotation = pose_prediction::PredictPose(timestamp_ns, pose_state);
-  } else {
-    predicted_rotation = pose_state.sensor_from_start_rotation;
-  }
+  predicted_rotation = pose_prediction::PredictPose(timestamp_ns, pose_state);
 
   // In order to update our pose as the sensor changes, we begin with the
   // inverse default orientation (the orientation returned by a reset sensor),
   // apply the current sensor transformation, and then transform into display
   // space.
-  // TODO(b/135488467): Support different screen orientations.
-  const Rotation ekf_to_head_tracker =
-      Rotation::FromYawPitchRoll(-M_PI / 2.0, 0, -M_PI / 2.0);
-  const Rotation sensor_to_display =
-      Rotation::FromAxisAndAngle(Vector3(0, 0, 1), M_PI / 2.0);
-
-  const Vector4 q =
-      (sensor_to_display * predicted_rotation * ekf_to_head_tracker)
-          .GetQuaternion();
-  Rotation rotation;
-  rotation.SetQuaternion(q);
-
-  out_orientation[0] = static_cast<float>(rotation.GetQuaternion()[0]);
-  out_orientation[1] = static_cast<float>(rotation.GetQuaternion()[1]);
-  out_orientation[2] = static_cast<float>(rotation.GetQuaternion()[2]);
-  out_orientation[3] = static_cast<float>(rotation.GetQuaternion()[3]);
-
-  out_position = ApplyNeckModel(out_orientation, 1.0);
+  Rotation sensor_to_display;
+
+  // Aryzon multiple orientations
+  // Very fast implementation on iOS, pretty fast for Android
+  screen_params::ScreenOrientation orientation = screen_params::getScreenOrientation();
+
+  if (orientation == screen_params::LandscapeLeft) {
+      sensor_to_display = Rotation::FromAxisAndAngle(Vector3(0, 0, 1), M_PI / 2.0);
+  } else  if (orientation == screen_params::LandscapeRight) {
+      sensor_to_display = Rotation::FromAxisAndAngle(Vector3(0, 0, 1), -M_PI / 2.0);
+  } else { // Portrait
+      sensor_to_display = Rotation::FromAxisAndAngle(Vector3(0, 0, 1), 0.0);
+  }
+
+  Rotation rotation = sensor_to_display * predicted_rotation * ekf_to_head_tracker;
+
+  // Aryzon 6DoF
+  // Save rotation sample with timestamp to be used in AddSixDoFData()
+  rotation_data_->AddSample(rotation.GetQuaternion(), timestamp_ns);
+
+  if (position_data_->IsValid() && pose_state.timestamp - position_data_->GetLatestTimestamp() < max6DoFTimeDifference) {
+      // 6DoF is recently updated
+
+      rotation = rotation * -difference_to_6DoF_;
+
+      Vector3 p = position_data_->GetExtrapolatedForTimeStamp(timestamp_ns);
+      std::array<float, 3> predicted_position_ = {(float)p[0], (float)p[1], (float)p[2]};
+
+      out_orientation[0] = static_cast<float>(rotation.GetQuaternion()[0]);
+      out_orientation[1] = static_cast<float>(rotation.GetQuaternion()[1]);
+      out_orientation[2] = static_cast<float>(rotation.GetQuaternion()[2]);
+      out_orientation[3] = static_cast<float>(rotation.GetQuaternion()[3]);
+
+      out_position = predicted_position_;
+  } else {
+      // 6DoF is not recently updated
+
+      out_orientation[0] = static_cast<float>(rotation.GetQuaternion()[0]);
+      out_orientation[1] = static_cast<float>(rotation.GetQuaternion()[1]);
+      out_orientation[2] = static_cast<float>(rotation.GetQuaternion()[2]);
+      out_orientation[3] = static_cast<float>(rotation.GetQuaternion()[3]);
+
+      out_position = ApplyNeckModel(out_orientation, 1.0);
+
+      if (position_data_->IsValid()) {
+          // Apply last known 6DoF position if 6DoF was data previously added, while still applying neckmodel.
+
+          Vector3 last_known_position_ = position_data_->GetLatestData();
+          out_position[0] += (float)last_known_position_[0];
+          out_position[1] += (float)last_known_position_[1];
+          out_position[2] += (float)last_known_position_[2];
+      }
+  }
 }
 
 Rotation HeadTracker::GetDefaultOrientation() const {
@@ -129,4 +177,28 @@ void HeadTracker::OnGyroscopeData(const GyroscopeData& event) {
   sensor_fusion_->ProcessGyroscopeSample(event);
 }
 
+// Aryzon 6DoF
+void HeadTracker::AddSixDoFData(int64_t timestamp_ns, float* pos, float* orientation) {
+  if (!is_tracking_) {
+    return;
+  }
+    position_data_->AddSample(Vector3(pos[0], pos[1], pos[2]), timestamp_ns);
+
+    if (position_data_->IsValid() && rotation_data_->IsValid()) {
+        // 6DoF timestamp needs to be before the latest rotation_data timestamp
+        Rotation gyroAtTimeOfSixDoF = Rotation::FromQuaternion(rotation_data_->GetInterpolatedForTimeStamp(timestamp_ns));
+        Rotation sixDoFRotation = Rotation::FromQuaternion(Vector4(0, orientation[1], 0, orientation[3]));
+
+        Rotation difference = gyroAtTimeOfSixDoF * -sixDoFRotation;
+
+        // Only synchronize rotation around the y axis
+        Vector4 diffQ = difference.GetQuaternion();
+        diffQ[0] = 0;
+        diffQ[2] = 0;
+
+        // Quaternion will be normalized in this call:
+        difference_to_6DoF_.SetQuaternion(diffQ);
+    }
+}
+
 }  // namespace cardboard

sdk/head_tracker.h

@@ -26,6 +26,13 @@
 #include "sensors/sensor_fusion_ekf.h"
 #include "util/rotation.h"
 
+// Aryzon 6DoF
+#include "sixdof/rotation_data.h"
+#include "sixdof/position_data.h"
+
+// Aryzon multiple orientations
+#include "screen_params.h"
+
 namespace cardboard {
 
 // HeadTracker encapsulates pose tracking by connecting sensors
@@ -46,6 +53,12 @@ class HeadTracker {
   // TODO(b/135488467): Support different display to sensor orientations.
   void GetPose(int64_t timestamp_ns, std::array<float, 3>& out_position,
                std::array<float, 4>& out_orientation) const;
+
+  // Function to be called when receiving SixDoFData.
+  //
+  // Aryzon 6DoF
+  // @param event sensor event.
+  void AddSixDoFData(int64_t timestamp_ns, float* position, float* orientation);
 
  private:
   // Function called when receiving AccelerometerData.
@@ -83,6 +96,15 @@ class HeadTracker {
   // Callback functions registered to the input SingleTypeEventProducer.
   std::function<void(AccelerometerData)> on_accel_callback_;
   std::function<void(GyroscopeData)> on_gyro_callback_;
+
+  // Aryzon 6DoF
+  RotationData *rotation_data_;
+  PositionData *position_data_;
+  Rotation difference_to_6DoF_;
+
+  // Aryzon multiple orientations
+  Rotation ekf_to_head_tracker;
+  const screen_params::ScreenOrientation start_orientation_;
 };
 
 }  // namespace cardboard

sdk/include/cardboard.h

@@ -398,6 +398,22 @@ void CardboardHeadTracker_getPose(CardboardHeadTracker* head_tracker,
                                   int64_t timestamp_ns, float* position,
                                   float* orientation);
 
+/// Aryzon 6DoF
+/// Sends through the event with pose and timestamp data from 6DoF tracker
+///
+/// @pre @p head_tracker Must not be null.
+/// When it is unmet, a call to this function results in a no-op.
+///
+/// @param[in]      head_tracker            Head tracker object pointer.
+/// @param[in]      timestamp_ns            The timestamp for the data in
+///     nanoseconds in system monotonic clock.
+/// @param[out]     position                3 floats for (x, y, z).
+/// @param[out]     orientation             4 floats for quaternion
+void CardboardHeadTracker_addSixDoFData(CardboardHeadTracker* head_tracker,
+                                        int64_t timestamp_ns,
+                                        float* position,
+                                        float* orientation);
+
 /// @}
 
 /////////////////////////////////////////////////////////////////////////////

sdk/screen_params.h

@@ -28,6 +28,16 @@ void initializeAndroid(JavaVM* vm, jobject context);
 #endif
 void getScreenSizeInMeters(int width_pixels, int height_pixels,
                            float* out_width_meters, float* out_height_meters);
+
+// Aryzon multiple orientations
+enum ScreenOrientation {
+    LandscapeLeft,
+    Portrait,
+    LandscapeRight,
+    PortraitUpsideDown
+};
+ScreenOrientation getScreenOrientation();
+
 }  // namespace screen_params
 }  // namespace cardboard
 

sdk/screen_params/android/java/com/google/cardboard/sdk/screenparams/ScreenParamsUtils.java

@@ -21,6 +21,13 @@
 import android.util.DisplayMetrics;
 import android.view.WindowManager;
 
+// Aryzon multiple orientations
+import android.content.res.Configuration;
+import android.hardware.SensorManager;
+import android.view.Display;
+import android.view.OrientationEventListener;
+import android.view.Surface;
+
 /** Utility methods to manage the screen parameters. */
 public abstract class ScreenParamsUtils {
   /** Holds the screen pixel density. */
@@ -68,4 +75,81 @@ public static ScreenPixelDensity getScreenPixelDensity(Context context) {
     }
     return new ScreenPixelDensity(displayMetrics.xdpi, displayMetrics.ydpi);
   }
+
+    // Aryzon multiple orientations
+    public static class ScreenOrientation {
+      /**
+       * The screen orientation
+       * 0 = lanscape left
+       * 1 = portrait
+       * 2 = landscape right
+       * 3 = portrait upside-down
+       */
+      public static Context context;
+      public static int orientation;
+      public static int previousOrientation;
+      public static OrientationEventListener orientationEventListener;
+
+      public ScreenOrientation(final Context context) {
+        this.context = context;
+        this.orientation = CurrentOrientation(context);
+
+        this.orientationEventListener = new OrientationEventListener(context, SensorManager.SENSOR_DELAY_UI) {
+          public void onOrientationChanged(int orientation_) {
+
+            int newOrientation = context.getResources().getConfiguration().orientation;
+
+            if (orientation_ != ORIENTATION_UNKNOWN && previousOrientation != newOrientation) {
+              orientation = CurrentOrientation(ScreenOrientation.context);
+              previousOrientation = newOrientation;
+            }
+          }
+        };
+        if (this.orientationEventListener.canDetectOrientation()) {
+          this.orientationEventListener.enable();
+        }
+      }
+    }
+
+    private static ScreenOrientation screenOrientation;
+    private static Display defaultDisplay;
+
+    public static int getScreenOrientation(Context context) {
+      if (screenOrientation == null) {
+
+        screenOrientation = new ScreenOrientation(context);
+      }
+      return screenOrientation.orientation;
+
+    }
+
+    private static int CurrentOrientation (Context context) {
+      if (defaultDisplay == null) {
+        WindowManager windowManager = (WindowManager) context.getSystemService(Context.WINDOW_SERVICE);
+        defaultDisplay = windowManager.getDefaultDisplay();
+      }
+
+      int orientation = context.getResources().getConfiguration().orientation;
+      // This call takes a long time so we don't call this every frame, only when rotation changes
+      int rotation = defaultDisplay.getRotation();
+
+      if (orientation == Configuration.ORIENTATION_LANDSCAPE
+              &&  (rotation == Surface.ROTATION_0
+              ||  rotation == Surface.ROTATION_90)) {
+        return 0;
+      } else if (orientation == Configuration.ORIENTATION_PORTRAIT
+              &&  (rotation == Surface.ROTATION_0
+              ||  rotation == Surface.ROTATION_90)) {
+        return 1;
+      } else if (orientation == Configuration.ORIENTATION_LANDSCAPE
+              &&  (rotation == Surface.ROTATION_180
+              ||  rotation == Surface.ROTATION_270)) {
+        return 2;
+      } else if (orientation == Configuration.ORIENTATION_PORTRAIT
+              &&  (rotation == Surface.ROTATION_180
+              ||  rotation == Surface.ROTATION_270)) {
+        return 3;
+      }
+      return 3;
+    }
 }