1 /*
2 * Copyright (C) 2021 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 #include "StillnessDetector.h"
18
19 namespace android {
20 namespace media {
21
StillnessDetector(const Options & options)22 StillnessDetector::StillnessDetector(const Options& options)
23 : mOptions(options), mCosHalfRotationalThreshold(cos(mOptions.rotationalThreshold / 2)) {}
24
reset()25 void StillnessDetector::reset() {
26 mFifo.clear();
27 mWindowFull = false;
28 mSuppressionDeadline.reset();
29 // A "true" state indicates stillness is detected (default = true)
30 mCurrentState = true;
31 mPreviousState = true;
32 }
33
setInput(int64_t timestamp,const Pose3f & input)34 void StillnessDetector::setInput(int64_t timestamp, const Pose3f& input) {
35 mFifo.push_back(TimestampedPose{timestamp, input});
36 discardOld(timestamp);
37 }
38
getPreviousState() const39 bool StillnessDetector::getPreviousState() const {
40 return mPreviousState;
41 }
42
calculate(int64_t timestamp)43 bool StillnessDetector::calculate(int64_t timestamp) {
44 // Move the current stillness state to the previous state.
45 // This allows us to detect transitions into and out of stillness.
46 mPreviousState = mCurrentState;
47
48 discardOld(timestamp);
49
50 // Check whether all the poses in the queue are in the proximity of the new one. We want to do
51 // this before checking the overriding conditions below, in order to update the suppression
52 // deadline correctly. We always go from end to start, to find the most recent pose that
53 // violated stillness and update the suppression deadline if it has not been set or if the new
54 // one ends after the current one.
55 bool moved = false;
56
57 if (!mFifo.empty()) {
58 for (auto iter = mFifo.rbegin() + 1; iter != mFifo.rend(); ++iter) {
59 const auto& event = *iter;
60 if (!areNear(event.pose, mFifo.back().pose)) {
61 // Enable suppression for the duration of the window.
62 int64_t deadline = event.timestamp + mOptions.windowDuration;
63 if (!mSuppressionDeadline.has_value() || mSuppressionDeadline.value() < deadline) {
64 mSuppressionDeadline = deadline;
65 }
66 moved = true;
67 break;
68 }
69 }
70 }
71
72 // If the window has not been full, return the default value.
73 if (!mWindowFull) {
74 mCurrentState = mOptions.defaultValue;
75 }
76 // Force "in motion" while the suppression deadline is active.
77 else if (mSuppressionDeadline.has_value()) {
78 mCurrentState = false;
79 }
80 else {
81 mCurrentState = !moved;
82 }
83
84 return mCurrentState;
85 }
86
discardOld(int64_t timestamp)87 void StillnessDetector::discardOld(int64_t timestamp) {
88 // Handle the special case of the window duration being zero (always considered full).
89 if (mOptions.windowDuration == 0) {
90 mFifo.clear();
91 mWindowFull = true;
92 }
93
94 // Remove any events from the queue that are older than the window. If there were any such
95 // events we consider the window full.
96 const int64_t windowStart = timestamp - mOptions.windowDuration;
97 while (!mFifo.empty() && mFifo.front().timestamp <= windowStart) {
98 mWindowFull = true;
99 mFifo.pop_front();
100 }
101
102 // Expire the suppression deadline.
103 if (mSuppressionDeadline.has_value() && mSuppressionDeadline <= timestamp) {
104 mSuppressionDeadline.reset();
105 }
106 }
107
areNear(const Pose3f & pose1,const Pose3f & pose2) const108 bool StillnessDetector::areNear(const Pose3f& pose1, const Pose3f& pose2) const {
109 // Check translation. We use the L1 norm to reduce computational load on expense of accuracy.
110 // The L1 norm is an upper bound for the actual (L2) norm, so this approach will err on the side
111 // of "not near".
112 if ((pose1.translation() - pose2.translation()).lpNorm<1>() > mOptions.translationalThreshold) {
113 return false;
114 }
115
116 // Check orientation.
117 // The angle x between the quaternions is greater than that threshold iff
118 // cos(x/2) < cos(threshold/2).
119 // cos(x/2) can be efficiently calculated as the dot product of both quaternions.
120 if (pose1.rotation().dot(pose2.rotation()) < mCosHalfRotationalThreshold) {
121 return false;
122 }
123
124 return true;
125 }
126
127 } // namespace media
128 } // namespace android
129