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
diff --git a/media/libheadtracking/StillnessDetector-test.cpp b/media/libheadtracking/StillnessDetector-test.cpp
index a53ba8c..29b036e 100644
--- a/media/libheadtracking/StillnessDetector-test.cpp
+++ b/media/libheadtracking/StillnessDetector-test.cpp
@@ -84,8 +84,8 @@
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));
diff --git a/media/libheadtracking/StillnessDetector.cpp b/media/libheadtracking/StillnessDetector.cpp
index 832351d..5fa4e3a 100644
--- a/media/libheadtracking/StillnessDetector.cpp
+++ b/media/libheadtracking/StillnessDetector.cpp
@@ -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 @@
// 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;
}
diff --git a/media/libheadtracking/StillnessDetector.h b/media/libheadtracking/StillnessDetector.h
index fd26aa9..cae9d9d 100644
--- a/media/libheadtracking/StillnessDetector.h
+++ b/media/libheadtracking/StillnessDetector.h
@@ -83,6 +83,8 @@
};
const Options mOptions;
+ // Precalculated cos(mOptions.rotationalThreshold / 2)
+ const float mCosHalfRotationalThreshold;
std::deque<TimestampedPose> mFifo;
bool mWindowFull = false;