Merge "Fix a math error in StillnessDetector::areNear()"

    const Pose3f baseline(Vector3f{1, 2, 3}, Quaternionf::UnitRandom());

    const Pose3f withinThreshold =

            baseline * Pose3f(Vector3f(0.3, -0.3, 0), rotateX(0.01) * rotateY(-0.01));

    const Pose3f outsideThreshold = baseline * Pose3f(rotateZ(0.08));

            baseline * Pose3f(Vector3f(0.3, -0.3, 0), rotateX(0.03) * rotateY(-0.03));

    const Pose3f outsideThreshold = baseline * Pose3f(rotateZ(0.06));

    EXPECT_FALSE(detector.calculate(0));

    detector.setInput(0, baseline);

    EXPECT_FALSE(detector.calculate(0));

namespace android {

namespace media {

StillnessDetector::StillnessDetector(const Options& options) : mOptions(options) {}

StillnessDetector::StillnessDetector(const Options& options)

    : mOptions(options), mCosHalfRotationalThreshold(cos(mOptions.rotationalThreshold / 2)) {}

void StillnessDetector::reset() {

    mFifo.clear();

    // 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) {

    if ((pose1.translation() - pose2.translation()).lpNorm<1>() > mOptions.translationalThreshold) {

        return false;

    }

    // Check orientation. We use the L1 norm of the imaginary components of the quaternion to reduce

    // computational load on expense of accuracy. For small angles, those components are approx.

    // equal to the angle of rotation and so the norm is approx. the total angle of rotation. The

    // L1 norm is an upper bound, so this approach will err on the side of "not near".

    if ((pose1.rotation().vec() - pose2.rotation().vec()).lpNorm<1>() >=

        mOptions.rotationalThreshold) {

    // 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;

    }

    };

    const Options mOptions;

    // Precalculated cos(mOptions.rotationalThreshold / 2)

    const float mCosHalfRotationalThreshold;

    std::deque<TimestampedPose> mFifo;

    bool mWindowFull = false;