Improve recentering logic am: b2a87e5dc0
Original change: https://googleplex-android-review.googlesource.com/c/platform/frameworks/av/+/15645198
Change-Id: I1ff49df420b8ffdc7b0f4288e1d77a4062ba4a3e
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();
}