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.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;
}