Merge changes Iae5090c5,I22b9e590
* changes:
Switch to static mode and recenter whenever screen moves
Auto-recenter head
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..257ee42 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,16 @@
.translationalDriftTimeConstant = options.translationalDriftTimeConstant,
.rotationalDriftTimeConstant = options.rotationalDriftTimeConstant,
}),
+ mHeadStillnessDetector(StillnessDetector::Options{
+ .windowDuration = options.autoRecenterWindowDuration,
+ .translationalThreshold = options.autoRecenterTranslationalThreshold,
+ .rotationalThreshold = options.autoRecenterRotationalThreshold,
+ }),
+ mScreenStillnessDetector(StillnessDetector::Options{
+ .windowDuration = options.screenStillnessWindowDuration,
+ .translationalThreshold = options.screenStillnessTranslationalThreshold,
+ .rotationalThreshold = options.screenStillnessRotationalThreshold,
+ }),
mModeSelector(ModeSelector::Options{.freshnessTimeout = options.freshnessTimeout},
initialMode),
mRateLimiter(PoseRateLimiter::Options{
@@ -77,18 +88,35 @@
}
void calculate(int64_t timestamp) override {
- if (mWorldToHeadTimestamp.has_value()) {
- const Pose3f worldToHead = mHeadPoseDriftCompensator.getOutput();
- mScreenHeadFusion.setWorldToHeadPose(mWorldToHeadTimestamp.value(), worldToHead);
- mModeSelector.setWorldToHeadPose(mWorldToHeadTimestamp.value(), worldToHead);
- }
-
+ // Handle the screen first, since it might trigger a recentering of the head.
if (mWorldToScreenTimestamp.has_value()) {
const Pose3f worldToLogicalScreen = mScreenPoseDriftCompensator.getOutput();
+ mScreenStillnessDetector.setInput(mWorldToScreenTimestamp.value(),
+ worldToLogicalScreen);
+ bool screenStable = mScreenStillnessDetector.calculate(timestamp);
+ mModeSelector.setScreenStable(mWorldToScreenTimestamp.value(), screenStable);
+ // Whenever the screen is unstable, recenter the head pose.
+ if (!screenStable) {
+ recenter(true, false);
+ }
mScreenHeadFusion.setWorldToScreenPose(mWorldToScreenTimestamp.value(),
worldToLogicalScreen);
}
+ // Handle head.
+ if (mWorldToHeadTimestamp.has_value()) {
+ 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);
+ }
+
auto maybeScreenToHead = mScreenHeadFusion.calculate();
if (maybeScreenToHead.has_value()) {
mModeSelector.setScreenToHeadPose(maybeScreenToHead->timestamp,
@@ -114,9 +142,11 @@
void recenter(bool recenterHead, bool recenterScreen) override {
if (recenterHead) {
mHeadPoseDriftCompensator.recenter();
+ mHeadStillnessDetector.reset();
}
if (recenterScreen) {
mScreenPoseDriftCompensator.recenter();
+ mScreenStillnessDetector.reset();
}
// If a sensor being recentered is included in the current mode, apply rate limiting to
@@ -140,6 +170,8 @@
Pose3f mHeadToStagePose;
PoseDriftCompensator mHeadPoseDriftCompensator;
PoseDriftCompensator mScreenPoseDriftCompensator;
+ StillnessDetector mHeadStillnessDetector;
+ StillnessDetector mScreenStillnessDetector;
ScreenHeadFusion mScreenHeadFusion;
ModeSelector mModeSelector;
PoseRateLimiter mRateLimiter;
diff --git a/media/libheadtracking/ModeSelector-test.cpp b/media/libheadtracking/ModeSelector-test.cpp
index 6247d84..a136e6b 100644
--- a/media/libheadtracking/ModeSelector-test.cpp
+++ b/media/libheadtracking/ModeSelector-test.cpp
@@ -44,6 +44,7 @@
ModeSelector selector(options, HeadTrackingMode::WORLD_RELATIVE);
selector.setWorldToHeadPose(0, worldToHead);
+ selector.setScreenStable(0, true);
selector.calculate(0);
EXPECT_EQ(HeadTrackingMode::WORLD_RELATIVE, selector.getActualMode());
EXPECT_EQ(selector.getHeadToStagePose(), worldToHead.inverse());
@@ -69,14 +70,46 @@
ModeSelector selector(options);
selector.setScreenToStagePose(screenToStage);
-
selector.setDesiredMode(HeadTrackingMode::WORLD_RELATIVE);
selector.setWorldToHeadPose(0, worldToHead);
+ selector.setScreenStable(0, true);
selector.calculate(0);
EXPECT_EQ(HeadTrackingMode::WORLD_RELATIVE, selector.getActualMode());
EXPECT_EQ(selector.getHeadToStagePose(), worldToHead.inverse() * screenToStage);
}
+TEST(ModeSelector, WorldRelativeUnstable) {
+ const Pose3f worldToHead({1, 2, 3}, Quaternionf::UnitRandom());
+ const Pose3f screenToStage({4, 5, 6}, Quaternionf::UnitRandom());
+
+ ModeSelector::Options options{.freshnessTimeout = 100};
+ ModeSelector selector(options);
+
+ selector.setScreenToStagePose(screenToStage);
+ selector.setDesiredMode(HeadTrackingMode::WORLD_RELATIVE);
+ selector.setWorldToHeadPose(0, worldToHead);
+ selector.setScreenStable(0, false);
+ selector.calculate(10);
+ EXPECT_EQ(HeadTrackingMode::STATIC, selector.getActualMode());
+ EXPECT_EQ(selector.getHeadToStagePose(), screenToStage);
+}
+
+TEST(ModeSelector, WorldRelativeStableStale) {
+ const Pose3f worldToHead({1, 2, 3}, Quaternionf::UnitRandom());
+ const Pose3f screenToStage({4, 5, 6}, Quaternionf::UnitRandom());
+
+ ModeSelector::Options options{.freshnessTimeout = 100};
+ ModeSelector selector(options);
+
+ selector.setScreenToStagePose(screenToStage);
+ selector.setDesiredMode(HeadTrackingMode::WORLD_RELATIVE);
+ selector.setWorldToHeadPose(100, worldToHead);
+ selector.setScreenStable(0, true);
+ selector.calculate(101);
+ EXPECT_EQ(HeadTrackingMode::STATIC, selector.getActualMode());
+ EXPECT_EQ(selector.getHeadToStagePose(), screenToStage);
+}
+
TEST(ModeSelector, WorldRelativeStale) {
const Pose3f worldToHead({1, 2, 3}, Quaternionf::UnitRandom());
const Pose3f screenToStage({4, 5, 6}, Quaternionf::UnitRandom());
@@ -85,7 +118,6 @@
ModeSelector selector(options);
selector.setScreenToStagePose(screenToStage);
-
selector.setDesiredMode(HeadTrackingMode::WORLD_RELATIVE);
selector.setWorldToHeadPose(0, worldToHead);
selector.calculate(101);
@@ -101,7 +133,6 @@
ModeSelector selector(options);
selector.setScreenToStagePose(screenToStage);
-
selector.setDesiredMode(HeadTrackingMode::SCREEN_RELATIVE);
selector.setScreenToHeadPose(0, screenToHead);
selector.calculate(0);
@@ -118,10 +149,10 @@
ModeSelector selector(options);
selector.setScreenToStagePose(screenToStage);
-
selector.setDesiredMode(HeadTrackingMode::SCREEN_RELATIVE);
selector.setScreenToHeadPose(0, screenToHead);
selector.setWorldToHeadPose(50, worldToHead);
+ selector.setScreenStable(50, true);
selector.calculate(101);
EXPECT_EQ(HeadTrackingMode::WORLD_RELATIVE, selector.getActualMode());
EXPECT_EQ(selector.getHeadToStagePose(), worldToHead.inverse() * screenToStage);
@@ -139,6 +170,7 @@
selector.setDesiredMode(HeadTrackingMode::SCREEN_RELATIVE);
selector.setScreenToHeadPose(50, std::nullopt);
selector.setWorldToHeadPose(50, worldToHead);
+ selector.setScreenStable(50, true);
selector.calculate(101);
EXPECT_EQ(HeadTrackingMode::WORLD_RELATIVE, selector.getActualMode());
EXPECT_EQ(selector.getHeadToStagePose(), worldToHead.inverse() * screenToStage);
diff --git a/media/libheadtracking/ModeSelector.cpp b/media/libheadtracking/ModeSelector.cpp
index 16e1712..cb3a27f 100644
--- a/media/libheadtracking/ModeSelector.cpp
+++ b/media/libheadtracking/ModeSelector.cpp
@@ -41,11 +41,18 @@
mWorldToHeadTimestamp = timestamp;
}
+void ModeSelector::setScreenStable(int64_t timestamp, bool stable) {
+ mScreenStable = stable;
+ mScreenStableTimestamp = timestamp;
+}
+
void ModeSelector::calculateActualMode(int64_t timestamp) {
bool isValidScreenToHead = mScreenToHead.has_value() &&
timestamp - mScreenToHeadTimestamp < mOptions.freshnessTimeout;
bool isValidWorldToHead = mWorldToHead.has_value() &&
timestamp - mWorldToHeadTimestamp < mOptions.freshnessTimeout;
+ bool isValidScreenStable = mScreenStable.has_value() &&
+ timestamp - mScreenStableTimestamp < mOptions.freshnessTimeout;
HeadTrackingMode mode = mDesiredMode;
@@ -58,7 +65,7 @@
// Optional downgrade from world-relative to static.
if (mode == HeadTrackingMode::WORLD_RELATIVE) {
- if (!isValidWorldToHead) {
+ if (!isValidWorldToHead || !isValidScreenStable || !mScreenStable.value()) {
mode = HeadTrackingMode::STATIC;
}
}
diff --git a/media/libheadtracking/ModeSelector.h b/media/libheadtracking/ModeSelector.h
index 17a5142..e537040 100644
--- a/media/libheadtracking/ModeSelector.h
+++ b/media/libheadtracking/ModeSelector.h
@@ -56,6 +56,7 @@
* from screen-relative to world-relative.
* - When we cannot get a fresh estimate of the world-to-head pose, we will fall back from
* world-relative to static.
+ * - In world-relative mode, if the screen is unstable, we will fall back to static.
*
* All the timestamps used here are of arbitrary units and origin. They just need to be consistent
* between all the calls and with the Options provided for determining freshness and rate limiting.
@@ -92,6 +93,12 @@
void setWorldToHeadPose(int64_t timestamp, const Pose3f& worldToHead);
/**
+ * Set whether the screen is considered stable.
+ * The timestamp needs to reflect how fresh the sample is.
+ */
+ void setScreenStable(int64_t timestamp, bool stable);
+
+ /**
* Process all the previous inputs and update the outputs.
*/
void calculate(int64_t timestamp);
@@ -116,6 +123,8 @@
int64_t mScreenToHeadTimestamp;
std::optional<Pose3f> mWorldToHead;
int64_t mWorldToHeadTimestamp;
+ std::optional<bool> mScreenStable;
+ int64_t mScreenStableTimestamp;
HeadTrackingMode mActualMode;
Pose3f mHeadToStage;
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..2af560e 100644
--- a/media/libheadtracking/include/media/HeadTrackingProcessor.h
+++ b/media/libheadtracking/include/media/HeadTrackingProcessor.h
@@ -42,6 +42,12 @@
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();
+ int64_t screenStillnessWindowDuration = 0;
+ float screenStillnessTranslationalThreshold = std::numeric_limits<float>::infinity();
+ float screenStillnessRotationalThreshold = std::numeric_limits<float>::infinity();
};
/** Sets the desired head-tracking mode. */
diff --git a/services/audiopolicy/service/SpatializerPoseController.cpp b/services/audiopolicy/service/SpatializerPoseController.cpp
index ffedf63..80a3d29 100644
--- a/services/audiopolicy/service/SpatializerPoseController.cpp
+++ b/services/audiopolicy/service/SpatializerPoseController.cpp
@@ -45,14 +45,14 @@
// determine the time constants used for high-pass filtering those readings. If the value is set
// too high, we may experience drift. If it is set too low, we may experience poses tending toward
// identity too fast.
-constexpr auto kTranslationalDriftTimeConstant = 20s;
+constexpr auto kTranslationalDriftTimeConstant = 40s;
// This should be set to the typical time scale that the rotation sensors used drift in. This
// means, loosely, for how long we can trust the reading to be "accurate enough". This would
// determine the time constants used for high-pass filtering those readings. If the value is set
// too high, we may experience drift. If it is set too low, we may experience poses tending toward
// identity too fast.
-constexpr auto kRotationalDriftTimeConstant = 20s;
+constexpr auto kRotationalDriftTimeConstant = 60s;
// This is how far into the future we predict the head pose, using linear extrapolation based on
// twist (velocity). It should be set to a value that matches the characteristic durations of moving
@@ -64,6 +64,25 @@
// stale;
constexpr auto kMaxLostSamples = 4;
+// Auto-recenter kicks in after the head has been still for this long.
+constexpr auto kAutoRecenterWindowDuration = 10s;
+
+// Auto-recenter considers head not still if translated by this much (in meters, approx).
+constexpr float kAutoRecenterTranslationThreshold = 0.1f;
+
+// Auto-recenter considers head not still if rotated by this much (in radians, approx).
+constexpr float kAutoRecenterRotationThreshold = 5.0f / 180 * M_PI;
+
+// Screen is considered to be unstable (not still) if it has moved significantly within the last
+// time window of this duration.
+constexpr auto kScreenStillnessWindowDuration = 10s;
+
+// Screen is considered to have moved significantly if translated by this much (in meter, approx).
+constexpr float kScreenStillnessTranslationThreshold = 0.1f;
+
+// Screen is considered to have moved significantly if rotated by this much (in radians, approx).
+constexpr float kScreenStillnessRotationThreshold = 5.0f / 180 * M_PI;
+
// Time units for system clock ticks. This is what the Sensor Framework timestamps represent and
// what we use for pose filtering.
using Ticks = std::chrono::nanoseconds;
@@ -81,10 +100,17 @@
mProcessor(createHeadTrackingProcessor(HeadTrackingProcessor::Options{
.maxTranslationalVelocity = kMaxTranslationalVelocity / kTicksPerSecond,
.maxRotationalVelocity = kMaxRotationalVelocity / kTicksPerSecond,
- .translationalDriftTimeConstant = Ticks(kTranslationalDriftTimeConstant).count(),
- .rotationalDriftTimeConstant = Ticks(kRotationalDriftTimeConstant).count(),
+ .translationalDriftTimeConstant =
+ double(Ticks(kTranslationalDriftTimeConstant).count()),
+ .rotationalDriftTimeConstant = double(Ticks(kRotationalDriftTimeConstant).count()),
.freshnessTimeout = Ticks(sensorPeriod * kMaxLostSamples).count(),
.predictionDuration = Ticks(kPredictionDuration).count(),
+ .autoRecenterWindowDuration = Ticks(kAutoRecenterWindowDuration).count(),
+ .autoRecenterTranslationalThreshold = kAutoRecenterTranslationThreshold,
+ .autoRecenterRotationalThreshold = kAutoRecenterRotationThreshold,
+ .screenStillnessWindowDuration = Ticks(kScreenStillnessWindowDuration).count(),
+ .screenStillnessTranslationalThreshold = kScreenStillnessTranslationThreshold,
+ .screenStillnessRotationalThreshold = kScreenStillnessRotationThreshold,
})),
mPoseProvider(SensorPoseProvider::create("headtracker", this)),
mThread([this, maxUpdatePeriod] {