commit b2a87e5dc0bbd496df3addccf4ccd913f132c920
author Ytai Ben-Tsvi <ytai@google.com> Fri Aug 20 10:17:05 2021 -0700
committer Ytai Ben-Tsvi <ytai@google.com> Mon Aug 23 10:00:53 2021 -0700
tree 47a833f4b74470a8abdeccc55ebd47f317f5d4b9
parent 779d1eec1bc53b707fc2d2b1dcff7c87be854098

Improve recentering logic

Allow recentering to be used to "repair" discontinuities in the input
stream by completely resetting the state. That includes considering
the next sample that arrives as the new baseline.

Test: atest --host libheadtracking-test
Bug: 188502620
Change-Id: Ib85679ce8f82c0059bd7260edf73418ab7ba9494

media/libheadtracking/PoseDriftCompensator-test.cpp

diff --git a/media/libheadtracking/PoseDriftCompensator-test.cpp b/media/libheadtracking/PoseDriftCompensator-test.cpp
index 74f4bee..df0a05f 100644
--- a/media/libheadtracking/PoseDriftCompensator-test.cpp
+++ b/media/libheadtracking/PoseDriftCompensator-test.cpp
@@ -39,17 +39,22 @@
     Pose3f pose2({4, 5, 6}, Quaternionf::UnitRandom());
     PoseDriftCompensator comp(Options{});
 
+    // First pose sets the baseline.
     comp.setInput(1000, pose1);
-    EXPECT_EQ(comp.getOutput(), pose1);
+    EXPECT_EQ(comp.getOutput(), Pose3f());
 
     comp.setInput(2000, pose2);
-    EXPECT_EQ(comp.getOutput(), pose2);
+    EXPECT_EQ(comp.getOutput(), pose1.inverse() * pose2);
 
+    // Recentering resets the baseline.
     comp.recenter();
     EXPECT_EQ(comp.getOutput(), Pose3f());
 
     comp.setInput(3000, pose1);
-    EXPECT_EQ(comp.getOutput(), pose2.inverse() * pose1);
+    EXPECT_EQ(comp.getOutput(), Pose3f());
+
+    comp.setInput(4000, pose2);
+    EXPECT_EQ(comp.getOutput(), pose1.inverse() * pose2);
 }
 
 TEST(PoseDriftCompensator, NoDriftZeroTime) {
@@ -58,16 +63,19 @@
     PoseDriftCompensator comp(Options{});
 
     comp.setInput(1000, pose1);
-    EXPECT_EQ(comp.getOutput(), pose1);
+    EXPECT_EQ(comp.getOutput(), Pose3f());
 
     comp.setInput(1000, pose2);
-    EXPECT_EQ(comp.getOutput(), pose2);
+    EXPECT_EQ(comp.getOutput(), pose1.inverse() * pose2);
 
     comp.recenter();
     EXPECT_EQ(comp.getOutput(), Pose3f());
 
     comp.setInput(1000, pose1);
-    EXPECT_EQ(comp.getOutput(), pose2.inverse() * pose1);
+    EXPECT_EQ(comp.getOutput(), Pose3f());
+
+    comp.setInput(1000, pose2);
+    EXPECT_EQ(comp.getOutput(), pose1.inverse() * pose2);
 }
 
 TEST(PoseDriftCompensator, Asymptotic) {
@@ -92,16 +100,19 @@
             Options{.translationalDriftTimeConstant = 1e7, .rotationalDriftTimeConstant = 1e7});
 
     comp.setInput(0, pose1);
-    EXPECT_EQ(comp.getOutput(), pose1);
+    EXPECT_EQ(comp.getOutput(), Pose3f());
 
     comp.setInput(1, pose2);
-    EXPECT_EQ(comp.getOutput(), pose2);
+    EXPECT_EQ(comp.getOutput(), pose1.inverse() * pose2);
 
     comp.recenter();
     EXPECT_EQ(comp.getOutput(), Pose3f());
 
     comp.setInput(2, pose1);
-    EXPECT_EQ(comp.getOutput(), pose2.inverse() * pose1);
+    EXPECT_EQ(comp.getOutput(), Pose3f());
+
+    comp.setInput(3, pose2);
+    EXPECT_EQ(comp.getOutput(), pose1.inverse() * pose2);
 }
 
 TEST(PoseDriftCompensator, Drift) {
@@ -109,6 +120,9 @@
     PoseDriftCompensator comp(
             Options{.translationalDriftTimeConstant = 500, .rotationalDriftTimeConstant = 1000});
 
+    // Establish a baseline.
+    comp.setInput(1000, Pose3f());
+
     // Initial pose is used as is.
     comp.setInput(1000, pose1);
     EXPECT_EQ(comp.getOutput(), pose1);