Auto-recenter head

This logic detects when the head has been approximately still for a
certain amount of time and triggers a head-recenter.

Documentation changes to follow.

Test: Integrated this code into the Spatial Audio Demo app and
      manually verified.
Test: Ran the included units tests.
Change-Id: I22b9e590aa1ca725d43b78fc58ade1144f2e4e52
diff --git a/media/libheadtracking/Android.bp b/media/libheadtracking/Android.bp
index 63b769e..b0563e2 100644
--- a/media/libheadtracking/Android.bp
+++ b/media/libheadtracking/Android.bp
@@ -18,6 +18,7 @@
       "PoseRateLimiter.cpp",
       "QuaternionUtil.cpp",
       "ScreenHeadFusion.cpp",
+      "StillnessDetector.cpp",
       "Twist.cpp",
     ],
     export_include_dirs: [
@@ -70,6 +71,7 @@
         "PoseRateLimiter-test.cpp",
         "QuaternionUtil-test.cpp",
         "ScreenHeadFusion-test.cpp",
+        "StillnessDetector-test.cpp",
         "Twist-test.cpp",
     ],
     shared_libs: [
diff --git a/media/libheadtracking/HeadTrackingProcessor.cpp b/media/libheadtracking/HeadTrackingProcessor.cpp
index 47f7cf0..dd2244a 100644
--- a/media/libheadtracking/HeadTrackingProcessor.cpp
+++ b/media/libheadtracking/HeadTrackingProcessor.cpp
@@ -20,6 +20,7 @@
 #include "PoseDriftCompensator.h"
 #include "QuaternionUtil.h"
 #include "ScreenHeadFusion.h"
+#include "StillnessDetector.h"
 
 namespace android {
 namespace media {
@@ -40,6 +41,11 @@
                   .translationalDriftTimeConstant = options.translationalDriftTimeConstant,
                   .rotationalDriftTimeConstant = options.rotationalDriftTimeConstant,
           }),
+          mHeadStillnessDetector(StillnessDetector::Options{
+                  .windowDuration = options.autoRecenterWindowDuration,
+                  .translationalThreshold = options.autoRecenterTranslationalThreshold,
+                  .rotationalThreshold = options.autoRecenterRotationalThreshold,
+          }),
           mModeSelector(ModeSelector::Options{.freshnessTimeout = options.freshnessTimeout},
                         initialMode),
           mRateLimiter(PoseRateLimiter::Options{
@@ -78,7 +84,14 @@
 
     void calculate(int64_t timestamp) override {
         if (mWorldToHeadTimestamp.has_value()) {
-            const Pose3f worldToHead = mHeadPoseDriftCompensator.getOutput();
+            Pose3f worldToHead = mHeadPoseDriftCompensator.getOutput();
+            mHeadStillnessDetector.setInput(mWorldToHeadTimestamp.value(), worldToHead);
+            // Auto-recenter.
+            if (mHeadStillnessDetector.calculate(timestamp)) {
+                recenter(true, false);
+                worldToHead = mHeadPoseDriftCompensator.getOutput();
+            }
+
             mScreenHeadFusion.setWorldToHeadPose(mWorldToHeadTimestamp.value(), worldToHead);
             mModeSelector.setWorldToHeadPose(mWorldToHeadTimestamp.value(), worldToHead);
         }
@@ -114,6 +127,7 @@
     void recenter(bool recenterHead, bool recenterScreen) override {
         if (recenterHead) {
             mHeadPoseDriftCompensator.recenter();
+            mHeadStillnessDetector.reset();
         }
         if (recenterScreen) {
             mScreenPoseDriftCompensator.recenter();
@@ -140,6 +154,7 @@
     Pose3f mHeadToStagePose;
     PoseDriftCompensator mHeadPoseDriftCompensator;
     PoseDriftCompensator mScreenPoseDriftCompensator;
+    StillnessDetector mHeadStillnessDetector;
     ScreenHeadFusion mScreenHeadFusion;
     ModeSelector mModeSelector;
     PoseRateLimiter mRateLimiter;
diff --git a/media/libheadtracking/StillnessDetector-test.cpp b/media/libheadtracking/StillnessDetector-test.cpp
new file mode 100644
index 0000000..a53ba8c
--- /dev/null
+++ b/media/libheadtracking/StillnessDetector-test.cpp
@@ -0,0 +1,128 @@
+/*
+ * Copyright (C) 2021 The Android Open Source Project
+ *
+ * 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 <gtest/gtest.h>
+
+#include "QuaternionUtil.h"
+#include "StillnessDetector.h"
+#include "TestUtil.h"
+
+namespace android {
+namespace media {
+namespace {
+
+using Eigen::Quaternionf;
+using Eigen::Vector3f;
+using Options = StillnessDetector::Options;
+
+TEST(StillnessDetectorTest, Still) {
+    StillnessDetector detector(Options{
+            .windowDuration = 1000, .translationalThreshold = 1, .rotationalThreshold = 0.05});
+
+    const Pose3f baseline(Vector3f{1, 2, 3}, Quaternionf::UnitRandom());
+    const Pose3f withinThreshold =
+            baseline * Pose3f(Vector3f(0.3, -0.3, 0), rotateX(0.01) * rotateY(-0.01));
+
+    EXPECT_FALSE(detector.calculate(0));
+    detector.setInput(0, baseline);
+    EXPECT_FALSE(detector.calculate(0));
+    detector.setInput(300, withinThreshold);
+    EXPECT_FALSE(detector.calculate(300));
+    detector.setInput(600, baseline);
+    EXPECT_FALSE(detector.calculate(600));
+    detector.setInput(999, withinThreshold);
+    EXPECT_FALSE(detector.calculate(999));
+    detector.setInput(1000, baseline);
+    EXPECT_TRUE(detector.calculate(1000));
+}
+
+TEST(StillnessDetectorTest, ZeroDuration) {
+    StillnessDetector detector(Options{.windowDuration = 0});
+    EXPECT_TRUE(detector.calculate(0));
+    EXPECT_TRUE(detector.calculate(1000));
+}
+
+TEST(StillnessDetectorTest, NotStillTranslation) {
+    StillnessDetector detector(Options{
+            .windowDuration = 1000, .translationalThreshold = 1, .rotationalThreshold = 0.05});
+
+    const Pose3f baseline(Vector3f{1, 2, 3}, Quaternionf::UnitRandom());
+    const Pose3f withinThreshold =
+            baseline * Pose3f(Vector3f(0.3, -0.3, 0), rotateX(0.01) * rotateY(-0.01));
+    const Pose3f outsideThreshold = baseline * Pose3f(Vector3f(1, 1, 0));
+
+    EXPECT_FALSE(detector.calculate(0));
+    detector.setInput(0, baseline);
+    EXPECT_FALSE(detector.calculate(0));
+    detector.setInput(300, outsideThreshold);
+    EXPECT_FALSE(detector.calculate(300));
+    detector.setInput(600, baseline);
+    EXPECT_FALSE(detector.calculate(600));
+    detector.setInput(900, withinThreshold);
+    EXPECT_FALSE(detector.calculate(900));
+    detector.setInput(1299, baseline);
+    EXPECT_FALSE(detector.calculate(1299));
+    EXPECT_TRUE(detector.calculate(1300));
+}
+
+TEST(StillnessDetectorTest, NotStillRotation) {
+    StillnessDetector detector(Options{
+            .windowDuration = 1000, .translationalThreshold = 1, .rotationalThreshold = 0.05});
+
+    const Pose3f baseline(Vector3f{1, 2, 3}, Quaternionf::UnitRandom());
+    const Pose3f withinThreshold =
+            baseline * Pose3f(Vector3f(0.3, -0.3, 0), rotateX(0.01) * rotateY(-0.01));
+    const Pose3f outsideThreshold = baseline * Pose3f(rotateZ(0.08));
+    EXPECT_FALSE(detector.calculate(0));
+    detector.setInput(0, baseline);
+    EXPECT_FALSE(detector.calculate(0));
+    detector.setInput(300, outsideThreshold);
+    EXPECT_FALSE(detector.calculate(300));
+    detector.setInput(600, baseline);
+    EXPECT_FALSE(detector.calculate(600));
+    detector.setInput(900, withinThreshold);
+    EXPECT_FALSE(detector.calculate(900));
+    detector.setInput(1299, baseline);
+    EXPECT_FALSE(detector.calculate(1299));
+    EXPECT_TRUE(detector.calculate(1300));
+}
+
+TEST(StillnessDetectorTest, Reset) {
+    StillnessDetector detector(Options{
+            .windowDuration = 1000, .translationalThreshold = 1, .rotationalThreshold = 0.05});
+
+    const Pose3f baseline(Vector3f{1, 2, 3}, Quaternionf::UnitRandom());
+    const Pose3f withinThreshold =
+            baseline * Pose3f(Vector3f(0.3, -0.3, 0), rotateX(0.01) * rotateY(-0.01));
+    EXPECT_FALSE(detector.calculate(0));
+    detector.setInput(0, baseline);
+    EXPECT_FALSE(detector.calculate(0));
+    detector.reset();
+    detector.setInput(600, baseline);
+    EXPECT_FALSE(detector.calculate(600));
+    detector.setInput(900, withinThreshold);
+    EXPECT_FALSE(detector.calculate(900));
+    detector.setInput(1200, baseline);
+    EXPECT_FALSE(detector.calculate(1200));
+    detector.setInput(1599, withinThreshold);
+    EXPECT_FALSE(detector.calculate(1599));
+    detector.setInput(1600, baseline);
+    EXPECT_TRUE(detector.calculate(1600));
+}
+
+}  // namespace
+}  // namespace media
+}  // namespace android
diff --git a/media/libheadtracking/StillnessDetector.cpp b/media/libheadtracking/StillnessDetector.cpp
new file mode 100644
index 0000000..832351d
--- /dev/null
+++ b/media/libheadtracking/StillnessDetector.cpp
@@ -0,0 +1,98 @@
+/*
+ * Copyright (C) 2021 The Android Open Source Project
+ *
+ * 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 "StillnessDetector.h"
+
+namespace android {
+namespace media {
+
+StillnessDetector::StillnessDetector(const Options& options) : mOptions(options) {}
+
+void StillnessDetector::reset() {
+    mFifo.clear();
+    mWindowFull = false;
+}
+
+void StillnessDetector::setInput(int64_t timestamp, const Pose3f& input) {
+    mFifo.push_back(TimestampedPose{timestamp, input});
+    discardOld(timestamp);
+}
+
+bool StillnessDetector::calculate(int64_t timestamp) {
+    discardOld(timestamp);
+
+    // If the window has not been full, we don't consider ourselves still.
+    if (!mWindowFull) {
+        return false;
+    }
+
+    // An empty FIFO and window full is considered still (this will happen in the unlikely case when
+    // the window duration is shorter than the gap between samples).
+    if (mFifo.empty()) {
+        return true;
+    }
+
+    // Otherwise, check whether all the poses remaining in the queue are in the proximity of the new
+    // one.
+    for (auto iter = mFifo.begin(); iter != mFifo.end() - 1; ++iter) {
+        const auto& event = *iter;
+        if (!areNear(event.pose, mFifo.back().pose)) {
+            return false;
+        }
+    }
+
+    return true;
+}
+
+void StillnessDetector::discardOld(int64_t timestamp) {
+    // Handle the special case of the window duration being zero (always considered full).
+    if (mOptions.windowDuration == 0) {
+        mFifo.clear();
+        mWindowFull = true;
+    }
+
+    // Remove any events from the queue that are older than the window. If there were any such
+    // events we consider the window full.
+    const int64_t windowStart = timestamp - mOptions.windowDuration;
+    while (!mFifo.empty() && mFifo.front().timestamp <= windowStart) {
+        mWindowFull = true;
+        mFifo.pop_front();
+    }
+}
+
+bool StillnessDetector::areNear(const Pose3f& pose1, const Pose3f& pose2) const {
+    // Check translation. We use the L1 norm to reduce computational load on expense of accuracy.
+    // The L1 norm is an upper bound for the actual (L2) norm, so this approach will err on the side
+    // of "not near".
+    if ((pose1.translation() - pose2.translation()).lpNorm<1>() >=
+        mOptions.translationalThreshold) {
+        return false;
+    }
+
+    // Check orientation. We use the L1 norm of the imaginary components of the quaternion to reduce
+    // computational load on expense of accuracy. For small angles, those components are approx.
+    // equal to the angle of rotation and so the norm is approx. the total angle of rotation. The
+    // L1 norm is an upper bound, so this approach will err on the side of "not near".
+    if ((pose1.rotation().vec() - pose2.rotation().vec()).lpNorm<1>() >=
+        mOptions.rotationalThreshold) {
+        return false;
+    }
+
+    return true;
+}
+
+}  // namespace media
+}  // namespace android
diff --git a/media/libheadtracking/StillnessDetector.h b/media/libheadtracking/StillnessDetector.h
new file mode 100644
index 0000000..fd26aa9
--- /dev/null
+++ b/media/libheadtracking/StillnessDetector.h
@@ -0,0 +1,94 @@
+/*
+ * Copyright (C) 2021 The Android Open Source Project
+ *
+ * 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.
+ */
+#pragma once
+
+#include <deque>
+
+#include <media/Pose.h>
+
+namespace android {
+namespace media {
+
+/**
+ * Given a stream of poses, determines if the pose is stable ("still").
+ * Stillness is defined as all poses in the recent history ("window") being near the most recent
+ * sample.
+ *
+ * Typical usage:
+ *
+ * StillnessDetector detector(StilnessDetector::Options{...});
+ *
+ * while (...) {
+ *    detector.setInput(timestamp, pose);
+ *    bool still = detector.calculate(timestamp);
+ * }
+ *
+ * The stream is considered not still until a sufficient number of samples has been provided for an
+ * initial fill-up of the window. In the special case of the window size being 0, this is not
+ * required and the state is considered always "still". The reset() method can be used to empty the
+ * window again and get back to this initial state.
+ */
+class StillnessDetector {
+  public:
+    /**
+     * Configuration options for the detector.
+     */
+    struct Options {
+        /**
+         * How long is the window, in ticks. The special value of 0 indicates that the stream is
+         * always considered still.
+         */
+        int64_t windowDuration;
+        /**
+         * How much of a translational deviation from the target (in meters) is considered motion.
+         * This is an approximate quantity - the actual threshold might be a little different as we
+         * trade-off accuracy with computational efficiency.
+         */
+        float translationalThreshold;
+        /**
+         * How much of a rotational deviation from the target (in radians) is considered motion.
+         * This is an approximate quantity - the actual threshold might be a little different as we
+         * trade-off accuracy with computational efficiency.
+         */
+        float rotationalThreshold;
+    };
+
+    /** Ctor. */
+    explicit StillnessDetector(const Options& options);
+
+    /** Clear the window. */
+    void reset();
+    /** Push a new sample. */
+    void setInput(int64_t timestamp, const Pose3f& input);
+    /** Calculate whether the stream is still at the given timestamp. */
+    bool calculate(int64_t timestamp);
+
+  private:
+    struct TimestampedPose {
+        int64_t timestamp;
+        Pose3f pose;
+    };
+
+    const Options mOptions;
+    std::deque<TimestampedPose> mFifo;
+    bool mWindowFull = false;
+
+    bool areNear(const Pose3f& pose1, const Pose3f& pose2) const;
+    void discardOld(int64_t timestamp);
+};
+
+}  // namespace media
+}  // namespace android
diff --git a/media/libheadtracking/include/media/HeadTrackingProcessor.h b/media/libheadtracking/include/media/HeadTrackingProcessor.h
index 9fea273..c90b57c 100644
--- a/media/libheadtracking/include/media/HeadTrackingProcessor.h
+++ b/media/libheadtracking/include/media/HeadTrackingProcessor.h
@@ -42,6 +42,9 @@
         float rotationalDriftTimeConstant = std::numeric_limits<float>::infinity();
         int64_t freshnessTimeout = std::numeric_limits<int64_t>::max();
         float predictionDuration = 0;
+        int64_t autoRecenterWindowDuration = std::numeric_limits<int64_t>::max();
+        float autoRecenterTranslationalThreshold = std::numeric_limits<float>::infinity();
+        float autoRecenterRotationalThreshold = std::numeric_limits<float>::infinity();
     };
 
     /** Sets the desired head-tracking mode. */