6DoF tracking support

hellocardboard-android/src/main/AndroidManifest.xml

@@ -18,10 +18,10 @@
         android:supportsRtl="true"
         android:theme="@style/AppTheme">
         <activity
+            android:configChanges="orientation"
             android:name="com.google.cardboard.VrActivity"
             android:label="@string/title_activity_vr"
             android:theme="@style/Theme.AppCompat.NoActionBar"
-            android:screenOrientation="landscape"
             android:exported="true">
             <intent-filter>
                 <action android:name="android.intent.action.MAIN" />

hellocardboard-ios/HelloCardboardViewController.mm

@@ -85,9 +85,14 @@ - (BOOL)prefersHomeIndicatorAutoHidden {
   return true;
 }
 
+- (void)viewLayoutMarginsDidChange {
+  [super viewLayoutMarginsDidChange];
+  _updateParams = YES;
+}
+
 - (UIInterfaceOrientationMask)supportedInterfaceOrientations {
-  // Cardboard only supports landscape right orientation for inserting the phone in the viewer.
-  return UIInterfaceOrientationMaskLandscapeRight;
+  // Cardboard supports all screen orientations except Portrait Upside Down
+  return UIInterfaceOrientationMaskAllButUpsideDown;
 }
 
 - (void)glkView:(GLKView *)view drawInRect:(CGRect)rect {
@@ -119,13 +124,6 @@ - (BOOL)updateCardboardParams {
   int height = screenRect.size.height * screenScale;
   int width = screenRect.size.width * screenScale;
 
-  // Rendering coordinates asumes landscape orientation.
-  if (height > width) {
-    int temp = height;
-    height = width;
-    width = temp;
-  }
-
   // Create CardboardLensDistortion.
   CardboardLensDistortion_destroy(_cardboardLensDistortion);
   _cardboardLensDistortion =

sdk/CMakeLists.txt

@@ -50,6 +50,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/opengl_*.cc")
+# 6DoF sources
+file(GLOB sixdof_srcs "sixdof/*.cc")
 
 # === Cardboard Unity JNI ===
 file(GLOB cardboard_unity_jni_srcs "unity/android/*.cc")
@@ -73,6 +75,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
@@ -81,8 +84,9 @@ add_library(cardboard_api SHARED
     ${cardboard_xr_provider_srcs})
 
 # Includes
-target_include_directories(cardboard_api
-    PRIVATE ../third_party/unity_plugin_api)
+target_include_directories(cardboard_api PRIVATE
+    ../third_party/unity_plugin_api
+    include)
 
 # Build
 target_link_libraries(cardboard_api

sdk/cardboard.cc

@@ -119,6 +119,13 @@ void Cardboard_initializeAndroid(JavaVM* vm, jobject context) {
 }
 #endif
 
+CardboardScreenOrientation CardboardScreenParameters_getScreenOrientation() {
+  if (CARDBOARD_IS_NOT_INITIALIZED()) {
+    return kUnknown;
+  }
+  return cardboard::screen_params::getScreenOrientation();
+}
+
 CardboardLensDistortion* CardboardLensDistortion_create(
     const uint8_t* encoded_device_params, int size, int display_width,
     int display_height) {
@@ -313,6 +320,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

@@ -19,28 +19,48 @@
 #include "util/logging.h"
 #include "util/vector.h"
 #include "util/vectorutils.h"
+#include "screen_params.h"
 
 namespace cardboard {
 
-// TODO(b/135488467): Support different screen orientations.
-const Rotation HeadTracker::kEkfToHeadTrackerRotation =
-    Rotation::FromYawPitchRoll(-M_PI / 2.0, 0, -M_PI / 2.0);
-
-const Rotation HeadTracker::kSensorToDisplayRotation =
-    Rotation::FromAxisAndAngle(Vector3(0, 0, 1), M_PI / 2.0);
+// Aryzon 6DoF
+constexpr int kRotationSamples = 10;
+constexpr int kPositionSamples = 6;
+constexpr int64_t kMaxSixDoFTimeDifference = 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
+constexpr float kReduceBiasRate = 0.05;
 
 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(kRotationSamples)),
+      position_data_(new PositionData(kPositionSamples)) {
+
   on_accel_callback_ = [&](const AccelerometerData& event) {
     OnAccelerometerData(event);
   };
   on_gyro_callback_ = [&](const GyroscopeData& event) {
     OnGyroscopeData(event);
   };
+
+  switch(screen_params::getScreenOrientation()) {
+    case kLandscapeLeft:
+        ekf_to_head_tracker_ = Rotation::FromYawPitchRoll(-M_PI / 2.0, 0, -M_PI / 2.0);
+      break;
+    case kLandscapeRight:
+        ekf_to_head_tracker_ = Rotation::FromYawPitchRoll(M_PI / 2.0, 0, M_PI / 2.0);
+      break;
+    default: // Portrait and PortraitUpsideDown
+        ekf_to_head_tracker_ = Rotation::FromYawPitchRoll(M_PI / 2.0, M_PI / 2.0, M_PI / 2.0);
+      break;
+  }
+  ekf_to_sixDoF_ = Rotation::Identity();
+  smooth_ekf_to_sixDoF_ = Rotation::Identity();
+  steady_start_ = Rotation::Identity();
+  steady_frames_ = -1;
 }
 
 HeadTracker::~HeadTracker() { UnregisterCallbacks(); }
@@ -58,35 +78,79 @@ void HeadTracker::Pause() {
   event.data = Vector3::Zero();
 
   OnGyroscopeData(event);
-
   is_tracking_ = false;
 }
 
 void HeadTracker::Resume() {
+  if (!is_tracking_) {
+    RegisterCallbacks();
+  }
+  steady_frames_ = -1;
+  steady_start_ = Rotation::Identity();
   is_tracking_ = true;
-  RegisterCallbacks();
 }
 
 void HeadTracker::GetPose(int64_t timestamp_ns,
                           std::array<float, 3>& out_position,
                           std::array<float, 4>& out_orientation) const {
-  const Rotation predicted_rotation =
-      sensor_fusion_->PredictRotation(timestamp_ns);
-
-  // 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.
-  const Vector4 orientation = (kSensorToDisplayRotation * predicted_rotation *
-                               kEkfToHeadTrackerRotation)
-                                  .GetQuaternion();
-
-  out_orientation[0] = static_cast<float>(orientation[0]);
-  out_orientation[1] = static_cast<float>(orientation[1]);
-  out_orientation[2] = static_cast<float>(orientation[2]);
-  out_orientation[3] = static_cast<float>(orientation[3]);
-
-  out_position = ApplyNeckModel(out_orientation, 1.0);
+
+    Rotation sensor_to_display;
+
+    switch(screen_params::getScreenOrientation()) {
+      case kLandscapeLeft:
+        sensor_to_display = Rotation::FromAxisAndAngle(Vector3(0, 0, 1), M_PI / 2.0);
+        break;
+      case kLandscapeRight:
+        sensor_to_display = Rotation::FromAxisAndAngle(Vector3(0, 0, 1), -M_PI / 2.0);
+        break;
+      default: // Portrait and PortraitUpsideDown
+        sensor_to_display = Rotation::FromAxisAndAngle(Vector3(0, 0, 1), 0.);
+        break;
+    }
+
+  const RotationState rotation_state = sensor_fusion_->GetLatestRotationState();
+  const Rotation unpredicted_rotation = rotation_state.sensor_from_start_rotation;
+  const Rotation predicted_rotation = sensor_fusion_->PredictRotation(timestamp_ns);
+
+  const Rotation adjusted_unpredicted_rotation = (sensor_to_display * unpredicted_rotation *
+                                                  ekf_to_head_tracker_);
+
+  const Rotation adjusted_rotation = (sensor_to_display * predicted_rotation *
+                                      ekf_to_head_tracker_);
+
+  // Save rotation sample with timestamp to be used in AddSixDoFData()
+  rotation_data_->AddSample(adjusted_unpredicted_rotation.GetQuaternion(), rotation_state.timestamp);
+
+  if (position_data_->IsValid() && rotation_state.timestamp - position_data_->GetLatestTimestamp() < kMaxSixDoFTimeDifference) {
+
+    // 6DoF is recently updated
+    const Vector4 orientation = (adjusted_rotation * smooth_ekf_to_sixDoF_).GetQuaternion();
+
+    out_orientation[0] = static_cast<float>(orientation[0]);
+    out_orientation[1] = static_cast<float>(orientation[1]);
+    out_orientation[2] = static_cast<float>(orientation[2]);
+    out_orientation[3] = static_cast<float>(orientation[3]);
+
+    Vector3 p = position_data_->GetExtrapolatedForTimeStamp(timestamp_ns);
+    out_position = {(float)p[0], (float)p[1], (float)p[2]};
+  } else {
+    // 6DoF is not recently updated
+    const Vector4 orientation = adjusted_rotation.GetQuaternion();
+
+    out_orientation[0] = static_cast<float>(orientation[0]);
+    out_orientation[1] = static_cast<float>(orientation[1]);
+    out_orientation[2] = static_cast<float>(orientation[2]);
+    out_orientation[3] = static_cast<float>(orientation[3]);
+
+    out_position = ApplyNeckModel(out_orientation, 1.0);
+    if (position_data_->IsValid()) {
+        // Apply last known 6DoF position if 6DoF data was 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];
+    }
+  }
 }
 
 void HeadTracker::RegisterCallbacks() {
@@ -114,4 +178,63 @@ void HeadTracker::OnGyroscopeData(const GyroscopeData& event) {
   sensor_fusion_->ProcessGyroscopeSample(event);
 }
 
+Rotation ShortestRotation(Rotation a, Rotation b) {
+
+    Vector4 aQ = a.GetQuaternion();
+    Vector4 bQ = b.GetQuaternion();
+
+    if (Dot(aQ, bQ) < 0) {
+        return -a * Rotation::FromQuaternion(-bQ);
+    } else {
+        return -a * b;
+    }
+}
+
+// Aryzon 6DoF
+void HeadTracker::AddSixDoFData(int64_t timestamp_ns, float* pos, float* orientation) {
+  if (!is_tracking_) {
+    return;
+  }
+    if (position_data_->GetLatestTimestamp() != timestamp_ns) {
+        position_data_->AddSample(Vector3(pos[0], pos[1], pos[2]), timestamp_ns);
+    }
+
+    // There will be a difference in rotation between ekf and sixDoF.
+    // SixDoF sensor is the 'truth' but is slower then ekf
+    // When the device is steady the difference between rotatations is saved
+    // smooth_ekf_to_sixDoF is slowly adjusted to smoothly close the gap
+    // between ekf and sixDoF. This value is used in GetPose().
+
+    if (position_data_->IsValid() && rotation_data_->IsValid()) {
+        if ((steady_frames_ == 30 || steady_frames_ < 0) && rotation_data_->GetLatestTimeStamp() > timestamp_ns) {
+            // Match rotation timestamps of ekf to sixDoF by interpolating the saved ekf rotations
+            // 6DoF timestamp should be before the latest rotation_data timestamp otherwise extrapolation
+            // needs to happen which will be less accurate.
+            const Rotation ekf_at_time_of_sixDoF = Rotation::FromQuaternion(rotation_data_->GetInterpolatedForTimeStamp(timestamp_ns));
+            const Rotation six_DoF_rotation = Rotation::FromQuaternion(Vector4(orientation[0], orientation[1], orientation[2], orientation[3]));
+
+            ekf_to_sixDoF_ = ShortestRotation(ekf_at_time_of_sixDoF, six_DoF_rotation);
+
+        } else if (steady_frames_ == 0) {
+            steady_start_ = Rotation::FromQuaternion(rotation_data_->GetLatestData());
+        }
+
+        const Rotation steady_difference = steady_start_ * -Rotation::FromQuaternion(rotation_data_->GetLatestData());
+
+        if (steady_difference.GetQuaternion()[3] > 0.9995) {
+            steady_frames_ += 1;
+        } else {
+            steady_frames_ = 0;
+        }
+
+        const Rotation bias_to_fill =  ShortestRotation(smooth_ekf_to_sixDoF_, ekf_to_sixDoF_);
+        Vector3 axis;
+        double angle;
+        bias_to_fill.GetAxisAndAngle(&axis, &angle);
+
+        const Rotation add_to_bias = Rotation::FromAxisAndAngle(axis, angle * kReduceBiasRate);
+
+        smooth_ekf_to_sixDoF_ *= add_to_bias;
+    }
+}
 }  // 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.
@@ -81,9 +94,17 @@ class HeadTracker {
   // Callback functions registered to the input SingleTypeEventProducer.
   std::function<void(AccelerometerData)> on_accel_callback_;
   std::function<void(GyroscopeData)> on_gyro_callback_;
-
-  static const Rotation kEkfToHeadTrackerRotation;
-  static const Rotation kSensorToDisplayRotation;
+
+  // Aryzon 6DoF
+  RotationData *rotation_data_;
+  PositionData *position_data_;
+
+  Rotation ekf_to_sixDoF_;
+  Rotation smooth_ekf_to_sixDoF_;
+  Rotation ekf_to_head_tracker_;
+
+  float steady_frames_;
+  Rotation steady_start_;
 };
 
 }  // namespace cardboard