Auto-recenter head (44e7c3db) · Commits · e / os / android_frameworks_av

media/libheadtracking/Android.bp

+2 −0

Original line number	Diff line number	Diff line
		@@ -18,6 +18,7 @@ cc_library {
		"PoseRateLimiter.cpp",
		"QuaternionUtil.cpp",
		"ScreenHeadFusion.cpp",
		"StillnessDetector.cpp",
		"Twist.cpp",
		],
		export_include_dirs: [
		@@ -70,6 +71,7 @@ cc_test_host {
		"PoseRateLimiter-test.cpp",
		"QuaternionUtil-test.cpp",
		"ScreenHeadFusion-test.cpp",
		"StillnessDetector-test.cpp",
		"Twist-test.cpp",
		],
		shared_libs: [

media/libheadtracking/HeadTrackingProcessor.cpp

+16 −1

Original line number	Diff line number	Diff line
		@@ -20,6 +20,7 @@
		#include "PoseDriftCompensator.h"
		#include "QuaternionUtil.h"
		#include "ScreenHeadFusion.h"
		#include "StillnessDetector.h"

		namespace android {
		namespace media {
		@@ -40,6 +41,11 @@ class HeadTrackingProcessorImpl : public HeadTrackingProcessor {
		.translationalDriftTimeConstant = options.translationalDriftTimeConstant,
		.rotationalDriftTimeConstant = options.rotationalDriftTimeConstant,
		}),
		mHeadStillnessDetector(StillnessDetector::Options{
		.windowDuration = options.autoRecenterWindowDuration,
		.translationalThreshold = options.autoRecenterTranslationalThreshold,
		.rotationalThreshold = options.autoRecenterRotationalThreshold,
		}),
		mModeSelector(ModeSelector::Options{.freshnessTimeout = options.freshnessTimeout},
		initialMode),
		mRateLimiter(PoseRateLimiter::Options{
		@@ -78,7 +84,14 @@ class HeadTrackingProcessorImpl : public HeadTrackingProcessor {

		void calculate(int64_t timestamp) override {
		if (mWorldToHeadTimestamp.has_value()) {
		const Pose3f worldToHead = mHeadPoseDriftCompensator.getOutput();
		Pose3f worldToHead = mHeadPoseDriftCompensator.getOutput();
		mHeadStillnessDetector.setInput(mWorldToHeadTimestamp.value(), worldToHead);
		// Auto-recenter.
		if (mHeadStillnessDetector.calculate(timestamp)) {
		recenter(true, false);
		worldToHead = mHeadPoseDriftCompensator.getOutput();
		}

		mScreenHeadFusion.setWorldToHeadPose(mWorldToHeadTimestamp.value(), worldToHead);
		mModeSelector.setWorldToHeadPose(mWorldToHeadTimestamp.value(), worldToHead);
		}
		@@ -114,6 +127,7 @@ class HeadTrackingProcessorImpl : public HeadTrackingProcessor {
		void recenter(bool recenterHead, bool recenterScreen) override {
		if (recenterHead) {
		mHeadPoseDriftCompensator.recenter();
		mHeadStillnessDetector.reset();
		}
		if (recenterScreen) {
		mScreenPoseDriftCompensator.recenter();
		@@ -140,6 +154,7 @@ class HeadTrackingProcessorImpl : public HeadTrackingProcessor {
		Pose3f mHeadToStagePose;
		PoseDriftCompensator mHeadPoseDriftCompensator;
		PoseDriftCompensator mScreenPoseDriftCompensator;
		StillnessDetector mHeadStillnessDetector;
		ScreenHeadFusion mScreenHeadFusion;
		ModeSelector mModeSelector;
		PoseRateLimiter mRateLimiter;

media/libheadtracking/StillnessDetector-test.cpp

0 → 100644

+128 −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 <gtest/gtest.h>

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

		namespace android {
		namespace media {
		namespace {

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

		TEST(StillnessDetectorTest, Still) {
		StillnessDetector detector(Options{
		.windowDuration = 1000, .translationalThreshold = 1, .rotationalThreshold = 0.05});

		const Pose3f baseline(Vector3f{1, 2, 3}, Quaternionf::UnitRandom());
		const Pose3f withinThreshold =
		baseline * Pose3f(Vector3f(0.3, -0.3, 0), rotateX(0.01) * rotateY(-0.01));

		EXPECT_FALSE(detector.calculate(0));
		detector.setInput(0, baseline);
		EXPECT_FALSE(detector.calculate(0));
		detector.setInput(300, withinThreshold);
		EXPECT_FALSE(detector.calculate(300));
		detector.setInput(600, baseline);
		EXPECT_FALSE(detector.calculate(600));
		detector.setInput(999, withinThreshold);
		EXPECT_FALSE(detector.calculate(999));
		detector.setInput(1000, baseline);
		EXPECT_TRUE(detector.calculate(1000));
		}

		TEST(StillnessDetectorTest, ZeroDuration) {
		StillnessDetector detector(Options{.windowDuration = 0});
		EXPECT_TRUE(detector.calculate(0));
		EXPECT_TRUE(detector.calculate(1000));
		}

		TEST(StillnessDetectorTest, NotStillTranslation) {
		StillnessDetector detector(Options{
		.windowDuration = 1000, .translationalThreshold = 1, .rotationalThreshold = 0.05});

		const Pose3f baseline(Vector3f{1, 2, 3}, Quaternionf::UnitRandom());
		const Pose3f withinThreshold =
		baseline * Pose3f(Vector3f(0.3, -0.3, 0), rotateX(0.01) * rotateY(-0.01));
		const Pose3f outsideThreshold = baseline * Pose3f(Vector3f(1, 1, 0));

		EXPECT_FALSE(detector.calculate(0));
		detector.setInput(0, baseline);
		EXPECT_FALSE(detector.calculate(0));
		detector.setInput(300, outsideThreshold);
		EXPECT_FALSE(detector.calculate(300));
		detector.setInput(600, baseline);
		EXPECT_FALSE(detector.calculate(600));
		detector.setInput(900, withinThreshold);
		EXPECT_FALSE(detector.calculate(900));
		detector.setInput(1299, baseline);
		EXPECT_FALSE(detector.calculate(1299));
		EXPECT_TRUE(detector.calculate(1300));
		}

		TEST(StillnessDetectorTest, NotStillRotation) {
		StillnessDetector detector(Options{
		.windowDuration = 1000, .translationalThreshold = 1, .rotationalThreshold = 0.05});

		const Pose3f baseline(Vector3f{1, 2, 3}, Quaternionf::UnitRandom());
		const Pose3f withinThreshold =
		baseline * Pose3f(Vector3f(0.3, -0.3, 0), rotateX(0.01) * rotateY(-0.01));
		const Pose3f outsideThreshold = baseline * Pose3f(rotateZ(0.08));
		EXPECT_FALSE(detector.calculate(0));
		detector.setInput(0, baseline);
		EXPECT_FALSE(detector.calculate(0));
		detector.setInput(300, outsideThreshold);
		EXPECT_FALSE(detector.calculate(300));
		detector.setInput(600, baseline);
		EXPECT_FALSE(detector.calculate(600));
		detector.setInput(900, withinThreshold);
		EXPECT_FALSE(detector.calculate(900));
		detector.setInput(1299, baseline);
		EXPECT_FALSE(detector.calculate(1299));
		EXPECT_TRUE(detector.calculate(1300));
		}

		TEST(StillnessDetectorTest, Reset) {
		StillnessDetector detector(Options{
		.windowDuration = 1000, .translationalThreshold = 1, .rotationalThreshold = 0.05});

		const Pose3f baseline(Vector3f{1, 2, 3}, Quaternionf::UnitRandom());
		const Pose3f withinThreshold =
		baseline * Pose3f(Vector3f(0.3, -0.3, 0), rotateX(0.01) * rotateY(-0.01));
		EXPECT_FALSE(detector.calculate(0));
		detector.setInput(0, baseline);
		EXPECT_FALSE(detector.calculate(0));
		detector.reset();
		detector.setInput(600, baseline);
		EXPECT_FALSE(detector.calculate(600));
		detector.setInput(900, withinThreshold);
		EXPECT_FALSE(detector.calculate(900));
		detector.setInput(1200, baseline);
		EXPECT_FALSE(detector.calculate(1200));
		detector.setInput(1599, withinThreshold);
		EXPECT_FALSE(detector.calculate(1599));
		detector.setInput(1600, baseline);
		EXPECT_TRUE(detector.calculate(1600));
		}

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

media/libheadtracking/StillnessDetector.cpp

0 → 100644

+98 −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 "StillnessDetector.h"

		namespace android {
		namespace media {

		StillnessDetector::StillnessDetector(const Options& options) : mOptions(options) {}

		void StillnessDetector::reset() {
		mFifo.clear();
		mWindowFull = false;
		}

		void StillnessDetector::setInput(int64_t timestamp, const Pose3f& input) {
		mFifo.push_back(TimestampedPose{timestamp, input});
		discardOld(timestamp);
		}

		bool StillnessDetector::calculate(int64_t timestamp) {
		discardOld(timestamp);

		// If the window has not been full, we don't consider ourselves still.
		if (!mWindowFull) {
		return false;
		}

		// An empty FIFO and window full is considered still (this will happen in the unlikely case when
		// the window duration is shorter than the gap between samples).
		if (mFifo.empty()) {
		return true;
		}

		// Otherwise, check whether all the poses remaining in the queue are in the proximity of the new
		// one.
		for (auto iter = mFifo.begin(); iter != mFifo.end() - 1; ++iter) {
		const auto& event = *iter;
		if (!areNear(event.pose, mFifo.back().pose)) {
		return false;
		}
		}

		return true;
		}

		void StillnessDetector::discardOld(int64_t timestamp) {
		// Handle the special case of the window duration being zero (always considered full).
		if (mOptions.windowDuration == 0) {
		mFifo.clear();
		mWindowFull = true;
		}

		// Remove any events from the queue that are older than the window. If there were any such
		// events we consider the window full.
		const int64_t windowStart = timestamp - mOptions.windowDuration;
		while (!mFifo.empty() && mFifo.front().timestamp <= windowStart) {
		mWindowFull = true;
		mFifo.pop_front();
		}
		}

		bool StillnessDetector::areNear(const Pose3f& pose1, const Pose3f& pose2) const {
		// Check translation. We use the L1 norm to reduce computational load on expense of accuracy.
		// The L1 norm is an upper bound for the actual (L2) norm, so this approach will err on the side
		// of "not near".
		if ((pose1.translation() - pose2.translation()).lpNorm<1>() >=
		mOptions.translationalThreshold) {
		return false;
		}

		// Check orientation. We use the L1 norm of the imaginary components of the quaternion to reduce
		// computational load on expense of accuracy. For small angles, those components are approx.
		// equal to the angle of rotation and so the norm is approx. the total angle of rotation. The
		// L1 norm is an upper bound, so this approach will err on the side of "not near".
		if ((pose1.rotation().vec() - pose2.rotation().vec()).lpNorm<1>() >=
		mOptions.rotationalThreshold) {
		return false;
		}

		return true;
		}

		} // namespace media
		} // namespace android

media/libheadtracking/StillnessDetector.h

0 → 100644

+94 −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.
		*/
		#pragma once

		#include <deque>

		#include <media/Pose.h>

		namespace android {
		namespace media {

		/**
		* Given a stream of poses, determines if the pose is stable ("still").
		* Stillness is defined as all poses in the recent history ("window") being near the most recent
		* sample.
		*
		* Typical usage:
		*
		* StillnessDetector detector(StilnessDetector::Options{...});
		*
		* while (...) {
		* detector.setInput(timestamp, pose);
		* bool still = detector.calculate(timestamp);
		* }
		*
		* The stream is considered not still until a sufficient number of samples has been provided for an
		* initial fill-up of the window. In the special case of the window size being 0, this is not
		* required and the state is considered always "still". The reset() method can be used to empty the
		* window again and get back to this initial state.
		*/
		class StillnessDetector {
		public:
		/**
		* Configuration options for the detector.
		*/
		struct Options {
		/**
		* How long is the window, in ticks. The special value of 0 indicates that the stream is
		* always considered still.
		*/
		int64_t windowDuration;
		/**
		* How much of a translational deviation from the target (in meters) is considered motion.
		* This is an approximate quantity - the actual threshold might be a little different as we
		* trade-off accuracy with computational efficiency.
		*/
		float translationalThreshold;
		/**
		* How much of a rotational deviation from the target (in radians) is considered motion.
		* This is an approximate quantity - the actual threshold might be a little different as we
		* trade-off accuracy with computational efficiency.
		*/
		float rotationalThreshold;
		};

		/** Ctor. */
		explicit StillnessDetector(const Options& options);

		/** Clear the window. */
		void reset();
		/** Push a new sample. */
		void setInput(int64_t timestamp, const Pose3f& input);
		/** Calculate whether the stream is still at the given timestamp. */
		bool calculate(int64_t timestamp);

		private:
		struct TimestampedPose {
		int64_t timestamp;
		Pose3f pose;
		};

		const Options mOptions;
		std::deque<TimestampedPose> mFifo;
		bool mWindowFull = false;

		bool areNear(const Pose3f& pose1, const Pose3f& pose2) const;
		void discardOld(int64_t timestamp);
		};

		} // namespace media
		} // namespace android