1 /*
2 * Copyright (C) 2021 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License"){}
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 #include "media/HeadTrackingProcessor.h"
18
19 #include "ModeSelector.h"
20 #include "PoseDriftCompensator.h"
21 #include "QuaternionUtil.h"
22 #include "ScreenHeadFusion.h"
23
24 namespace android {
25 namespace media {
26 namespace {
27
28 using Eigen::Quaternionf;
29 using Eigen::Vector3f;
30
31 class HeadTrackingProcessorImpl : public HeadTrackingProcessor {
32 public:
HeadTrackingProcessorImpl(const Options & options,HeadTrackingMode initialMode)33 HeadTrackingProcessorImpl(const Options& options, HeadTrackingMode initialMode)
34 : mOptions(options),
35 mHeadPoseDriftCompensator(PoseDriftCompensator::Options{
36 .translationalDriftTimeConstant = options.translationalDriftTimeConstant,
37 .rotationalDriftTimeConstant = options.rotationalDriftTimeConstant,
38 }),
39 mScreenPoseDriftCompensator(PoseDriftCompensator::Options{
40 .translationalDriftTimeConstant = options.translationalDriftTimeConstant,
41 .rotationalDriftTimeConstant = options.rotationalDriftTimeConstant,
42 }),
43 mModeSelector(ModeSelector::Options{.freshnessTimeout = options.freshnessTimeout},
44 initialMode),
45 mRateLimiter(PoseRateLimiter::Options{
46 .maxTranslationalVelocity = options.maxTranslationalVelocity,
47 .maxRotationalVelocity = options.maxRotationalVelocity}) {}
48
setDesiredMode(HeadTrackingMode mode)49 void setDesiredMode(HeadTrackingMode mode) override { mModeSelector.setDesiredMode(mode); }
50
setWorldToHeadPose(int64_t timestamp,const Pose3f & worldToHead,const Twist3f & headTwist)51 void setWorldToHeadPose(int64_t timestamp, const Pose3f& worldToHead,
52 const Twist3f& headTwist) override {
53 Pose3f predictedWorldToHead =
54 worldToHead * integrate(headTwist, mOptions.predictionDuration);
55 mHeadPoseDriftCompensator.setInput(timestamp, predictedWorldToHead);
56 mWorldToHeadTimestamp = timestamp;
57 }
58
setWorldToScreenPose(int64_t timestamp,const Pose3f & worldToScreen)59 void setWorldToScreenPose(int64_t timestamp, const Pose3f& worldToScreen) override {
60 if (mPhysicalToLogicalAngle != mPendingPhysicalToLogicalAngle) {
61 // We're introducing an artificial discontinuity. Enable the rate limiter.
62 mRateLimiter.enable();
63 mPhysicalToLogicalAngle = mPendingPhysicalToLogicalAngle;
64 }
65
66 mScreenPoseDriftCompensator.setInput(
67 timestamp, worldToScreen * Pose3f(rotateY(-mPhysicalToLogicalAngle)));
68 mWorldToScreenTimestamp = timestamp;
69 }
70
setScreenToStagePose(const Pose3f & screenToStage)71 void setScreenToStagePose(const Pose3f& screenToStage) override {
72 mModeSelector.setScreenToStagePose(screenToStage);
73 }
74
setDisplayOrientation(float physicalToLogicalAngle)75 void setDisplayOrientation(float physicalToLogicalAngle) override {
76 mPendingPhysicalToLogicalAngle = physicalToLogicalAngle;
77 }
78
calculate(int64_t timestamp)79 void calculate(int64_t timestamp) override {
80 if (mWorldToHeadTimestamp.has_value()) {
81 const Pose3f worldToHead = mHeadPoseDriftCompensator.getOutput();
82 mScreenHeadFusion.setWorldToHeadPose(mWorldToHeadTimestamp.value(), worldToHead);
83 mModeSelector.setWorldToHeadPose(mWorldToHeadTimestamp.value(), worldToHead);
84 }
85
86 if (mWorldToScreenTimestamp.has_value()) {
87 const Pose3f worldToLogicalScreen = mScreenPoseDriftCompensator.getOutput();
88 mScreenHeadFusion.setWorldToScreenPose(mWorldToScreenTimestamp.value(),
89 worldToLogicalScreen);
90 }
91
92 auto maybeScreenToHead = mScreenHeadFusion.calculate();
93 if (maybeScreenToHead.has_value()) {
94 mModeSelector.setScreenToHeadPose(maybeScreenToHead->timestamp,
95 maybeScreenToHead->pose);
96 } else {
97 mModeSelector.setScreenToHeadPose(timestamp, std::nullopt);
98 }
99
100 HeadTrackingMode prevMode = mModeSelector.getActualMode();
101 mModeSelector.calculate(timestamp);
102 if (mModeSelector.getActualMode() != prevMode) {
103 // Mode has changed, enable rate limiting.
104 mRateLimiter.enable();
105 }
106 mRateLimiter.setTarget(mModeSelector.getHeadToStagePose());
107 mHeadToStagePose = mRateLimiter.calculatePose(timestamp);
108 }
109
getHeadToStagePose() const110 Pose3f getHeadToStagePose() const override { return mHeadToStagePose; }
111
getActualMode() const112 HeadTrackingMode getActualMode() const override { return mModeSelector.getActualMode(); }
113
recenter(bool recenterHead,bool recenterScreen)114 void recenter(bool recenterHead, bool recenterScreen) override {
115 if (recenterHead) {
116 mHeadPoseDriftCompensator.recenter();
117 }
118 if (recenterScreen) {
119 mScreenPoseDriftCompensator.recenter();
120 }
121
122 // If a sensor being recentered is included in the current mode, apply rate limiting to
123 // avoid discontinuities.
124 HeadTrackingMode mode = mModeSelector.getActualMode();
125 if ((recenterHead && (mode == HeadTrackingMode::WORLD_RELATIVE ||
126 mode == HeadTrackingMode::SCREEN_RELATIVE)) ||
127 (recenterScreen && mode == HeadTrackingMode::SCREEN_RELATIVE)) {
128 mRateLimiter.enable();
129 }
130 }
131
132 private:
133 const Options mOptions;
134 float mPhysicalToLogicalAngle = 0;
135 // We store the physical to logical angle as "pending" until the next world-to-screen sample it
136 // applies to arrives.
137 float mPendingPhysicalToLogicalAngle = 0;
138 std::optional<int64_t> mWorldToHeadTimestamp;
139 std::optional<int64_t> mWorldToScreenTimestamp;
140 Pose3f mHeadToStagePose;
141 PoseDriftCompensator mHeadPoseDriftCompensator;
142 PoseDriftCompensator mScreenPoseDriftCompensator;
143 ScreenHeadFusion mScreenHeadFusion;
144 ModeSelector mModeSelector;
145 PoseRateLimiter mRateLimiter;
146 };
147
148 } // namespace
149
createHeadTrackingProcessor(const HeadTrackingProcessor::Options & options,HeadTrackingMode initialMode)150 std::unique_ptr<HeadTrackingProcessor> createHeadTrackingProcessor(
151 const HeadTrackingProcessor::Options& options, HeadTrackingMode initialMode) {
152 return std::make_unique<HeadTrackingProcessorImpl>(options, initialMode);
153 }
154
155 } // namespace media
156 } // namespace android
157