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/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