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. */