Remove drift compensator

Drift compensation is better done by the actual sensor, as it has the
knowledge of the best method to do so. For example, some sensors don't
suffer from drift at all.

This change replaces the drift compensation with a much simpler module
which only takes care of biasing (for recentering purposes).

Test: atest --host libheadtracking-test
Test: Manual verification of spatial audio with head-tracking.
Change-Id: I50ad8f53a6d7016bb8f75772702b35634adbc413
diff --git a/media/libheadtracking/HeadTrackingProcessor.cpp b/media/libheadtracking/HeadTrackingProcessor.cpp
index f2f15df..71fae8a 100644
--- a/media/libheadtracking/HeadTrackingProcessor.cpp
+++ b/media/libheadtracking/HeadTrackingProcessor.cpp
@@ -17,7 +17,7 @@
 #include "media/HeadTrackingProcessor.h"
 
 #include "ModeSelector.h"
-#include "PoseDriftCompensator.h"
+#include "PoseBias.h"
 #include "QuaternionUtil.h"
 #include "ScreenHeadFusion.h"
 #include "StillnessDetector.h"
@@ -33,14 +33,6 @@
   public:
     HeadTrackingProcessorImpl(const Options& options, HeadTrackingMode initialMode)
         : mOptions(options),
-          mHeadPoseDriftCompensator(PoseDriftCompensator::Options{
-                  .translationalDriftTimeConstant = options.translationalDriftTimeConstant,
-                  .rotationalDriftTimeConstant = options.rotationalDriftTimeConstant,
-          }),
-          mScreenPoseDriftCompensator(PoseDriftCompensator::Options{
-                  .translationalDriftTimeConstant = options.translationalDriftTimeConstant,
-                  .rotationalDriftTimeConstant = options.rotationalDriftTimeConstant,
-          }),
           mHeadStillnessDetector(StillnessDetector::Options{
                   .defaultValue = false,
                   .windowDuration = options.autoRecenterWindowDuration,
@@ -65,7 +57,7 @@
                             const Twist3f& headTwist) override {
         Pose3f predictedWorldToHead =
                 worldToHead * integrate(headTwist, mOptions.predictionDuration);
-        mHeadPoseDriftCompensator.setInput(timestamp, predictedWorldToHead);
+        mHeadPoseBias.setInput(predictedWorldToHead);
         mHeadStillnessDetector.setInput(timestamp, predictedWorldToHead);
         mWorldToHeadTimestamp = timestamp;
     }
@@ -78,7 +70,7 @@
         }
 
         Pose3f worldToLogicalScreen = worldToScreen * Pose3f(rotateY(-mPhysicalToLogicalAngle));
-        mScreenPoseDriftCompensator.setInput(timestamp, worldToLogicalScreen);
+        mScreenPoseBias.setInput(worldToLogicalScreen);
         mScreenStillnessDetector.setInput(timestamp, worldToLogicalScreen);
         mWorldToScreenTimestamp = timestamp;
     }
@@ -94,7 +86,7 @@
     void calculate(int64_t timestamp) override {
         // Handle the screen first, since it might trigger a recentering of the head.
         if (mWorldToScreenTimestamp.has_value()) {
-            const Pose3f worldToLogicalScreen = mScreenPoseDriftCompensator.getOutput();
+            const Pose3f worldToLogicalScreen = mScreenPoseBias.getOutput();
             bool screenStable = mScreenStillnessDetector.calculate(timestamp);
             mModeSelector.setScreenStable(mWorldToScreenTimestamp.value(), screenStable);
             // Whenever the screen is unstable, recenter the head pose.
@@ -107,11 +99,11 @@
 
         // Handle head.
         if (mWorldToHeadTimestamp.has_value()) {
-            Pose3f worldToHead = mHeadPoseDriftCompensator.getOutput();
+            Pose3f worldToHead = mHeadPoseBias.getOutput();
             // Auto-recenter.
             if (mHeadStillnessDetector.calculate(timestamp)) {
                 recenter(true, false);
-                worldToHead = mHeadPoseDriftCompensator.getOutput();
+                worldToHead = mHeadPoseBias.getOutput();
             }
 
             mScreenHeadFusion.setWorldToHeadPose(mWorldToHeadTimestamp.value(), worldToHead);
@@ -142,11 +134,11 @@
 
     void recenter(bool recenterHead, bool recenterScreen) override {
         if (recenterHead) {
-            mHeadPoseDriftCompensator.recenter();
+            mHeadPoseBias.recenter();
             mHeadStillnessDetector.reset();
         }
         if (recenterScreen) {
-            mScreenPoseDriftCompensator.recenter();
+            mScreenPoseBias.recenter();
             mScreenStillnessDetector.reset();
         }
 
@@ -169,8 +161,8 @@
     std::optional<int64_t> mWorldToHeadTimestamp;
     std::optional<int64_t> mWorldToScreenTimestamp;
     Pose3f mHeadToStagePose;
-    PoseDriftCompensator mHeadPoseDriftCompensator;
-    PoseDriftCompensator mScreenPoseDriftCompensator;
+    PoseBias mHeadPoseBias;
+    PoseBias mScreenPoseBias;
     StillnessDetector mHeadStillnessDetector;
     StillnessDetector mScreenStillnessDetector;
     ScreenHeadFusion mScreenHeadFusion;