Head-tracking library for Immersive Audio (cbee7d45) · Commits · e / os / android_frameworks_av

media/libheadtracking/Android.bp

0 → 100644

+40 −0

Original line number	Original line	Diff line number	Diff line
			cc_library {
			name: "libheadtracking",
			host_supported: true,
			srcs: [
			"HeadTrackingProcessor.cpp",
			"ModeSelector.cpp",
			"Pose.cpp",
			"PoseDriftCompensator.cpp",
			"PoseRateLimiter.cpp",
			"QuaternionUtil.cpp",
			"ScreenHeadFusion.cpp",
			"Twist.cpp",
			],
			export_include_dirs: [
			"include",
			],
			header_libs: [
			"libeigen",
			],
			export_header_lib_headers: [
			"libeigen",
			],
			}

			cc_test_host {
			name: "libheadtracking-test",
			srcs: [
			"HeadTrackingProcessor-test.cpp",
			"ModeSelector-test.cpp",
			"Pose-test.cpp",
			"PoseDriftCompensator-test.cpp",
			"PoseRateLimiter-test.cpp",
			"QuaternionUtil-test.cpp",
			"ScreenHeadFusion-test.cpp",
			"Twist-test.cpp",
			],
			shared_libs: [
			"libheadtracking",
			],
			}

media/libheadtracking/HeadTrackingProcessor-test.cpp

0 → 100644

+130 −0

Original line number	Original line	Diff line number	Diff line
			/*
			* Copyright (C) 2021 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 "media/HeadTrackingProcessor.h"

			#include <gtest/gtest.h>

			#include "QuaternionUtil.h"
			#include "TestUtil.h"

			namespace android {
			namespace media {
			namespace {

			using Eigen::Quaternionf;
			using Eigen::Vector3f;
			using Options = HeadTrackingProcessor::Options;

			TEST(HeadTrackingProcessor, Initial) {
			for (auto mode : {HeadTrackingMode::STATIC, HeadTrackingMode::WORLD_RELATIVE,
			HeadTrackingMode::SCREEN_RELATIVE}) {
			std::unique_ptr<HeadTrackingProcessor> processor =
			createHeadTrackingProcess(Options{}, mode);
			processor->calculate(0);
			EXPECT_EQ(processor->getActualMode(), HeadTrackingMode::STATIC);
			EXPECT_EQ(processor->getHeadToStagePose(), Pose3f());
			}
			}

			TEST(HeadTrackingProcessor, BasicComposition) {
			const Pose3f worldToHead{{1, 2, 3}, Quaternionf::UnitRandom()};
			const Pose3f worldToScreen{{4, 5, 6}, Quaternionf::UnitRandom()};
			const Pose3f screenToStage{{7, 8, 9}, Quaternionf::UnitRandom()};
			const float physicalToLogical = M_PI_2;

			std::unique_ptr<HeadTrackingProcessor> processor =
			createHeadTrackingProcess(Options{}, HeadTrackingMode::SCREEN_RELATIVE);
			processor->setWorldToHeadPose(0, worldToHead, Twist3f());
			processor->setWorldToScreenPose(0, worldToScreen);
			processor->setScreenToStagePose(screenToStage);
			processor->setDisplayOrientation(physicalToLogical);
			processor->calculate(0);
			ASSERT_EQ(processor->getActualMode(), HeadTrackingMode::SCREEN_RELATIVE);
			EXPECT_EQ(processor->getHeadToStagePose(), worldToHead.inverse() * worldToScreen *
			Pose3f(rotateY(-physicalToLogical)) *
			screenToStage);

			processor->setDesiredMode(HeadTrackingMode::WORLD_RELATIVE);
			processor->calculate(0);
			ASSERT_EQ(processor->getActualMode(), HeadTrackingMode::WORLD_RELATIVE);
			EXPECT_EQ(processor->getHeadToStagePose(), worldToHead.inverse() * screenToStage);

			processor->setDesiredMode(HeadTrackingMode::STATIC);
			processor->calculate(0);
			ASSERT_EQ(processor->getActualMode(), HeadTrackingMode::STATIC);
			EXPECT_EQ(processor->getHeadToStagePose(), screenToStage);
			}

			TEST(HeadTrackingProcessor, Prediction) {
			const Pose3f worldToHead{{1, 2, 3}, Quaternionf::UnitRandom()};
			const Twist3f headTwist{{4, 5, 6}, quaternionToRotationVector(Quaternionf::UnitRandom()) / 10};
			const Pose3f worldToScreen{{4, 5, 6}, Quaternionf::UnitRandom()};

			std::unique_ptr<HeadTrackingProcessor> processor = createHeadTrackingProcess(
			Options{.predictionDuration = 2.f}, HeadTrackingMode::WORLD_RELATIVE);
			processor->setWorldToHeadPose(0, worldToHead, headTwist);
			processor->setWorldToScreenPose(0, worldToScreen);
			processor->calculate(0);
			ASSERT_EQ(processor->getActualMode(), HeadTrackingMode::WORLD_RELATIVE);
			EXPECT_EQ(processor->getHeadToStagePose(), (worldToHead * integrate(headTwist, 2.f)).inverse());

			processor->setDesiredMode(HeadTrackingMode::SCREEN_RELATIVE);
			processor->calculate(0);
			ASSERT_EQ(processor->getActualMode(), HeadTrackingMode::SCREEN_RELATIVE);
			EXPECT_EQ(processor->getHeadToStagePose(),
			(worldToHead * integrate(headTwist, 2.f)).inverse() * worldToScreen);

			processor->setDesiredMode(HeadTrackingMode::STATIC);
			processor->calculate(0);
			ASSERT_EQ(processor->getActualMode(), HeadTrackingMode::STATIC);
			EXPECT_EQ(processor->getHeadToStagePose(), Pose3f());
			}

			TEST(HeadTrackingProcessor, SmoothModeSwitch) {
			const Pose3f targetHeadToWorld = Pose3f({4, 0, 0}, rotateZ(M_PI / 2));

			std::unique_ptr<HeadTrackingProcessor> processor = createHeadTrackingProcess(
			Options{.maxTranslationalVelocity = 1}, HeadTrackingMode::STATIC);

			processor->calculate(0);

			processor->setDesiredMode(HeadTrackingMode::WORLD_RELATIVE);
			processor->setWorldToHeadPose(0, targetHeadToWorld.inverse(), Twist3f());

			// We're expecting a gradual move to the target.
			processor->calculate(0);
			EXPECT_EQ(HeadTrackingMode::WORLD_RELATIVE, processor->getActualMode());
			EXPECT_EQ(processor->getHeadToStagePose(), Pose3f());

			processor->calculate(2);
			EXPECT_EQ(HeadTrackingMode::WORLD_RELATIVE, processor->getActualMode());
			EXPECT_EQ(processor->getHeadToStagePose(), Pose3f({2, 0, 0}, rotateZ(M_PI / 4)));

			processor->calculate(4);
			EXPECT_EQ(HeadTrackingMode::WORLD_RELATIVE, processor->getActualMode());
			EXPECT_EQ(processor->getHeadToStagePose(), targetHeadToWorld);

			// Now that we've reached the target, we should no longer be rate limiting.
			processor->setWorldToHeadPose(4, Pose3f(), Twist3f());
			processor->calculate(5);
			EXPECT_EQ(HeadTrackingMode::WORLD_RELATIVE, processor->getActualMode());
			EXPECT_EQ(processor->getHeadToStagePose(), Pose3f());
			}

			} // namespace
			} // namespace media
			} // namespace android

media/libheadtracking/HeadTrackingProcessor.cpp

0 → 100644

+138 −0

Original line number	Original line	Diff line number	Diff line
			/*
			* Copyright (C) 2021 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 "media/HeadTrackingProcessor.h"

			#include "ModeSelector.h"
			#include "PoseDriftCompensator.h"
			#include "QuaternionUtil.h"
			#include "ScreenHeadFusion.h"

			namespace android {
			namespace media {
			namespace {

			using Eigen::Quaternionf;
			using Eigen::Vector3f;

			class HeadTrackingProcessorImpl : public HeadTrackingProcessor {
			public:
			HeadTrackingProcessorImpl(const Options& options, HeadTrackingMode initialMode)
			: mOptions(options),
			mHeadPoseDriftCompensator(PoseDriftCompensator::Options{
			.translationalDriftTimeConstant = options.translationalDriftTimeConstant,
			.rotationalDriftTimeConstant = options.rotationalDriftTimeConstant,
			}),
			mScreenPoseDriftCompensator(PoseDriftCompensator::Options{
			.translationalDriftTimeConstant = options.translationalDriftTimeConstant,
			.rotationalDriftTimeConstant = options.rotationalDriftTimeConstant,
			}),
			mModeSelector(ModeSelector::Options{.freshnessTimeout = options.freshnessTimeout},
			initialMode),
			mRateLimiter(PoseRateLimiter::Options{
			.maxTranslationalVelocity = options.maxTranslationalVelocity,
			.maxRotationalVelocity = options.maxRotationalVelocity}) {}

			void setDesiredMode(HeadTrackingMode mode) override { mModeSelector.setDesiredMode(mode); }

			void setWorldToHeadPose(int64_t timestamp, const Pose3f& worldToHead,
			const Twist3f& headTwist) override {
			Pose3f predictedWorldToHead =
			worldToHead * integrate(headTwist, mOptions.predictionDuration);
			mHeadPoseDriftCompensator.setInput(timestamp, predictedWorldToHead);
			mWorldToHeadTimestamp = timestamp;
			}

			void setWorldToScreenPose(int64_t timestamp, const Pose3f& worldToScreen) override {
			mScreenPoseDriftCompensator.setInput(timestamp, worldToScreen);
			mWorldToScreenTimestamp = timestamp;
			}

			void setScreenToStagePose(const Pose3f& screenToStage) override {
			mModeSelector.setScreenToStagePose(screenToStage);
			}

			void setDisplayOrientation(float physicalToLogicalAngle) override {
			if (mPhysicalToLogicalAngle != physicalToLogicalAngle) {
			mRateLimiter.enable();
			}
			mPhysicalToLogicalAngle = physicalToLogicalAngle;
			}

			void calculate(int64_t timestamp) override {
			if (mWorldToHeadTimestamp.has_value()) {
			const Pose3f worldToHead = mHeadPoseDriftCompensator.getOutput();
			mScreenHeadFusion.setWorldToHeadPose(mWorldToHeadTimestamp.value(), worldToHead);
			mModeSelector.setWorldToHeadPose(mWorldToHeadTimestamp.value(), worldToHead);
			}

			if (mWorldToScreenTimestamp.has_value()) {
			const Pose3f worldToLogicalScreen = mScreenPoseDriftCompensator.getOutput() *
			Pose3f(rotateY(-mPhysicalToLogicalAngle));
			mScreenHeadFusion.setWorldToScreenPose(mWorldToScreenTimestamp.value(),
			worldToLogicalScreen);
			}

			auto maybeScreenToHead = mScreenHeadFusion.calculate();
			if (maybeScreenToHead.has_value()) {
			mModeSelector.setScreenToHeadPose(maybeScreenToHead->timestamp,
			maybeScreenToHead->pose);
			} else {
			mModeSelector.setScreenToHeadPose(timestamp, std::nullopt);
			}

			HeadTrackingMode prevMode = mModeSelector.getActualMode();
			mModeSelector.calculate(timestamp);
			if (mModeSelector.getActualMode() != prevMode) {
			// Mode has changed, enable rate limiting.
			mRateLimiter.enable();
			}
			mRateLimiter.setTarget(mModeSelector.getHeadToStagePose());
			mHeadToStagePose = mRateLimiter.calculatePose(timestamp);
			}

			Pose3f getHeadToStagePose() const override { return mHeadToStagePose; }

			HeadTrackingMode getActualMode() const override { return mModeSelector.getActualMode(); }

			void recenter() override {
			mHeadPoseDriftCompensator.recenter();
			mScreenPoseDriftCompensator.recenter();
			mRateLimiter.enable();
			}

			private:
			const Options mOptions;
			float mPhysicalToLogicalAngle = 0;
			std::optional<int64_t> mWorldToHeadTimestamp;
			std::optional<int64_t> mWorldToScreenTimestamp;
			Pose3f mHeadToStagePose;
			PoseDriftCompensator mHeadPoseDriftCompensator;
			PoseDriftCompensator mScreenPoseDriftCompensator;
			ScreenHeadFusion mScreenHeadFusion;
			ModeSelector mModeSelector;
			PoseRateLimiter mRateLimiter;
			};

			} // namespace

			std::unique_ptr<HeadTrackingProcessor> createHeadTrackingProcess(
			const HeadTrackingProcessor::Options& options, HeadTrackingMode initialMode) {
			return std::make_unique<HeadTrackingProcessorImpl>(options, initialMode);
			}

			} // namespace media
			} // namespace android

media/libheadtracking/ModeSelector-test.cpp

0 → 100644

+149 −0

Original line number	Original line	Diff line number	Diff line
			/*
			* Copyright (C) 2021 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 "ModeSelector.h"

			#include <gtest/gtest.h>

			#include "QuaternionUtil.h"
			#include "TestUtil.h"

			namespace android {
			namespace media {
			namespace {

			using Eigen::Quaternionf;
			using Eigen::Vector3f;

			TEST(ModeSelector, Initial) {
			ModeSelector::Options options;
			ModeSelector selector(options);

			selector.calculate(0);
			EXPECT_EQ(HeadTrackingMode::STATIC, selector.getActualMode());
			EXPECT_EQ(selector.getHeadToStagePose(), Pose3f());
			}

			TEST(ModeSelector, InitialWorldRelative) {
			const Pose3f worldToHead({1, 2, 3}, Quaternionf::UnitRandom());

			ModeSelector::Options options;
			ModeSelector selector(options, HeadTrackingMode::WORLD_RELATIVE);

			selector.setWorldToHeadPose(0, worldToHead);
			selector.calculate(0);
			EXPECT_EQ(HeadTrackingMode::WORLD_RELATIVE, selector.getActualMode());
			EXPECT_EQ(selector.getHeadToStagePose(), worldToHead.inverse());
			}

			TEST(ModeSelector, InitialScreenRelative) {
			const Pose3f screenToHead({1, 2, 3}, Quaternionf::UnitRandom());

			ModeSelector::Options options;
			ModeSelector selector(options, HeadTrackingMode::SCREEN_RELATIVE);

			selector.setScreenToHeadPose(0, screenToHead);
			selector.calculate(0);
			EXPECT_EQ(HeadTrackingMode::SCREEN_RELATIVE, selector.getActualMode());
			EXPECT_EQ(selector.getHeadToStagePose(), screenToHead.inverse());
			}

			TEST(ModeSelector, WorldRelative) {
			const Pose3f worldToHead({1, 2, 3}, Quaternionf::UnitRandom());
			const Pose3f screenToStage({4, 5, 6}, Quaternionf::UnitRandom());

			ModeSelector::Options options;
			ModeSelector selector(options);

			selector.setScreenToStagePose(screenToStage);

			selector.setDesiredMode(HeadTrackingMode::WORLD_RELATIVE);
			selector.setWorldToHeadPose(0, worldToHead);
			selector.calculate(0);
			EXPECT_EQ(HeadTrackingMode::WORLD_RELATIVE, selector.getActualMode());
			EXPECT_EQ(selector.getHeadToStagePose(), worldToHead.inverse() * screenToStage);
			}

			TEST(ModeSelector, WorldRelativeStale) {
			const Pose3f worldToHead({1, 2, 3}, Quaternionf::UnitRandom());
			const Pose3f screenToStage({4, 5, 6}, Quaternionf::UnitRandom());

			ModeSelector::Options options{.freshnessTimeout = 100};
			ModeSelector selector(options);

			selector.setScreenToStagePose(screenToStage);

			selector.setDesiredMode(HeadTrackingMode::WORLD_RELATIVE);
			selector.setWorldToHeadPose(0, worldToHead);
			selector.calculate(101);
			EXPECT_EQ(HeadTrackingMode::STATIC, selector.getActualMode());
			EXPECT_EQ(selector.getHeadToStagePose(), screenToStage);
			}

			TEST(ModeSelector, ScreenRelative) {
			const Pose3f screenToHead({1, 2, 3}, Quaternionf::UnitRandom());
			const Pose3f screenToStage({4, 5, 6}, Quaternionf::UnitRandom());

			ModeSelector::Options options;
			ModeSelector selector(options);

			selector.setScreenToStagePose(screenToStage);

			selector.setDesiredMode(HeadTrackingMode::SCREEN_RELATIVE);
			selector.setScreenToHeadPose(0, screenToHead);
			selector.calculate(0);
			EXPECT_EQ(HeadTrackingMode::SCREEN_RELATIVE, selector.getActualMode());
			EXPECT_EQ(selector.getHeadToStagePose(), screenToHead.inverse() * screenToStage);
			}

			TEST(ModeSelector, ScreenRelativeStaleToWorldRelative) {
			const Pose3f screenToHead({1, 2, 3}, Quaternionf::UnitRandom());
			const Pose3f screenToStage({4, 5, 6}, Quaternionf::UnitRandom());
			const Pose3f worldToHead({7, 8, 9}, Quaternionf::UnitRandom());

			ModeSelector::Options options{.freshnessTimeout = 100};
			ModeSelector selector(options);

			selector.setScreenToStagePose(screenToStage);

			selector.setDesiredMode(HeadTrackingMode::SCREEN_RELATIVE);
			selector.setScreenToHeadPose(0, screenToHead);
			selector.setWorldToHeadPose(50, worldToHead);
			selector.calculate(101);
			EXPECT_EQ(HeadTrackingMode::WORLD_RELATIVE, selector.getActualMode());
			EXPECT_EQ(selector.getHeadToStagePose(), worldToHead.inverse() * screenToStage);
			}

			TEST(ModeSelector, ScreenRelativeInvalidToWorldRelative) {
			const Pose3f screenToStage({4, 5, 6}, Quaternionf::UnitRandom());
			const Pose3f worldToHead({7, 8, 9}, Quaternionf::UnitRandom());

			ModeSelector::Options options;
			ModeSelector selector(options);

			selector.setScreenToStagePose(screenToStage);

			selector.setDesiredMode(HeadTrackingMode::SCREEN_RELATIVE);
			selector.setScreenToHeadPose(50, std::nullopt);
			selector.setWorldToHeadPose(50, worldToHead);
			selector.calculate(101);
			EXPECT_EQ(HeadTrackingMode::WORLD_RELATIVE, selector.getActualMode());
			EXPECT_EQ(selector.getHeadToStagePose(), worldToHead.inverse() * screenToStage);
			}

			} // namespace
			} // namespace media
			} // namespace android

media/libheadtracking/ModeSelector.cpp

0 → 100644

+96 −0

Original line number	Original line	Diff line number	Diff line
			/*
			* Copyright (C) 2021 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 "ModeSelector.h"

			namespace android {
			namespace media {

			ModeSelector::ModeSelector(const Options& options, HeadTrackingMode initialMode)
			: mOptions(options), mDesiredMode(initialMode), mActualMode(initialMode) {}

			void ModeSelector::setDesiredMode(HeadTrackingMode mode) {
			mDesiredMode = mode;
			}

			void ModeSelector::setScreenToStagePose(const Pose3f& screenToStage) {
			mScreenToStage = screenToStage;
			}

			void ModeSelector::setScreenToHeadPose(int64_t timestamp,
			const std::optional<Pose3f>& screenToHead) {
			mScreenToHead = screenToHead;
			mScreenToHeadTimestamp = timestamp;
			}

			void ModeSelector::setWorldToHeadPose(int64_t timestamp, const Pose3f& worldToHead) {
			mWorldToHead = worldToHead;
			mWorldToHeadTimestamp = timestamp;
			}

			void ModeSelector::calculateActualMode(int64_t timestamp) {
			bool isValidScreenToHead = mScreenToHead.has_value() &&
			timestamp - mScreenToHeadTimestamp < mOptions.freshnessTimeout;
			bool isValidWorldToHead = mWorldToHead.has_value() &&
			timestamp - mWorldToHeadTimestamp < mOptions.freshnessTimeout;

			HeadTrackingMode mode = mDesiredMode;

			// Optional downgrade from screen-relative to world-relative.
			if (mode == HeadTrackingMode::SCREEN_RELATIVE) {
			if (!isValidScreenToHead) {
			mode = HeadTrackingMode::WORLD_RELATIVE;
			}
			}

			// Optional downgrade from world-relative to static.
			if (mode == HeadTrackingMode::WORLD_RELATIVE) {
			if (!isValidWorldToHead) {
			mode = HeadTrackingMode::STATIC;
			}
			}

			mActualMode = mode;
			}

			void ModeSelector::calculate(int64_t timestamp) {
			calculateActualMode(timestamp);

			switch (mActualMode) {
			case HeadTrackingMode::STATIC:
			mHeadToStage = mScreenToStage;
			break;

			case HeadTrackingMode::WORLD_RELATIVE:
			mHeadToStage = mWorldToHead.value().inverse() * mScreenToStage;
			break;

			case HeadTrackingMode::SCREEN_RELATIVE:
			mHeadToStage = mScreenToHead.value().inverse() * mScreenToStage;
			break;
			}
			}

			Pose3f ModeSelector::getHeadToStagePose() const {
			return mHeadToStage;
			}

			HeadTrackingMode ModeSelector::getActualMode() const {
			return mActualMode;
			}

			} // namespace media
			} // namespace android