Revert "Revert "Air Mouse""

This reverts commit bbc0888b45.

applications/plugins/airmouse/tracking/orientation_tracker.cc

@@ -0,0 +1,95 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include "orientation_tracker.h"
+
+#include "sensors/pose_prediction.h"
+#include "util/logging.h"
+#include "util/vector.h"
+#include "util/vectorutils.h"
+
+namespace cardboard {
+
+OrientationTracker::OrientationTracker(const long sampling_period_ns)
+    : sampling_period_ns_(sampling_period_ns)
+    , calibration_(Vector3::Zero())
+    , is_tracking_(false)
+    , sensor_fusion_(new SensorFusionEkf())
+    , latest_gyroscope_data_({ 0, 0, Vector3::Zero() })
+{
+    sensor_fusion_->SetBiasEstimationEnabled(/*kGyroBiasEstimationEnabled*/ true);
+}
+
+void OrientationTracker::SetCalibration(const Vector3& calibration) {
+    calibration_ = calibration;
+}
+
+void OrientationTracker::Pause()
+{
+    if (!is_tracking_) {
+        return;
+    }
+
+    // Create a gyro event with zero velocity. This effectively stops the prediction.
+    GyroscopeData event = latest_gyroscope_data_;
+    event.data = Vector3::Zero();
+
+    OnGyroscopeData(event);
+
+    is_tracking_ = false;
+}
+
+void OrientationTracker::Resume() { is_tracking_ = true; }
+
+Vector4 OrientationTracker::GetPose(int64_t timestamp_ns) const
+{
+    Rotation predicted_rotation;
+    const PoseState pose_state = sensor_fusion_->GetLatestPoseState();
+    if (sensor_fusion_->IsFullyInitialized()) {
+        predicted_rotation = pose_state.sensor_from_start_rotation;
+    } else {
+        CARDBOARD_LOGI("Tracker not fully initialized yet. Using pose prediction only.");
+        predicted_rotation = pose_prediction::PredictPose(timestamp_ns, pose_state);
+    }
+
+    return (-predicted_rotation).GetQuaternion();
+}
+
+void OrientationTracker::OnAccelerometerData(const AccelerometerData& event)
+{
+    if (!is_tracking_) {
+        return;
+    }
+    sensor_fusion_->ProcessAccelerometerSample(event);
+}
+
+Vector4 OrientationTracker::OnGyroscopeData(const GyroscopeData& event)
+{
+    if (!is_tracking_) {
+        return Vector4();
+    }
+
+    const GyroscopeData data = { .system_timestamp = event.system_timestamp,
+        .sensor_timestamp_ns = event.sensor_timestamp_ns,
+        .data = event.data - calibration_ };
+
+    latest_gyroscope_data_ = data;
+
+    sensor_fusion_->ProcessGyroscopeSample(data);
+
+    return OrientationTracker::GetPose(data.sensor_timestamp_ns + sampling_period_ns_);
+}
+
+} // namespace cardboard