110 lines
3.9 KiB
C++
110 lines
3.9 KiB
C++
/*
|
|
* Copyright (C) 2021 The Android Open Source Project
|
|
*
|
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
|
* you may not use this file except in compliance with the License.
|
|
* You may obtain a copy of the License at
|
|
*
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, software
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
* See the License for the specific language governing permissions and
|
|
* limitations under the License.
|
|
*/
|
|
#pragma once
|
|
|
|
#include <deque>
|
|
|
|
#include <media/Pose.h>
|
|
|
|
namespace android {
|
|
namespace media {
|
|
|
|
/**
|
|
* Given a stream of poses, determines if the pose is stable ("still").
|
|
* Stillness is defined as all poses in the recent history ("window") being near the most recent
|
|
* sample.
|
|
*
|
|
* Typical usage:
|
|
*
|
|
* StillnessDetector detector(StilnessDetector::Options{...});
|
|
*
|
|
* while (...) {
|
|
* detector.setInput(timestamp, pose);
|
|
* bool still = detector.calculate(timestamp);
|
|
* }
|
|
*
|
|
* The detection is not considered reliable until a sufficient number of samples has been provided
|
|
* for an initial fill-up of the window. During that time, the detector will return whatever default
|
|
* value has been configured.
|
|
* The reset() method can be used to empty the window again and get back to this initial state.
|
|
* In the special case of the window size being 0, the state will always be considered "still".
|
|
*/
|
|
class StillnessDetector {
|
|
public:
|
|
/**
|
|
* Configuration options for the detector.
|
|
*/
|
|
struct Options {
|
|
/**
|
|
* During the initial fill of the window, should we consider the state still?
|
|
*/
|
|
bool defaultValue;
|
|
/**
|
|
* How long is the window, in ticks. The special value of 0 indicates that the stream is
|
|
* always considered still.
|
|
*/
|
|
int64_t windowDuration;
|
|
/**
|
|
* How much of a translational deviation from the target (in meters) is considered motion.
|
|
* This is an approximate quantity - the actual threshold might be a little different as we
|
|
* trade-off accuracy with computational efficiency.
|
|
*/
|
|
float translationalThreshold;
|
|
/**
|
|
* How much of a rotational deviation from the target (in radians) is considered motion.
|
|
* This is an approximate quantity - the actual threshold might be a little different as we
|
|
* trade-off accuracy with computational efficiency.
|
|
*/
|
|
float rotationalThreshold;
|
|
};
|
|
|
|
/** Ctor. */
|
|
explicit StillnessDetector(const Options& options);
|
|
|
|
/** Clear the window. */
|
|
void reset();
|
|
/** Push a new sample. */
|
|
void setInput(int64_t timestamp, const Pose3f& input);
|
|
/** Calculate whether the stream is still at the given timestamp. */
|
|
bool calculate(int64_t timestamp);
|
|
/** Return the stillness state from the previous call to calculate() */
|
|
bool getPreviousState() const;
|
|
private:
|
|
struct TimestampedPose {
|
|
int64_t timestamp;
|
|
Pose3f pose;
|
|
};
|
|
|
|
const Options mOptions;
|
|
// Precalculated cos(mOptions.rotationalThreshold / 2)
|
|
const float mCosHalfRotationalThreshold;
|
|
std::deque<TimestampedPose> mFifo;
|
|
bool mWindowFull = false;
|
|
bool mCurrentState = true;
|
|
bool mPreviousState = true;
|
|
// 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);
|
|
};
|
|
|
|
} // namespace media
|
|
} // namespace android
|