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

Commit a1ad269b authored by Ytai Ben-tsvi's avatar Ytai Ben-tsvi Committed by Android (Google) Code Review
Browse files

Merge changes Iae5090c5,I22b9e590 

* changes:
  Switch to static mode and recenter whenever screen moves
  Auto-recenter head
parents eecdcb5e c7e8a483

media/libheadtracking/Android.bp

+2 −0
Original line number Diff line number Diff line
@@ -18,6 +18,7 @@ cc_library { 
      "PoseRateLimiter.cpp",

      "QuaternionUtil.cpp",

      "ScreenHeadFusion.cpp",

      "StillnessDetector.cpp",

      "Twist.cpp",

    ],

    export_include_dirs: [
@@ -70,6 +71,7 @@ cc_test_host { 
        "PoseRateLimiter-test.cpp",

        "QuaternionUtil-test.cpp",

        "ScreenHeadFusion-test.cpp",

        "StillnessDetector-test.cpp",

        "Twist-test.cpp",

    ],

    shared_libs: [

media/libheadtracking/HeadTrackingProcessor.cpp

+38 −6
Original line number Diff line number Diff line
@@ -20,6 +20,7 @@ 
#include "PoseDriftCompensator.h"

#include "QuaternionUtil.h"

#include "ScreenHeadFusion.h"

#include "StillnessDetector.h"



namespace android {

namespace media {
@@ -40,6 +41,16 @@ class HeadTrackingProcessorImpl : public HeadTrackingProcessor { 
                  .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 @@ class HeadTrackingProcessorImpl : public HeadTrackingProcessor { 
    }



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

    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;