Donate to e Foundation | Murena handsets with /e/OS | Own a part of Murena! Learn more

Commit c7e8a483 authored by Ytai Ben-Tsvi's avatar Ytai Ben-Tsvi Committed by Ytai Ben-tsvi
Browse files

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
parent 44e7c3db

media/libheadtracking/HeadTrackingProcessor.cpp

+23 −6
Original line number Diff line number Diff line
@@ -46,6 +46,11 @@ class HeadTrackingProcessorImpl : public HeadTrackingProcessor { 
                  .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 @@ class HeadTrackingProcessorImpl : public HeadTrackingProcessor { 
    }



    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 @@ class HeadTrackingProcessorImpl : public HeadTrackingProcessor { 
            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 @@ class HeadTrackingProcessorImpl : public HeadTrackingProcessor { 
        }

        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 @@ class HeadTrackingProcessorImpl : public HeadTrackingProcessor { 
    PoseDriftCompensator mHeadPoseDriftCompensator;

    PoseDriftCompensator mScreenPoseDriftCompensator;

    StillnessDetector mHeadStillnessDetector;

    StillnessDetector mScreenStillnessDetector;

    ScreenHeadFusion mScreenHeadFusion;

    ModeSelector mModeSelector;

    PoseRateLimiter mRateLimiter;

media/libheadtracking/ModeSelector-test.cpp

+36 −4
Original line number Diff line number Diff line
@@ -44,6 +44,7 @@ TEST(ModeSelector, InitialWorldRelative) { 
    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,15 +70,31 @@ TEST(ModeSelector, WorldRelative) { 
    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, WorldRelativeStale) {

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());
@@ -85,7 +102,22 @@ TEST(ModeSelector, WorldRelativeStale) { 
    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());



    ModeSelector::Options options{.freshnessTimeout = 100};

    ModeSelector selector(options);



    selector.setScreenToStagePose(screenToStage);

    selector.setDesiredMode(HeadTrackingMode::WORLD_RELATIVE);

    selector.setWorldToHeadPose(0, worldToHead);

    selector.calculate(101);
@@ -101,7 +133,6 @@ TEST(ModeSelector, ScreenRelative) { 
    ModeSelector selector(options);



    selector.setScreenToStagePose(screenToStage);



    selector.setDesiredMode(HeadTrackingMode::SCREEN_RELATIVE);

    selector.setScreenToHeadPose(0, screenToHead);

    selector.calculate(0);
@@ -118,10 +149,10 @@ TEST(ModeSelector, ScreenRelativeStaleToWorldRelative) { 
    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 @@ TEST(ModeSelector, ScreenRelativeInvalidToWorldRelative) { 
    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);

media/libheadtracking/ModeSelector.cpp

+8 −1
Original line number Diff line number Diff line
@@ -41,11 +41,18 @@ void ModeSelector::setWorldToHeadPose(int64_t timestamp, const Pose3f& worldToHe 
    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 @@ void ModeSelector::calculateActualMode(int64_t timestamp) { 


    // Optional downgrade from world-relative to static.

    if (mode == HeadTrackingMode::WORLD_RELATIVE) {

        if (!isValidWorldToHead) {

        if (!isValidWorldToHead || !isValidScreenStable || !mScreenStable.value()) {

            mode = HeadTrackingMode::STATIC;

        }

    }

media/libheadtracking/ModeSelector.h

+9 −0
Original line number Diff line number Diff line
@@ -56,6 +56,7 @@ namespace media { 
 *   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.
@@ -91,6 +92,12 @@ class ModeSelector { 
     */

    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.

     */
@@ -116,6 +123,8 @@ class ModeSelector { 
    int64_t mScreenToHeadTimestamp;

    std::optional<Pose3f> mWorldToHead;

    int64_t mWorldToHeadTimestamp;

    std::optional<bool> mScreenStable;

    int64_t mScreenStableTimestamp;



    HeadTrackingMode mActualMode;

    Pose3f mHeadToStage;

media/libheadtracking/include/media/HeadTrackingProcessor.h

+3 −0
Original line number Diff line number Diff line
@@ -45,6 +45,9 @@ class HeadTrackingProcessor { 
        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. */