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/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();
}