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 #pragma once 17 18 #include <deque> 19 20 #include <media/Pose.h> 21 22 namespace android { 23 namespace media { 24 25 /** 26 * Given a stream of poses, determines if the pose is stable ("still"). 27 * Stillness is defined as all poses in the recent history ("window") being near the most recent 28 * sample. 29 * 30 * Typical usage: 31 * 32 * StillnessDetector detector(StilnessDetector::Options{...}); 33 * 34 * while (...) { 35 * detector.setInput(timestamp, pose); 36 * bool still = detector.calculate(timestamp); 37 * } 38 * 39 * The detection is not considered reliable until a sufficient number of samples has been provided 40 * for an initial fill-up of the window. During that time, the detector will return whatever default 41 * value has been configured. 42 * The reset() method can be used to empty the window again and get back to this initial state. 43 * In the special case of the window size being 0, the state will always be considered "still". 44 */ 45 class StillnessDetector { 46 public: 47 /** 48 * Configuration options for the detector. 49 */ 50 struct Options { 51 /** 52 * During the initial fill of the window, should we consider the state still? 53 */ 54 bool defaultValue; 55 /** 56 * How long is the window, in ticks. The special value of 0 indicates that the stream is 57 * always considered still. 58 */ 59 int64_t windowDuration; 60 /** 61 * How much of a translational deviation from the target (in meters) is considered motion. 62 * This is an approximate quantity - the actual threshold might be a little different as we 63 * trade-off accuracy with computational efficiency. 64 */ 65 float translationalThreshold; 66 /** 67 * How much of a rotational deviation from the target (in radians) is considered motion. 68 * This is an approximate quantity - the actual threshold might be a little different as we 69 * trade-off accuracy with computational efficiency. 70 */ 71 float rotationalThreshold; 72 }; 73 74 /** Ctor. */ 75 explicit StillnessDetector(const Options& options); 76 77 /** Clear the window. */ 78 void reset(); 79 /** Push a new sample. */ 80 void setInput(int64_t timestamp, const Pose3f& input); 81 /** Calculate whether the stream is still at the given timestamp. */ 82 bool calculate(int64_t timestamp); 83 /** Return the stillness state from the previous call to calculate() */ 84 bool getPreviousState() const; 85 private: 86 struct TimestampedPose { 87 int64_t timestamp; 88 Pose3f pose; 89 }; 90 91 const Options mOptions; 92 // Precalculated cos(mOptions.rotationalThreshold / 2) 93 const float mCosHalfRotationalThreshold; 94 std::deque<TimestampedPose> mFifo; 95 bool mWindowFull = false; 96 bool mCurrentState = true; 97 bool mPreviousState = true; 98 // As soon as motion is detected, this will be set for the time of detection + window duration, 99 // and during this time we will always consider outselves in motion without checking. This is 100 // used for hyteresis purposes, since because of the approximate method we use for determining 101 // stillness, we may toggle back and forth at a rate faster than the window side. 102 std::optional<int64_t> mSuppressionDeadline; 103 104 bool areNear(const Pose3f& pose1, const Pose3f& pose2) const; 105 void discardOld(int64_t timestamp); 106 }; 107 108 } // namespace media 109 } // namespace android 110