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
diff --git a/media/libheadtracking/StillnessDetector-test.cpp b/media/libheadtracking/StillnessDetector-test.cpp
index 646496a..02f9d8a 100644
--- a/media/libheadtracking/StillnessDetector-test.cpp
+++ b/media/libheadtracking/StillnessDetector-test.cpp
@@ -85,9 +85,11 @@
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 @@
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 @@
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) {
diff --git a/media/libheadtracking/StillnessDetector.cpp b/media/libheadtracking/StillnessDetector.cpp
index 8f9b53a..9806352 100644
--- a/media/libheadtracking/StillnessDetector.cpp
+++ b/media/libheadtracking/StillnessDetector.cpp
@@ -25,6 +25,7 @@
void StillnessDetector::reset() {
mFifo.clear();
mWindowFull = false;
+ mSuppressionDeadline.reset();
}
void StillnessDetector::setInput(int64_t timestamp, const Pose3f& input) {
@@ -35,27 +36,34 @@
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;
+ // Force "in motion" while the suppression deadline is active.
+ if (mSuppressionDeadline.has_value()) {
+ return false;
}
- // 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;
+ return !moved;
}
void StillnessDetector::discardOld(int64_t timestamp) {
@@ -72,6 +80,11 @@
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 {
diff --git a/media/libheadtracking/StillnessDetector.h b/media/libheadtracking/StillnessDetector.h
index 0ee6b6f..ee4b2d8 100644
--- a/media/libheadtracking/StillnessDetector.h
+++ b/media/libheadtracking/StillnessDetector.h
@@ -92,6 +92,11 @@
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);