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

Commit f7663989 authored by Ytai Ben-Tsvi's avatar Ytai Ben-Tsvi
Browse files

Force "not still" for the duration of the window after motion 

Because of our motion detection heuristic, we have situations when the
state is considered in motion and then back to still before the window
duration elapses (this is because we always compare the samples in the
buffer to the last one). In order to avoid toggling back-and-forth
faster than the window duration, we add a suppression period after
detecting motion.

Test: atest --host libheadtracking-test
Change-Id: Ia85d5699bef1b3313d007193375df4892f3365ac
parent 4cb1e487

media/libheadtracking/StillnessDetector-test.cpp

+32 −6
Original line number Diff line number Diff line
@@ -85,9 +85,11 @@ TEST_P(StillnessDetectorTest, NotStillTranslation) { 
    EXPECT_EQ(mDefaultValue, detector.calculate(600));

    detector.setInput(900, withinThreshold);

    EXPECT_EQ(mDefaultValue, detector.calculate(900));

    detector.setInput(1299, baseline);

    EXPECT_FALSE(detector.calculate(1299));

    EXPECT_TRUE(detector.calculate(1300));

    detector.setInput(1300, baseline);

    EXPECT_FALSE(detector.calculate(1300));

    detector.setInput(1500, baseline);

    EXPECT_FALSE(detector.calculate(1899));

    EXPECT_TRUE(detector.calculate(1900));

}



TEST_P(StillnessDetectorTest, NotStillRotation) {
@@ -100,6 +102,7 @@ TEST_P(StillnessDetectorTest, NotStillRotation) { 
    const Pose3f withinThreshold =

            baseline * Pose3f(Vector3f(0.3, -0.3, 0), rotateX(0.03) * rotateY(-0.03));

    const Pose3f outsideThreshold = baseline * Pose3f(rotateZ(0.06));



    EXPECT_EQ(mDefaultValue, detector.calculate(0));

    detector.setInput(0, baseline);

    EXPECT_EQ(mDefaultValue, detector.calculate(0));
@@ -109,9 +112,32 @@ TEST_P(StillnessDetectorTest, NotStillRotation) { 
    EXPECT_EQ(mDefaultValue, detector.calculate(600));

    detector.setInput(900, withinThreshold);

    EXPECT_EQ(mDefaultValue, detector.calculate(900));

    detector.setInput(1299, baseline);

    EXPECT_FALSE(detector.calculate(1299));

    EXPECT_TRUE(detector.calculate(1300));

    detector.setInput(1300, baseline);

    EXPECT_FALSE(detector.calculate(1300));

    detector.setInput(1500, baseline);

    EXPECT_FALSE(detector.calculate(1899));

    EXPECT_TRUE(detector.calculate(1900));

}



TEST_P(StillnessDetectorTest, Suppression) {

    StillnessDetector detector(Options{.defaultValue = mDefaultValue,

                                       .windowDuration = 1000,

                                       .translationalThreshold = 1,

                                       .rotationalThreshold = 0.05});



    const Pose3f baseline(Vector3f{1, 2, 3}, Quaternionf::UnitRandom());

    const Pose3f outsideThreshold = baseline * Pose3f(Vector3f(1.1, 0, 0));

    const Pose3f middlePoint = baseline * Pose3f(Vector3f(0.55, 0, 0));



    detector.setInput(0, baseline);

    detector.setInput(1000, baseline);

    EXPECT_TRUE(detector.calculate(1000));

    detector.setInput(1100, outsideThreshold);

    EXPECT_FALSE(detector.calculate(1100));

    detector.setInput(2000, middlePoint);

    EXPECT_FALSE(detector.calculate(2000));

    EXPECT_FALSE(detector.calculate(2099));

    EXPECT_TRUE(detector.calculate(2100));

}



TEST_P(StillnessDetectorTest, Reset) {

media/libheadtracking/StillnessDetector.cpp

+27 −14
Original line number Diff line number Diff line
@@ -25,6 +25,7 @@ StillnessDetector::StillnessDetector(const Options& options) 
void StillnessDetector::reset() {

    mFifo.clear();

    mWindowFull = false;

    mSuppressionDeadline.reset();

}



void StillnessDetector::setInput(int64_t timestamp, const Pose3f& input) {
@@ -35,27 +36,34 @@ void StillnessDetector::setInput(int64_t timestamp, const Pose3f& input) { 
bool StillnessDetector::calculate(int64_t timestamp) {

    discardOld(timestamp);



    // Check whether all the poses in the queue are in the proximity of the new

    // one. We want to do this before checking the overriding conditions below, in order to update

    // the suppression deadline correctly.

    bool moved = false;



    if (!mFifo.empty()) {

        for (auto iter = mFifo.begin(); iter != mFifo.end() - 1; ++iter) {

            const auto& event = *iter;

            if (!areNear(event.pose, mFifo.back().pose)) {

                // Enable suppression for the duration of the window.

                mSuppressionDeadline = timestamp + mOptions.windowDuration;

                moved = true;

                break;

            }

        }

    }



    // If the window has not been full, return the default value.

    if (!mWindowFull) {

        return mOptions.defaultValue;

    }



    // An empty FIFO and window full is considered still (this will happen when the window duration

    // is shorter than the gap between samples, including the window size being 0).

    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)) {

    // Force "in motion" while the suppression deadline is active.

    if (mSuppressionDeadline.has_value()) {

        return false;

    }

    }



    return true;

    return !moved;

}



void StillnessDetector::discardOld(int64_t timestamp) {
@@ -72,6 +80,11 @@ void StillnessDetector::discardOld(int64_t timestamp) { 
        mWindowFull = true;

        mFifo.pop_front();

    }



    // Expire the suppression deadline.

    if (mSuppressionDeadline.has_value() && mSuppressionDeadline <= timestamp) {

        mSuppressionDeadline.reset();

    }

}



bool StillnessDetector::areNear(const Pose3f& pose1, const Pose3f& pose2) const {

media/libheadtracking/StillnessDetector.h

+5 −0
Original line number Diff line number Diff line
@@ -92,6 +92,11 @@ class StillnessDetector { 
    const float mCosHalfRotationalThreshold;

    std::deque<TimestampedPose> mFifo;

    bool mWindowFull = false;

    // As soon as motion is detected, this will be set for the time of detection + window duration,

    // and during this time we will always consider outselves in motion without checking. This is

    // used for hyteresis purposes, since because of the approximate method we use for determining

    // stillness, we may toggle back and forth at a rate faster than the window side.

    std::optional<int64_t> mSuppressionDeadline;



    bool areNear(const Pose3f& pose1, const Pose3f& pose2) const;

    void discardOld(int64_t timestamp);