Donate to e Foundation | Murena handsets with /e/OS | Own a part of Murena! Learn more

Commit c6d7b831 authored by Ytai Ben-Tsvi's avatar Ytai Ben-Tsvi
Browse files

Fix a math error in StillnessDetector::areNear() 

The method used to determine the angular distance has been outright
wrong. Using a slightly more expensive, but accurate method instead.

Tightened up the test.

Test: atest --host libheadtracking-test:StillnessDetectorTest --rerun-until-failure 1000
Change-Id: I0f6b01340b6666b95ad032da8cd09ac1a33a8b41
parent 187097e8

media/libheadtracking/StillnessDetector-test.cpp

+2 −2
Original line number Diff line number Diff line
@@ -84,8 +84,8 @@ TEST(StillnessDetectorTest, NotStillRotation) { 


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

media/libheadtracking/StillnessDetector.cpp

+8 −9
Original line number Diff line number Diff line
@@ -19,7 +19,8 @@ 
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();
@@ -77,17 +78,15 @@ 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) {

    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;

    }

media/libheadtracking/StillnessDetector.h

+2 −0
Original line number Diff line number Diff line
@@ -83,6 +83,8 @@ class StillnessDetector { 
    };



    const Options mOptions;

    // Precalculated cos(mOptions.rotationalThreshold / 2)

    const float mCosHalfRotationalThreshold;

    std::deque<TimestampedPose> mFifo;

    bool mWindowFull = false;