Switch to static mode and recenter whenever screen moves
This logic intends to solve issues related to using head-tracking
while moving (e.g. being in a moving vehicle of walking).
We do so by switching to static mode and recentering whenever we
detect a significant motion of the screen.
Test: Manual verification via the SpatialAudioDemo app.
Test: atest --host libheadtracking-test
Change-Id: Iae5090c5a315d31ff89ada8d8a13694ea68ccf8e
diff --git a/media/libheadtracking/HeadTrackingProcessor.cpp b/media/libheadtracking/HeadTrackingProcessor.cpp
index dd2244a..257ee42 100644
--- a/media/libheadtracking/HeadTrackingProcessor.cpp
+++ b/media/libheadtracking/HeadTrackingProcessor.cpp
@@ -46,6 +46,11 @@
.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{
@@ -83,6 +88,22 @@
}
void calculate(int64_t timestamp) override {
+ // 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);
@@ -96,12 +117,6 @@
mModeSelector.setWorldToHeadPose(mWorldToHeadTimestamp.value(), worldToHead);
}
- if (mWorldToScreenTimestamp.has_value()) {
- const Pose3f worldToLogicalScreen = mScreenPoseDriftCompensator.getOutput();
- mScreenHeadFusion.setWorldToScreenPose(mWorldToScreenTimestamp.value(),
- worldToLogicalScreen);
- }
-
auto maybeScreenToHead = mScreenHeadFusion.calculate();
if (maybeScreenToHead.has_value()) {
mModeSelector.setScreenToHeadPose(maybeScreenToHead->timestamp,
@@ -131,6 +146,7 @@
}
if (recenterScreen) {
mScreenPoseDriftCompensator.recenter();
+ mScreenStillnessDetector.reset();
}
// If a sensor being recentered is included in the current mode, apply rate limiting to
@@ -155,6 +171,7 @@
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/include/media/HeadTrackingProcessor.h b/media/libheadtracking/include/media/HeadTrackingProcessor.h
index c90b57c..2af560e 100644
--- a/media/libheadtracking/include/media/HeadTrackingProcessor.h
+++ b/media/libheadtracking/include/media/HeadTrackingProcessor.h
@@ -45,6 +45,9 @@
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 04b0a4a..80a3d29 100644
--- a/services/audiopolicy/service/SpatializerPoseController.cpp
+++ b/services/audiopolicy/service/SpatializerPoseController.cpp
@@ -52,7 +52,7 @@
// 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 = 40s;
+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
@@ -73,6 +73,16 @@
// 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;
@@ -90,13 +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] {