Spatial Audio: Improve Head-Tracking prediction
1) Add LEAST_SQUARES pose prediction.
Define 3 types of prediction: LAST, TWIST, and LEAST_SQUARES.
Furthermore, add a 4th AUTO prediction type to automatically
choose between the 3 (currently always selects LEAST_SQUARES).
2) Add Verification of pose prediction,
Enabled on userdebug and eng builds.
Prediction checked for 50ms, 100ms, 200ms look-ahead.
3) Add 2 properties for predictor control
audio.spatializer.pose_predictor_type
// Example: 0 == AUTO, 1 == LAST,
// 2 == TWIST, 3 == LEAST_SQUARES
audio.spatializer.prediction_duration_ms
// Example: 50 or 150
4) By default
Use the new LEAST_SQUARES predictor. (Old was TWIST)
Increase prediction_duration_ms from 50ms to 120ms.
Test: adb shell dumpsys media.audio_policy
Test: atest libheadtracking-test
Test: adb shell setprop on properties.
Test: force user / eng compilation modes
Bug: 270763710
Merged-In: I13662df10bc06480f336bb894fddd83e09c03c7e
Change-Id: I13662df10bc06480f336bb894fddd83e09c03c7e
diff --git a/media/libheadtracking/Android.bp b/media/libheadtracking/Android.bp
index 9636949..9955862 100644
--- a/media/libheadtracking/Android.bp
+++ b/media/libheadtracking/Android.bp
@@ -16,6 +16,7 @@
"Pose.cpp",
"PoseBias.cpp",
"PoseDriftCompensator.cpp",
+ "PosePredictor.cpp",
"PoseRateLimiter.cpp",
"QuaternionUtil.cpp",
"ScreenHeadFusion.cpp",
@@ -39,6 +40,12 @@
cflags: [
"-Wthread-safety",
],
+ product_variables: {
+ debuggable: {
+ // enable experiments only in userdebug and eng builds
+ cflags: ["-DENABLE_VERIFICATION"],
+ },
+ },
}
cc_library {
@@ -80,6 +87,7 @@
"Pose-test.cpp",
"PoseBias-test.cpp",
"PoseDriftCompensator-test.cpp",
+ "PosePredictor.cpp",
"PoseRateLimiter-test.cpp",
"QuaternionUtil-test.cpp",
"ScreenHeadFusion-test.cpp",
diff --git a/media/libheadtracking/HeadTrackingProcessor-test.cpp b/media/libheadtracking/HeadTrackingProcessor-test.cpp
index b9dd0b8..5190f52 100644
--- a/media/libheadtracking/HeadTrackingProcessor-test.cpp
+++ b/media/libheadtracking/HeadTrackingProcessor-test.cpp
@@ -82,6 +82,8 @@
std::unique_ptr<HeadTrackingProcessor> processor = createHeadTrackingProcessor(
Options{.predictionDuration = 2.f}, HeadTrackingMode::WORLD_RELATIVE);
+ processor->setPosePredictorType(PosePredictorType::TWIST);
+
// Establish a baseline for the drift compensators.
processor->setWorldToHeadPose(0, Pose3f(), Twist3f());
processor->setWorldToScreenPose(0, Pose3f());
diff --git a/media/libheadtracking/HeadTrackingProcessor.cpp b/media/libheadtracking/HeadTrackingProcessor.cpp
index b70b6dc..8502af0 100644
--- a/media/libheadtracking/HeadTrackingProcessor.cpp
+++ b/media/libheadtracking/HeadTrackingProcessor.cpp
@@ -22,6 +22,7 @@
#include "ModeSelector.h"
#include "PoseBias.h"
+#include "PosePredictor.h"
#include "ScreenHeadFusion.h"
#include "StillnessDetector.h"
@@ -59,8 +60,8 @@
void setWorldToHeadPose(int64_t timestamp, const Pose3f& worldToHead,
const Twist3f& headTwist) override {
- Pose3f predictedWorldToHead =
- worldToHead * integrate(headTwist, mOptions.predictionDuration);
+ const Pose3f predictedWorldToHead = mPosePredictor.predict(
+ timestamp, worldToHead, headTwist, mOptions.predictionDuration);
mHeadPoseBias.setInput(predictedWorldToHead);
mHeadStillnessDetector.setInput(timestamp, predictedWorldToHead);
mWorldToHeadTimestamp = timestamp;
@@ -161,6 +162,10 @@
}
}
+ void setPosePredictorType(PosePredictorType type) override {
+ mPosePredictor.setPosePredictorType(type);
+ }
+
std::string toString_l(unsigned level) const override {
std::string prefixSpace(level, ' ');
std::string ss = prefixSpace + "HeadTrackingProcessor:\n";
@@ -186,6 +191,7 @@
prefixSpace.c_str(), mOptions.screenStillnessRotationalThreshold);
ss += mModeSelector.toString(level + 1);
ss += mRateLimiter.toString(level + 1);
+ ss += mPosePredictor.toString(level + 1);
ss.append(prefixSpace + "ReCenterHistory:\n");
ss += mLocalLog.dumpToString((prefixSpace + " ").c_str(), mMaxLocalLogLine);
return ss;
@@ -207,6 +213,7 @@
ScreenHeadFusion mScreenHeadFusion;
ModeSelector mModeSelector;
PoseRateLimiter mRateLimiter;
+ PosePredictor mPosePredictor;
static constexpr std::size_t mMaxLocalLogLine = 10;
SimpleLog mLocalLog{mMaxLocalLogLine};
};
@@ -230,5 +237,26 @@
return "EnumNotImplemented";
};
+std::string toString(PosePredictorType posePredictorType) {
+ switch (posePredictorType) {
+ case PosePredictorType::AUTO: return "AUTO";
+ case PosePredictorType::LAST: return "LAST";
+ case PosePredictorType::TWIST: return "TWIST";
+ case PosePredictorType::LEAST_SQUARES: return "LEAST_SQUARES";
+ }
+ return "UNKNOWN" + std::to_string((int)posePredictorType);
+}
+
+bool isValidPosePredictorType(PosePredictorType posePredictorType) {
+ switch (posePredictorType) {
+ case PosePredictorType::AUTO:
+ case PosePredictorType::LAST:
+ case PosePredictorType::TWIST:
+ case PosePredictorType::LEAST_SQUARES:
+ return true;
+ }
+ return false;
+}
+
} // namespace media
} // namespace android
diff --git a/media/libheadtracking/ModeSelector.cpp b/media/libheadtracking/ModeSelector.cpp
index 6277090..7ee21b3 100644
--- a/media/libheadtracking/ModeSelector.cpp
+++ b/media/libheadtracking/ModeSelector.cpp
@@ -117,10 +117,12 @@
std::string ModeSelector::toString(unsigned level) const {
std::string prefixSpace(level, ' ');
std::string ss(prefixSpace);
- StringAppendF(&ss, "ModeSelector: ScreenToStage %s\n",
- mScreenToStage.toString().c_str());
- ss.append(prefixSpace + "Mode downgrade history:\n");
- ss += mLocalLog.dumpToString((prefixSpace + " ").c_str(), sMaxLocalLogLine);
+ ss.append("ModeSelector: ScreenToStage ")
+ .append(mScreenToStage.toString())
+ .append("\n")
+ .append(prefixSpace)
+ .append("Mode change history:\n")
+ .append(mLocalLog.dumpToString((prefixSpace + " ").c_str(), sMaxLocalLogLine));
return ss;
}
diff --git a/media/libheadtracking/PosePredictor.cpp b/media/libheadtracking/PosePredictor.cpp
new file mode 100644
index 0000000..f67a966
--- /dev/null
+++ b/media/libheadtracking/PosePredictor.cpp
@@ -0,0 +1,238 @@
+/*
+ * Copyright (C) 2023 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "PosePredictor.h"
+
+namespace android::media {
+
+namespace {
+#ifdef ENABLE_VERIFICATION
+constexpr bool kEnableVerification = true;
+constexpr std::array<int, 3> kLookAheadMs{ 50, 100, 200 };
+#else
+constexpr bool kEnableVerification = false;
+constexpr std::array<int, 0> kLookAheadMs{};
+#endif
+
+} // namespace
+
+void LeastSquaresPredictor::add(int64_t atNs, const Pose3f& pose, const Twist3f& twist)
+{
+ (void)twist;
+ mLastAtNs = atNs;
+ mLastPose = pose;
+ const auto q = pose.rotation();
+ const double datNs = static_cast<double>(atNs);
+ mRw.add({datNs, q.w()});
+ mRx.add({datNs, q.x()});
+ mRy.add({datNs, q.y()});
+ mRz.add({datNs, q.z()});
+}
+
+Pose3f LeastSquaresPredictor::predict(int64_t atNs) const
+{
+ if (mRw.getN() < kMinimumSamplesForPrediction) return mLastPose;
+
+ /*
+ * Using parametric form, we have q(t) = { w(t), x(t), y(t), z(t) }.
+ * We compute the least squares prediction of w, x, y, z.
+ */
+ const double dLookahead = static_cast<double>(atNs);
+ Eigen::Quaternionf lsq(
+ mRw.getYFromX(dLookahead),
+ mRx.getYFromX(dLookahead),
+ mRy.getYFromX(dLookahead),
+ mRz.getYFromX(dLookahead));
+
+ /*
+ * We cheat here, since the result lsq is the least squares prediction
+ * in H (arbitrary quaternion), not the least squares prediction in
+ * SO(3) (unit quaternion).
+ *
+ * In other words, the result for lsq is most likely not a unit quaternion.
+ * To solve this, we normalize, thereby selecting the closest unit quaternion
+ * in SO(3) to the prediction in H.
+ */
+ lsq.normalize();
+ return Pose3f(lsq);
+}
+
+void LeastSquaresPredictor::reset() {
+ mLastAtNs = {};
+ mLastPose = {};
+ mRw.reset();
+ mRx.reset();
+ mRy.reset();
+ mRz.reset();
+}
+
+std::string LeastSquaresPredictor::toString(size_t index) const {
+ std::string s(index, ' ');
+ s.append("LeastSquaresPredictor using alpha: ")
+ .append(std::to_string(mAlpha))
+ .append(" last pose: ")
+ .append(mLastPose.toString())
+ .append("\n");
+ return s;
+}
+
+// Formatting
+static inline std::vector<size_t> createDelimiterIdx(size_t predictors, size_t lookaheads) {
+ if (predictors == 0) return {};
+ --predictors;
+ std::vector<size_t> delimiterIdx(predictors);
+ for (size_t i = 0; i < predictors; ++i) {
+ delimiterIdx[i] = (i + 1) * lookaheads;
+ }
+ return delimiterIdx;
+}
+
+PosePredictor::PosePredictor()
+ : mPredictors{ // must match switch in getCurrentPredictor()
+ std::make_shared<LastPredictor>(),
+ std::make_shared<TwistPredictor>(),
+ std::make_shared<LeastSquaresPredictor>(),
+ }
+ , mLookaheadMs(kLookAheadMs.begin(), kLookAheadMs.end())
+ , mVerifiers(std::size(mLookaheadMs) * std::size(mPredictors))
+ , mDelimiterIdx(createDelimiterIdx(std::size(mPredictors), std::size(mLookaheadMs)))
+ , mPredictionRecorder(
+ std::size(mVerifiers) /* vectorSize */, std::chrono::seconds(1), 10 /* maxLogLine */,
+ mDelimiterIdx)
+ , mPredictionDurableRecorder(
+ std::size(mVerifiers) /* vectorSize */, std::chrono::minutes(1), 10 /* maxLogLine */,
+ mDelimiterIdx)
+ {
+}
+
+Pose3f PosePredictor::predict(
+ int64_t timestampNs, const Pose3f& pose, const Twist3f& twist, float predictionDurationNs)
+{
+ if (timestampNs - mLastTimestampNs > kMaximumSampleIntervalBeforeResetNs) {
+ for (const auto& predictor : mPredictors) {
+ predictor->reset();
+ }
+ ++mResets;
+ }
+ mLastTimestampNs = timestampNs;
+
+ auto selectedPredictor = getCurrentPredictor();
+ if constexpr (kEnableVerification) {
+ // Update all Predictors
+ for (const auto& predictor : mPredictors) {
+ predictor->add(timestampNs, pose, twist);
+ }
+
+ // Update Verifiers and calculate errors
+ std::vector<float> error(std::size(mVerifiers));
+ for (size_t i = 0; i < mLookaheadMs.size(); ++i) {
+ constexpr float RADIAN_TO_DEGREES = 180 / M_PI;
+ const int64_t atNs =
+ timestampNs + mLookaheadMs[i] * PosePredictorVerifier::kMillisToNanos;
+
+ for (size_t j = 0; j < mPredictors.size(); ++j) {
+ const size_t idx = i * std::size(mPredictors) + j;
+ mVerifiers[idx].verifyActualPose(timestampNs, pose);
+ mVerifiers[idx].addPredictedPose(atNs, mPredictors[j]->predict(atNs));
+ error[idx] = RADIAN_TO_DEGREES * mVerifiers[idx].lastError();
+ }
+ }
+ // Record errors
+ mPredictionRecorder.record(error);
+ mPredictionDurableRecorder.record(error);
+ } else /* constexpr */ {
+ selectedPredictor->add(timestampNs, pose, twist);
+ }
+
+ // Deliver prediction
+ const int64_t predictionTimeNs = timestampNs + (int64_t)predictionDurationNs;
+ return selectedPredictor->predict(predictionTimeNs);
+}
+
+void PosePredictor::setPosePredictorType(PosePredictorType type) {
+ if (!isValidPosePredictorType(type)) return;
+ if (type == mSetType) return;
+ mSetType = type;
+ if (type == android::media::PosePredictorType::AUTO) {
+ type = android::media::PosePredictorType::LEAST_SQUARES;
+ }
+ if (type != mCurrentType) {
+ mCurrentType = type;
+ if constexpr (!kEnableVerification) {
+ // Verification keeps all predictors up-to-date.
+ // If we don't enable verification, we must reset the current predictor.
+ getCurrentPredictor()->reset();
+ }
+ }
+}
+
+std::string PosePredictor::toString(size_t index) const {
+ std::string prefixSpace(index, ' ');
+ std::string ss(prefixSpace);
+ ss.append("PosePredictor:\n")
+ .append(prefixSpace)
+ .append(" Current Prediction Type: ")
+ .append(android::media::toString(mCurrentType))
+ .append("\n")
+ .append(prefixSpace)
+ .append(" Resets: ")
+ .append(std::to_string(mResets))
+ .append("\n")
+ .append(getCurrentPredictor()->toString(index + 1));
+ if constexpr (kEnableVerification) {
+ // dump verification
+ ss.append(prefixSpace)
+ .append(" Prediction abs error (L1) degrees [ type (last twist least-squares) x ( ");
+ for (size_t i = 0; i < mLookaheadMs.size(); ++i) {
+ if (i > 0) ss.append(" : ");
+ ss.append(std::to_string(mLookaheadMs[i]));
+ }
+ std::vector<float> cumulativeAverageErrors(std::size(mVerifiers));
+ for (size_t i = 0; i < cumulativeAverageErrors.size(); ++i) {
+ cumulativeAverageErrors[i] = mVerifiers[i].cumulativeAverageError();
+ }
+ ss.append(" ) ms ]\n")
+ .append(prefixSpace)
+ .append(" Cumulative Average Error:\n")
+ .append(prefixSpace)
+ .append(" ")
+ .append(VectorRecorder::toString(cumulativeAverageErrors, mDelimiterIdx, "%.3g"))
+ .append("\n")
+ .append(prefixSpace)
+ .append(" PerMinuteHistory:\n")
+ .append(mPredictionDurableRecorder.toString(index + 3))
+ .append(prefixSpace)
+ .append(" PerSecondHistory:\n")
+ .append(mPredictionRecorder.toString(index + 3));
+ }
+ return ss;
+}
+
+std::shared_ptr<PredictorBase> PosePredictor::getCurrentPredictor() const {
+ // we don't use a map here, we look up directly
+ switch (mCurrentType) {
+ default:
+ case android::media::PosePredictorType::LAST:
+ return mPredictors[0];
+ case android::media::PosePredictorType::TWIST:
+ return mPredictors[1];
+ case android::media::PosePredictorType::AUTO: // shouldn't occur here.
+ case android::media::PosePredictorType::LEAST_SQUARES:
+ return mPredictors[2];
+ }
+}
+
+} // namespace android::media
diff --git a/media/libheadtracking/PosePredictor.h b/media/libheadtracking/PosePredictor.h
new file mode 100644
index 0000000..06983cc
--- /dev/null
+++ b/media/libheadtracking/PosePredictor.h
@@ -0,0 +1,203 @@
+/*
+ * Copyright (C) 2023 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#pragma once
+
+#include "PosePredictorVerifier.h"
+#include <memory>
+#include <audio_utils/Statistics.h>
+#include <media/PosePredictorType.h>
+#include <media/Twist.h>
+#include <media/VectorRecorder.h>
+
+namespace android::media {
+
+// Interface for generic pose predictors
+class PredictorBase {
+public:
+ virtual ~PredictorBase() = default;
+ virtual void add(int64_t atNs, const Pose3f& pose, const Twist3f& twist) = 0;
+ virtual Pose3f predict(int64_t atNs) const = 0;
+ virtual void reset() = 0;
+ virtual std::string toString(size_t index) const = 0;
+};
+
+/**
+ * LastPredictor uses the last sample Pose for prediction
+ *
+ * This class is not thread-safe.
+ */
+class LastPredictor : public PredictorBase {
+public:
+ void add(int64_t atNs, const Pose3f& pose, const Twist3f& twist) override {
+ (void)atNs;
+ (void)twist;
+ mLastPose = pose;
+ }
+
+ Pose3f predict(int64_t atNs) const override {
+ (void)atNs;
+ return mLastPose;
+ }
+
+ void reset() override {
+ mLastPose = {};
+ }
+
+ std::string toString(size_t index) const override {
+ std::string s(index, ' ');
+ s.append("LastPredictor using last pose: ")
+ .append(mLastPose.toString())
+ .append("\n");
+ return s;
+ }
+
+private:
+ Pose3f mLastPose;
+};
+
+/**
+ * TwistPredictor uses the last sample Twist and Pose for prediction
+ *
+ * This class is not thread-safe.
+ */
+class TwistPredictor : public PredictorBase {
+public:
+ void add(int64_t atNs, const Pose3f& pose, const Twist3f& twist) override {
+ mLastAtNs = atNs;
+ mLastPose = pose;
+ mLastTwist = twist;
+ }
+
+ Pose3f predict(int64_t atNs) const override {
+ return mLastPose * integrate(mLastTwist, atNs - mLastAtNs);
+ }
+
+ void reset() override {
+ mLastAtNs = {};
+ mLastPose = {};
+ mLastTwist = {};
+ }
+
+ std::string toString(size_t index) const override {
+ std::string s(index, ' ');
+ s.append("TwistPredictor using last pose: ")
+ .append(mLastPose.toString())
+ .append(" last twist: ")
+ .append(mLastTwist.toString())
+ .append("\n");
+ return s;
+ }
+
+private:
+ int64_t mLastAtNs{};
+ Pose3f mLastPose;
+ Twist3f mLastTwist;
+};
+
+
+/**
+ * LeastSquaresPredictor uses the Pose history for prediction.
+ *
+ * A exponential weighted least squares is used.
+ *
+ * This class is not thread-safe.
+ */
+class LeastSquaresPredictor : public PredictorBase {
+public:
+ // alpha is the exponential decay.
+ LeastSquaresPredictor(double alpha = kDefaultAlphaEstimator)
+ : mAlpha(alpha)
+ , mRw(alpha)
+ , mRx(alpha)
+ , mRy(alpha)
+ , mRz(alpha)
+ {}
+
+ void add(int64_t atNs, const Pose3f& pose, const Twist3f& twist) override;
+ Pose3f predict(int64_t atNs) const override;
+ void reset() override;
+ std::string toString(size_t index) const override;
+
+private:
+ const double mAlpha;
+ int64_t mLastAtNs{};
+ Pose3f mLastPose;
+ static constexpr double kDefaultAlphaEstimator = 0.5;
+ static constexpr size_t kMinimumSamplesForPrediction = 4;
+ audio_utils::LinearLeastSquaresFit<double> mRw;
+ audio_utils::LinearLeastSquaresFit<double> mRx;
+ audio_utils::LinearLeastSquaresFit<double> mRy;
+ audio_utils::LinearLeastSquaresFit<double> mRz;
+};
+
+/*
+ * PosePredictor predicts the pose given sensor input at a time in the future.
+ *
+ * This class is not thread safe.
+ */
+class PosePredictor {
+public:
+ PosePredictor();
+
+ Pose3f predict(int64_t timestampNs, const Pose3f& pose, const Twist3f& twist,
+ float predictionDurationNs);
+
+ void setPosePredictorType(PosePredictorType type);
+
+ // convert predictions to a printable string
+ std::string toString(size_t index) const;
+
+private:
+ static constexpr int64_t kMaximumSampleIntervalBeforeResetNs =
+ 300'000'000;
+
+ // Predictors
+ const std::vector<std::shared_ptr<PredictorBase>> mPredictors;
+
+ // Verifiers, create one for an array of future lookaheads for comparison.
+ const std::vector<int> mLookaheadMs;
+
+ std::vector<PosePredictorVerifier> mVerifiers;
+
+ const std::vector<size_t> mDelimiterIdx;
+
+ // Recorders
+ media::VectorRecorder mPredictionRecorder{
+ std::size(mVerifiers) /* vectorSize */, std::chrono::seconds(1), 10 /* maxLogLine */,
+ mDelimiterIdx};
+ media::VectorRecorder mPredictionDurableRecorder{
+ std::size(mVerifiers) /* vectorSize */, std::chrono::minutes(1), 10 /* maxLogLine */,
+ mDelimiterIdx};
+
+ // Status
+
+ // SetType is the externally set predictor type. It may include AUTO.
+ PosePredictorType mSetType = PosePredictorType::LEAST_SQUARES;
+
+ // CurrentType is the actual predictor type used by this class.
+ // It does not include AUTO because that metatype means the class
+ // chooses the best predictor type based on sensor statistics.
+ PosePredictorType mCurrentType = PosePredictorType::LEAST_SQUARES;
+
+ int64_t mResets{};
+ int64_t mLastTimestampNs{};
+
+ // Returns current predictor
+ std::shared_ptr<PredictorBase> getCurrentPredictor() const;
+};
+
+} // namespace android::media
diff --git a/media/libheadtracking/PosePredictorVerifier.h b/media/libheadtracking/PosePredictorVerifier.h
new file mode 100644
index 0000000..6b4a357
--- /dev/null
+++ b/media/libheadtracking/PosePredictorVerifier.h
@@ -0,0 +1,75 @@
+/*
+ * Copyright (C) 2023 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#pragma once
+
+#include <string>
+
+#include <audio_utils/Statistics.h>
+#include <media/Pose.h>
+
+namespace android::media {
+
+/**
+ * PosePredictorVerifier is used to validate predictions
+ *
+ * This class is not thread-safe
+ */
+class PosePredictorVerifier {
+public:
+ std::string toString() const {
+ return mErrorStats.toString();
+ }
+
+ static constexpr int64_t kMillisToNanos = 1000000;
+
+ void verifyActualPose(int64_t timestampNs, const Pose3f& pose) {
+ for (auto it = mPredictions.begin(); it != mPredictions.end();) {
+ if (it->first < timestampNs) {
+ it = mPredictions.erase(it);
+ } else {
+ int64_t dt = it->first - timestampNs;
+ if (std::abs(dt) < 10 * kMillisToNanos) {
+ const float angle = pose.rotation().angularDistance(it->second.rotation());
+ const float error = std::abs(angle); // L1 (absolute difference) here.
+ mLastError = error;
+ mErrorStats.add(error);
+ }
+ break;
+ }
+ }
+ }
+
+ void addPredictedPose(int64_t atNs, const Pose3f& pose) {
+ mPredictions.emplace_back(atNs, pose);
+ }
+
+ float lastError() const {
+ return mLastError;
+ }
+
+ float cumulativeAverageError() const {
+ return mErrorStats.getMean();
+ }
+
+private:
+ static constexpr double kCumulativeErrorAlpha = 0.999;
+ std::deque<std::pair<int64_t, Pose3f>> mPredictions;
+ float mLastError{};
+ android::audio_utils::Statistics<double> mErrorStats{kCumulativeErrorAlpha};
+};
+
+} // namespace androd::media
diff --git a/media/libheadtracking/Twist.cpp b/media/libheadtracking/Twist.cpp
index 63b9e69..fdec694 100644
--- a/media/libheadtracking/Twist.cpp
+++ b/media/libheadtracking/Twist.cpp
@@ -15,7 +15,7 @@
*/
#include "media/Twist.h"
-
+#include <android-base/stringprintf.h>
#include "media/QuaternionUtil.h"
namespace android {
@@ -39,5 +39,11 @@
return os;
}
+std::string Twist3f::toString() const {
+ return base::StringPrintf("[%0.2f, %0.2f, %0.2f, %0.2f, %0.2f, %0.2f]",
+ mTranslationalVelocity[0], mTranslationalVelocity[1], mTranslationalVelocity[2],
+ mRotationalVelocity[0], mRotationalVelocity[1], mRotationalVelocity[2]);
+}
+
} // namespace media
} // namespace android
diff --git a/media/libheadtracking/include/media/HeadTrackingProcessor.h b/media/libheadtracking/include/media/HeadTrackingProcessor.h
index 8ef8ab0..a3c1e97 100644
--- a/media/libheadtracking/include/media/HeadTrackingProcessor.h
+++ b/media/libheadtracking/include/media/HeadTrackingProcessor.h
@@ -19,6 +19,7 @@
#include "HeadTrackingMode.h"
#include "Pose.h"
+#include "PosePredictorType.h"
#include "Twist.h"
namespace android {
@@ -98,6 +99,11 @@
virtual void recenter(bool recenterHead = true, bool recenterScreen = true) = 0;
/**
+ * Set the predictor type.
+ */
+ virtual void setPosePredictorType(PosePredictorType type) = 0;
+
+ /**
* Dump HeadTrackingProcessor parameters under caller lock.
*/
virtual std::string toString_l(unsigned level) const = 0;
diff --git a/media/libheadtracking/include/media/PosePredictorType.h b/media/libheadtracking/include/media/PosePredictorType.h
new file mode 100644
index 0000000..aa76d5d
--- /dev/null
+++ b/media/libheadtracking/include/media/PosePredictorType.h
@@ -0,0 +1,39 @@
+/*
+ * Copyright (C) 2023 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#pragma once
+
+#include <string>
+
+namespace android::media {
+
+enum class PosePredictorType {
+ /** Use best predictor determined from sensor input */
+ AUTO,
+
+ /** Use last pose for future prediction */
+ LAST,
+
+ /** Use twist angular velocity for future prediction */
+ TWIST,
+
+ /** Use weighted least squares history of prior poses (ignoring twist) */
+ LEAST_SQUARES,
+};
+
+std::string toString(PosePredictorType posePredictorType);
+bool isValidPosePredictorType(PosePredictorType posePredictorType);
+
+} // namespace android::media
diff --git a/media/libheadtracking/include/media/Twist.h b/media/libheadtracking/include/media/Twist.h
index 291cea3..51b83d8 100644
--- a/media/libheadtracking/include/media/Twist.h
+++ b/media/libheadtracking/include/media/Twist.h
@@ -66,6 +66,9 @@
return Twist3f(mTranslationalVelocity / s, mRotationalVelocity / s);
}
+ // Convert instance to a string representation.
+ std::string toString() const;
+
private:
Eigen::Vector3f mTranslationalVelocity;
Eigen::Vector3f mRotationalVelocity;
diff --git a/services/audiopolicy/service/Spatializer.cpp b/services/audiopolicy/service/Spatializer.cpp
index d868d31..9ee9037 100644
--- a/services/audiopolicy/service/Spatializer.cpp
+++ b/services/audiopolicy/service/Spatializer.cpp
@@ -808,8 +808,7 @@
}
}
callback = mHeadTrackingCallback;
- mLocalLog.log("%s: %s, spatializerMode %s", __func__, media::toString(mode).c_str(),
- media::toString(spatializerMode).c_str());
+ mLocalLog.log("%s: updating mode to %s", __func__, media::toString(mode).c_str());
}
if (callback != nullptr) {
callback->onHeadTrackingModeChanged(spatializerMode);
diff --git a/services/audiopolicy/service/SpatializerPoseController.cpp b/services/audiopolicy/service/SpatializerPoseController.cpp
index 08e9759..63f53b7 100644
--- a/services/audiopolicy/service/SpatializerPoseController.cpp
+++ b/services/audiopolicy/service/SpatializerPoseController.cpp
@@ -22,6 +22,7 @@
#define LOG_TAG "SpatializerPoseController"
//#define LOG_NDEBUG 0
+#include <cutils/properties.h>
#include <sensor/Sensor.h>
#include <media/MediaMetricsItem.h>
#include <media/QuaternionUtil.h>
@@ -47,11 +48,17 @@
// This is how fast, in rad/s, we allow rotation angle to shift during rate-limiting.
constexpr float kMaxRotationalVelocity = 0.8f;
-// This is how far into the future we predict the head pose, using linear extrapolation based on
-// twist (velocity). It should be set to a value that matches the characteristic durations of moving
-// one's head. The higher we set this, the more latency we are able to reduce, but setting this too
-// high will result in high prediction errors whenever the head accelerates (changes velocity).
-constexpr auto kPredictionDuration = 50ms;
+// This is how far into the future we predict the head pose.
+// The prediction duration should be based on the actual latency from
+// head-tracker to audio output, though setting the prediction duration too
+// high may result in higher prediction errors when the head accelerates or
+// decelerates (changes velocity).
+//
+// The head tracking predictor will do a best effort to achieve the requested
+// prediction duration. If the duration is too far in the future based on
+// current sensor variance, the predictor may internally restrict duration to what
+// is achievable with reasonable confidence as the "best prediction".
+constexpr auto kPredictionDuration = 120ms;
// After not getting a pose sample for this long, we would treat the measurement as stale.
// The max connection interval is 50ms, and HT sensor event interval can differ depending on the
@@ -99,7 +106,15 @@
.maxTranslationalVelocity = kMaxTranslationalVelocity / kTicksPerSecond,
.maxRotationalVelocity = kMaxRotationalVelocity / kTicksPerSecond,
.freshnessTimeout = Ticks(kFreshnessTimeout).count(),
- .predictionDuration = Ticks(kPredictionDuration).count(),
+ .predictionDuration = []() -> float {
+ const int duration_ms =
+ property_get_int32("audio.spatializer.prediction_duration_ms", 0);
+ if (duration_ms > 0) {
+ return duration_ms * 1'000'000LL;
+ } else {
+ return Ticks(kPredictionDuration).count();
+ }
+ }(),
.autoRecenterWindowDuration = Ticks(kAutoRecenterWindowDuration).count(),
.autoRecenterTranslationalThreshold = kAutoRecenterTranslationThreshold,
.autoRecenterRotationalThreshold = kAutoRecenterRotationThreshold,
@@ -147,7 +162,14 @@
mShouldCalculate = false;
}
}
- }) {}
+ }) {
+ const media::PosePredictorType posePredictorType =
+ (media::PosePredictorType)
+ property_get_int32("audio.spatializer.pose_predictor_type", -1);
+ if (isValidPosePredictorType(posePredictorType)) {
+ mProcessor->setPosePredictorType(posePredictorType);
+ }
+ }
SpatializerPoseController::~SpatializerPoseController() {
{