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
diff --git a/media/libheadtracking/HeadTrackingProcessor-test.cpp b/media/libheadtracking/HeadTrackingProcessor-test.cpp
index ae5567d..1739c6d 100644
--- a/media/libheadtracking/HeadTrackingProcessor-test.cpp
+++ b/media/libheadtracking/HeadTrackingProcessor-test.cpp
@@ -48,6 +48,11 @@
std::unique_ptr<HeadTrackingProcessor> processor =
createHeadTrackingProcessor(Options{}, HeadTrackingMode::SCREEN_RELATIVE);
+
+ // Establish a baseline for the drift compensators.
+ processor->setWorldToHeadPose(0, Pose3f(), Twist3f());
+ processor->setWorldToScreenPose(0, Pose3f());
+
processor->setWorldToHeadPose(0, worldToHead, Twist3f());
processor->setWorldToScreenPose(0, worldToScreen);
processor->setScreenToStagePose(screenToStage);
@@ -76,6 +81,11 @@
std::unique_ptr<HeadTrackingProcessor> processor = createHeadTrackingProcessor(
Options{.predictionDuration = 2.f}, HeadTrackingMode::WORLD_RELATIVE);
+
+ // Establish a baseline for the drift compensators.
+ processor->setWorldToHeadPose(0, Pose3f(), Twist3f());
+ processor->setWorldToScreenPose(0, Pose3f());
+
processor->setWorldToHeadPose(0, worldToHead, headTwist);
processor->setWorldToScreenPose(0, worldToScreen);
processor->calculate(0);
@@ -100,6 +110,10 @@
std::unique_ptr<HeadTrackingProcessor> processor = createHeadTrackingProcessor(
Options{.maxTranslationalVelocity = 1}, HeadTrackingMode::STATIC);
+ // Establish a baseline for the drift compensators.
+ processor->setWorldToHeadPose(0, Pose3f(), Twist3f());
+ processor->setWorldToScreenPose(0, Pose3f());
+
processor->calculate(0);
processor->setDesiredMode(HeadTrackingMode::WORLD_RELATIVE);
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);
diff --git a/media/libheadtracking/PoseDriftCompensator.cpp b/media/libheadtracking/PoseDriftCompensator.cpp
index 9dfe172..0e90cad 100644
--- a/media/libheadtracking/PoseDriftCompensator.cpp
+++ b/media/libheadtracking/PoseDriftCompensator.cpp
@@ -29,10 +29,8 @@
PoseDriftCompensator::PoseDriftCompensator(const Options& options) : mOptions(options) {}
void PoseDriftCompensator::setInput(int64_t timestamp, const Pose3f& input) {
- if (!mTimestamp.has_value()) {
- // First input sample sets the output directly.
- mOutput = input;
- } else {
+ if (mTimestamp.has_value()) {
+ // Avoid computation upon first input (only sets the initial state).
Pose3f prevInputToInput = mPrevInput.inverse() * input;
mOutput = scale(mOutput, timestamp - mTimestamp.value()) * prevInputToInput;
}
@@ -41,6 +39,7 @@
}
void PoseDriftCompensator::recenter() {
+ mTimestamp.reset();
mOutput = Pose3f();
}