Merge changes I13662df1,I64a194a9 (fff45242) · Commits · e / os / android_frameworks_av

media/libheadtracking/Android.bp

+8 −0

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

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

media/libheadtracking/HeadTrackingProcessor-test.cpp

+2 −0

Original line number	Diff line number	Diff line
		@@ -82,6 +82,8 @@ TEST(HeadTrackingProcessor, Prediction) {
		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());

media/libheadtracking/HeadTrackingProcessor.cpp

+30 −2

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

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

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

		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 @@ class HeadTrackingProcessorImpl : public HeadTrackingProcessor {
		}
		}

		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 @@ class HeadTrackingProcessorImpl : public HeadTrackingProcessor {
		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 @@ class HeadTrackingProcessorImpl : public HeadTrackingProcessor {
		ScreenHeadFusion mScreenHeadFusion;
		ModeSelector mModeSelector;
		PoseRateLimiter mRateLimiter;
		PosePredictor mPosePredictor;
		static constexpr std::size_t mMaxLocalLogLine = 10;
		SimpleLog mLocalLog{mMaxLocalLogLine};
		};
		@@ -230,5 +237,26 @@ std::string toString(HeadTrackingMode mode) {
		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

media/libheadtracking/ModeSelector.cpp

+6 −4

Original line number	Diff line number	Diff line
		@@ -117,10 +117,12 @@ HeadTrackingMode ModeSelector::getActualMode() const {
		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;
		}

media/libheadtracking/PosePredictor.cpp

0 → 100644

+238 −0

Original line number	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