media/libheadtracking/StillnessDetector.cpp

/*
 * 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.
 */

#include "StillnessDetector.h"

namespace android {
namespace media {

StillnessDetector::StillnessDetector(const Options& options)
    : mOptions(options), mCosHalfRotationalThreshold(cos(mOptions.rotationalThreshold / 2)) {}

void StillnessDetector::reset() {
    mFifo.clear();
    mWindowFull = false;
}

void StillnessDetector::setInput(int64_t timestamp, const Pose3f& input) {
    mFifo.push_back(TimestampedPose{timestamp, input});
    discardOld(timestamp);
}

bool StillnessDetector::calculate(int64_t timestamp) {
    discardOld(timestamp);

    // If the window has not been full, we don't consider ourselves still.
    if (!mWindowFull) {
        return false;
    }

    // An empty FIFO and window full is considered still (this will happen in the unlikely case when
    // the window duration is shorter than the gap between samples).
    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)) {
            return false;
        }
    }

    return true;
}

void StillnessDetector::discardOld(int64_t timestamp) {
    // Handle the special case of the window duration being zero (always considered full).
    if (mOptions.windowDuration == 0) {
        mFifo.clear();
        mWindowFull = true;
    }

    // Remove any events from the queue that are older than the window. If there were any such
    // events we consider the window full.
    const int64_t windowStart = timestamp - mOptions.windowDuration;
    while (!mFifo.empty() && mFifo.front().timestamp <= windowStart) {
        mWindowFull = true;
        mFifo.pop_front();
    }
}

bool StillnessDetector::areNear(const Pose3f& pose1, const Pose3f& pose2) const {
    // Check translation. We use the L1 norm to reduce computational load on expense of accuracy.
    // The L1 norm is an upper bound for the actual (L2) norm, so this approach will err on the side
    // of "not near".
    if ((pose1.translation() - pose2.translation()).lpNorm<1>() > mOptions.translationalThreshold) {
        return false;
    }

    // Check orientation.
    // The angle x between the quaternions is greater than that threshold iff
    // cos(x/2) < cos(threshold/2).
    // cos(x/2) can be efficiently calculated as the dot product of both quaternions.
    if (pose1.rotation().dot(pose2.rotation()) < mCosHalfRotationalThreshold) {
        return false;
    }

    return true;
}

}  // namespace media
}  // namespace android