Spatial Audio: Improve Head-Tracking prediction (8702c31c) · Commits · e / os / android_frameworks_av

media/libheadtracking/Android.bp

+8 −0

Original line number	Original line	Diff line number	Diff line
	@@ -16,6 +16,7 @@ cc_library {
	"Pose.cpp",		"Pose.cpp",
	"PoseBias.cpp",		"PoseBias.cpp",
	"PoseDriftCompensator.cpp",		"PoseDriftCompensator.cpp",
			"PosePredictor.cpp",
	"PoseRateLimiter.cpp",		"PoseRateLimiter.cpp",
	"QuaternionUtil.cpp",		"QuaternionUtil.cpp",
	"ScreenHeadFusion.cpp",		"ScreenHeadFusion.cpp",
	@@ -39,6 +40,12 @@ cc_library {
	cflags: [		cflags: [
	"-Wthread-safety",		"-Wthread-safety",
	],		],
			product_variables: {
			debuggable: {
			// enable experiments only in userdebug and eng builds
			cflags: ["-DENABLE_VERIFICATION"],
			},
			},
	}		}

	cc_library {		cc_library {
	@@ -80,6 +87,7 @@ cc_test_host {
	"Pose-test.cpp",		"Pose-test.cpp",
	"PoseBias-test.cpp",		"PoseBias-test.cpp",
	"PoseDriftCompensator-test.cpp",		"PoseDriftCompensator-test.cpp",
			"PosePredictor.cpp",
	"PoseRateLimiter-test.cpp",		"PoseRateLimiter-test.cpp",
	"QuaternionUtil-test.cpp",		"QuaternionUtil-test.cpp",
	"ScreenHeadFusion-test.cpp",		"ScreenHeadFusion-test.cpp",

media/libheadtracking/HeadTrackingProcessor-test.cpp

+2 −0

Original line number	Original line	Diff line number	Diff line
	@@ -82,6 +82,8 @@ TEST(HeadTrackingProcessor, Prediction) {
	std::unique_ptr<HeadTrackingProcessor> processor = createHeadTrackingProcessor(		std::unique_ptr<HeadTrackingProcessor> processor = createHeadTrackingProcessor(
	Options{.predictionDuration = 2.f}, HeadTrackingMode::WORLD_RELATIVE);		Options{.predictionDuration = 2.f}, HeadTrackingMode::WORLD_RELATIVE);

			processor->setPosePredictorType(PosePredictorType::TWIST);

	// Establish a baseline for the drift compensators.		// Establish a baseline for the drift compensators.
	processor->setWorldToHeadPose(0, Pose3f(), Twist3f());		processor->setWorldToHeadPose(0, Pose3f(), Twist3f());
	processor->setWorldToScreenPose(0, Pose3f());		processor->setWorldToScreenPose(0, Pose3f());

media/libheadtracking/HeadTrackingProcessor.cpp

+30 −2

Original line number	Original line	Diff line number	Diff line
	@@ -22,6 +22,7 @@

	#include "ModeSelector.h"		#include "ModeSelector.h"
	#include "PoseBias.h"		#include "PoseBias.h"
			#include "PosePredictor.h"
	#include "ScreenHeadFusion.h"		#include "ScreenHeadFusion.h"
	#include "StillnessDetector.h"		#include "StillnessDetector.h"

	@@ -59,8 +60,8 @@ class HeadTrackingProcessorImpl : public HeadTrackingProcessor {

	void setWorldToHeadPose(int64_t timestamp, const Pose3f& worldToHead,		void setWorldToHeadPose(int64_t timestamp, const Pose3f& worldToHead,
	const Twist3f& headTwist) override {		const Twist3f& headTwist) override {
	Pose3f predictedWorldToHead =		const Pose3f predictedWorldToHead = mPosePredictor.predict(
	worldToHead * integrate(headTwist, mOptions.predictionDuration);		timestamp, worldToHead, headTwist, mOptions.predictionDuration);
	mHeadPoseBias.setInput(predictedWorldToHead);		mHeadPoseBias.setInput(predictedWorldToHead);
	mHeadStillnessDetector.setInput(timestamp, predictedWorldToHead);		mHeadStillnessDetector.setInput(timestamp, predictedWorldToHead);
	mWorldToHeadTimestamp = timestamp;		mWorldToHeadTimestamp = timestamp;
	@@ -161,6 +162,10 @@ class HeadTrackingProcessorImpl : public HeadTrackingProcessor {
	}		}
	}		}

			void setPosePredictorType(PosePredictorType type) override {
			mPosePredictor.setPosePredictorType(type);
			}

	std::string toString_l(unsigned level) const override {		std::string toString_l(unsigned level) const override {
	std::string prefixSpace(level, ' ');		std::string prefixSpace(level, ' ');
	std::string ss = prefixSpace + "HeadTrackingProcessor:\n";		std::string ss = prefixSpace + "HeadTrackingProcessor:\n";
	@@ -186,6 +191,7 @@ class HeadTrackingProcessorImpl : public HeadTrackingProcessor {
	prefixSpace.c_str(), mOptions.screenStillnessRotationalThreshold);		prefixSpace.c_str(), mOptions.screenStillnessRotationalThreshold);
	ss += mModeSelector.toString(level + 1);		ss += mModeSelector.toString(level + 1);
	ss += mRateLimiter.toString(level + 1);		ss += mRateLimiter.toString(level + 1);
			ss += mPosePredictor.toString(level + 1);
	ss.append(prefixSpace + "ReCenterHistory:\n");		ss.append(prefixSpace + "ReCenterHistory:\n");
	ss += mLocalLog.dumpToString((prefixSpace + " ").c_str(), mMaxLocalLogLine);		ss += mLocalLog.dumpToString((prefixSpace + " ").c_str(), mMaxLocalLogLine);
	return ss;		return ss;
	@@ -207,6 +213,7 @@ class HeadTrackingProcessorImpl : public HeadTrackingProcessor {
	ScreenHeadFusion mScreenHeadFusion;		ScreenHeadFusion mScreenHeadFusion;
	ModeSelector mModeSelector;		ModeSelector mModeSelector;
	PoseRateLimiter mRateLimiter;		PoseRateLimiter mRateLimiter;
			PosePredictor mPosePredictor;
	static constexpr std::size_t mMaxLocalLogLine = 10;		static constexpr std::size_t mMaxLocalLogLine = 10;
	SimpleLog mLocalLog{mMaxLocalLogLine};		SimpleLog mLocalLog{mMaxLocalLogLine};
	};		};
	@@ -230,5 +237,26 @@ std::string toString(HeadTrackingMode mode) {
	return "EnumNotImplemented";		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 media
	} // namespace android		} // namespace android

media/libheadtracking/ModeSelector.cpp

+6 −4

Original line number	Original line	Diff line number	Diff line
	@@ -117,10 +117,12 @@ HeadTrackingMode ModeSelector::getActualMode() const {
	std::string ModeSelector::toString(unsigned level) const {		std::string ModeSelector::toString(unsigned level) const {
	std::string prefixSpace(level, ' ');		std::string prefixSpace(level, ' ');
	std::string ss(prefixSpace);		std::string ss(prefixSpace);
	StringAppendF(&ss, "ModeSelector: ScreenToStage %s\n",		ss.append("ModeSelector: ScreenToStage ")
	mScreenToStage.toString().c_str());		.append(mScreenToStage.toString())
	ss.append(prefixSpace + "Mode downgrade history:\n");		.append("\n")
	ss += mLocalLog.dumpToString((prefixSpace + " ").c_str(), sMaxLocalLogLine);		.append(prefixSpace)
			.append("Mode change history:\n")
			.append(mLocalLog.dumpToString((prefixSpace + " ").c_str(), sMaxLocalLogLine));
	return ss;		return ss;
	}		}

media/libheadtracking/PosePredictor.cpp

0 → 100644

+238 −0

Original line number	Original line	Diff line number	Diff line
			/*
			* 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