Merge "Head-tracking library for Immersive Audio" into sc-v2-dev (ec7db3cc) · Commits · e / os / android_frameworks_av

media/libheadtracking/Android.bp

0 → 100644

+40 −0

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