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 #include <inttypes.h>
17
18 #include <android-base/stringprintf.h>
19 #include <audio_utils/SimpleLog.h>
20 #include "media/HeadTrackingProcessor.h"
21 #include "media/QuaternionUtil.h"
22
23 #include "ModeSelector.h"
24 #include "PoseBias.h"
25 #include "PosePredictor.h"
26 #include "ScreenHeadFusion.h"
27 #include "StillnessDetector.h"
28
29 namespace android {
30 namespace media {
31 namespace {
32
33 using android::base::StringAppendF;
34 using Eigen::Quaternionf;
35 using Eigen::Vector3f;
36
37 class HeadTrackingProcessorImpl : public HeadTrackingProcessor {
38 public:
HeadTrackingProcessorImpl(const Options & options,HeadTrackingMode initialMode)39 HeadTrackingProcessorImpl(const Options& options, HeadTrackingMode initialMode)
40 : mOptions(options),
41 mHeadStillnessDetector(StillnessDetector::Options{
42 .defaultValue = false,
43 .windowDuration = options.autoRecenterWindowDuration,
44 .translationalThreshold = options.autoRecenterTranslationalThreshold,
45 .rotationalThreshold = options.autoRecenterRotationalThreshold,
46 }),
47 mScreenStillnessDetector(StillnessDetector::Options{
48 .defaultValue = true,
49 .windowDuration = options.screenStillnessWindowDuration,
50 .translationalThreshold = options.screenStillnessTranslationalThreshold,
51 .rotationalThreshold = options.screenStillnessRotationalThreshold,
52 }),
53 mModeSelector(ModeSelector::Options{.freshnessTimeout = options.freshnessTimeout},
54 initialMode),
55 mRateLimiter(PoseRateLimiter::Options{
56 .maxTranslationalVelocity = options.maxTranslationalVelocity,
57 .maxRotationalVelocity = options.maxRotationalVelocity}) {}
58
setDesiredMode(HeadTrackingMode mode)59 void setDesiredMode(HeadTrackingMode mode) override { mModeSelector.setDesiredMode(mode); }
60
setWorldToHeadPose(int64_t timestamp,const Pose3f & worldToHead,const Twist3f & headTwist)61 void setWorldToHeadPose(int64_t timestamp, const Pose3f& worldToHead,
62 const Twist3f& headTwist) override {
63 const Pose3f predictedWorldToHead = mPosePredictor.predict(
64 timestamp, worldToHead, headTwist, mOptions.predictionDuration);
65 mHeadPoseBias.setInput(predictedWorldToHead);
66 mHeadStillnessDetector.setInput(timestamp, predictedWorldToHead);
67 mWorldToHeadTimestamp = timestamp;
68 }
69
setWorldToScreenPose(int64_t timestamp,const Pose3f & worldToScreen)70 void setWorldToScreenPose(int64_t timestamp, const Pose3f& worldToScreen) override {
71 if (mPhysicalToLogicalAngle != mPendingPhysicalToLogicalAngle) {
72 // We're introducing an artificial discontinuity. Enable the rate limiter.
73 mRateLimiter.enable();
74 mPhysicalToLogicalAngle = mPendingPhysicalToLogicalAngle;
75 }
76
77 Pose3f worldToLogicalScreen = worldToScreen * Pose3f(rotateY(-mPhysicalToLogicalAngle));
78 mScreenPoseBias.setInput(worldToLogicalScreen);
79 mScreenStillnessDetector.setInput(timestamp, worldToLogicalScreen);
80 mWorldToScreenTimestamp = timestamp;
81 }
82
setScreenToStagePose(const Pose3f & screenToStage)83 void setScreenToStagePose(const Pose3f& screenToStage) override {
84 mModeSelector.setScreenToStagePose(screenToStage);
85 }
86
setDisplayOrientation(float physicalToLogicalAngle)87 void setDisplayOrientation(float physicalToLogicalAngle) override {
88 mPendingPhysicalToLogicalAngle = physicalToLogicalAngle;
89 }
90
calculate(int64_t timestamp)91 void calculate(int64_t timestamp) override {
92 bool screenStable = true;
93
94 // Handle the screen first, since it might: trigger a recentering of the head.
95 if (mWorldToScreenTimestamp.has_value()) {
96 const Pose3f worldToLogicalScreen = mScreenPoseBias.getOutput();
97 screenStable = mScreenStillnessDetector.calculate(timestamp);
98 mModeSelector.setScreenStable(mWorldToScreenTimestamp.value(), screenStable);
99 // Whenever the screen is unstable, recenter the head pose.
100 if (!screenStable) {
101 recenter(true, false, "calculate: screen movement");
102 }
103 mScreenHeadFusion.setWorldToScreenPose(mWorldToScreenTimestamp.value(),
104 worldToLogicalScreen);
105 }
106
107 // Handle head.
108 if (mWorldToHeadTimestamp.has_value()) {
109 Pose3f worldToHead = mHeadPoseBias.getOutput();
110 // Auto-recenter.
111 bool headStable = mHeadStillnessDetector.calculate(timestamp);
112 if (headStable || !screenStable) {
113 recenter(true, false, "calculate: head movement");
114 worldToHead = mHeadPoseBias.getOutput();
115 }
116
117 mScreenHeadFusion.setWorldToHeadPose(mWorldToHeadTimestamp.value(), worldToHead);
118 mModeSelector.setWorldToHeadPose(mWorldToHeadTimestamp.value(), worldToHead);
119 }
120
121 auto maybeScreenToHead = mScreenHeadFusion.calculate();
122 if (maybeScreenToHead.has_value()) {
123 mModeSelector.setScreenToHeadPose(maybeScreenToHead->timestamp,
124 maybeScreenToHead->pose);
125 } else {
126 mModeSelector.setScreenToHeadPose(timestamp, std::nullopt);
127 }
128
129 HeadTrackingMode prevMode = mModeSelector.getActualMode();
130 mModeSelector.calculate(timestamp);
131 if (mModeSelector.getActualMode() != prevMode) {
132 // Mode has changed, enable rate limiting.
133 mRateLimiter.enable();
134 }
135 mRateLimiter.setTarget(mModeSelector.getHeadToStagePose());
136 mHeadToStagePose = mRateLimiter.calculatePose(timestamp);
137 }
138
getHeadToStagePose() const139 Pose3f getHeadToStagePose() const override { return mHeadToStagePose; }
140
getActualMode() const141 HeadTrackingMode getActualMode() const override { return mModeSelector.getActualMode(); }
142
recenter(bool recenterHead,bool recenterScreen,std::string source)143 void recenter(bool recenterHead, bool recenterScreen, std::string source) override {
144 if (recenterHead) {
145 mHeadPoseBias.recenter();
146 mHeadStillnessDetector.reset();
147 mLocalLog.log("recenter Head from %s", source.c_str());
148 }
149 if (recenterScreen) {
150 mScreenPoseBias.recenter();
151 mScreenStillnessDetector.reset();
152 mLocalLog.log("recenter Screen from %s", source.c_str());
153 }
154
155 // If a sensor being recentered is included in the current mode, apply rate limiting to
156 // avoid discontinuities.
157 HeadTrackingMode mode = mModeSelector.getActualMode();
158 if ((recenterHead && (mode == HeadTrackingMode::WORLD_RELATIVE ||
159 mode == HeadTrackingMode::SCREEN_RELATIVE)) ||
160 (recenterScreen && mode == HeadTrackingMode::SCREEN_RELATIVE)) {
161 mRateLimiter.enable();
162 }
163 }
164
setPosePredictorType(PosePredictorType type)165 void setPosePredictorType(PosePredictorType type) override {
166 mPosePredictor.setPosePredictorType(type);
167 }
168
toString_l(unsigned level) const169 std::string toString_l(unsigned level) const override {
170 std::string prefixSpace(level, ' ');
171 std::string ss = prefixSpace + "HeadTrackingProcessor:\n";
172 StringAppendF(&ss, "%s maxTranslationalVelocity: %f meter/second\n", prefixSpace.c_str(),
173 mOptions.maxTranslationalVelocity);
174 StringAppendF(&ss, "%s maxRotationalVelocity: %f rad/second\n", prefixSpace.c_str(),
175 mOptions.maxRotationalVelocity);
176 StringAppendF(&ss, "%s freshnessTimeout: %0.4f ms\n", prefixSpace.c_str(),
177 media::nsToFloatMs(mOptions.freshnessTimeout));
178 StringAppendF(&ss, "%s predictionDuration: %0.4f ms\n", prefixSpace.c_str(),
179 media::nsToFloatMs(mOptions.predictionDuration));
180 StringAppendF(&ss, "%s autoRecenterWindowDuration: %0.4f ms\n", prefixSpace.c_str(),
181 media::nsToFloatMs(mOptions.autoRecenterWindowDuration));
182 StringAppendF(&ss, "%s autoRecenterTranslationalThreshold: %f meter\n", prefixSpace.c_str(),
183 mOptions.autoRecenterTranslationalThreshold);
184 StringAppendF(&ss, "%s autoRecenterRotationalThreshold: %f radians\n", prefixSpace.c_str(),
185 mOptions.autoRecenterRotationalThreshold);
186 StringAppendF(&ss, "%s screenStillnessWindowDuration: %0.4f ms\n", prefixSpace.c_str(),
187 media::nsToFloatMs(mOptions.screenStillnessWindowDuration));
188 StringAppendF(&ss, "%s screenStillnessTranslationalThreshold: %f meter\n",
189 prefixSpace.c_str(), mOptions.screenStillnessTranslationalThreshold);
190 StringAppendF(&ss, "%s screenStillnessRotationalThreshold: %f radians\n",
191 prefixSpace.c_str(), mOptions.screenStillnessRotationalThreshold);
192 ss += mModeSelector.toString(level + 1);
193 ss += mRateLimiter.toString(level + 1);
194 ss += mPosePredictor.toString(level + 1);
195 ss.append(prefixSpace + "ReCenterHistory:\n");
196 ss += mLocalLog.dumpToString((prefixSpace + " ").c_str(), mMaxLocalLogLine);
197 return ss;
198 }
199
200 private:
201 const Options mOptions;
202 float mPhysicalToLogicalAngle = 0;
203 // We store the physical to logical angle as "pending" until the next world-to-screen sample it
204 // applies to arrives.
205 float mPendingPhysicalToLogicalAngle = 0;
206 std::optional<int64_t> mWorldToHeadTimestamp;
207 std::optional<int64_t> mWorldToScreenTimestamp;
208 Pose3f mHeadToStagePose;
209 PoseBias mHeadPoseBias;
210 PoseBias mScreenPoseBias;
211 StillnessDetector mHeadStillnessDetector;
212 StillnessDetector mScreenStillnessDetector;
213 ScreenHeadFusion mScreenHeadFusion;
214 ModeSelector mModeSelector;
215 PoseRateLimiter mRateLimiter;
216 PosePredictor mPosePredictor;
217 static constexpr std::size_t mMaxLocalLogLine = 10;
218 SimpleLog mLocalLog{mMaxLocalLogLine};
219 };
220
221 } // namespace
222
createHeadTrackingProcessor(const HeadTrackingProcessor::Options & options,HeadTrackingMode initialMode)223 std::unique_ptr<HeadTrackingProcessor> createHeadTrackingProcessor(
224 const HeadTrackingProcessor::Options& options, HeadTrackingMode initialMode) {
225 return std::make_unique<HeadTrackingProcessorImpl>(options, initialMode);
226 }
227
toString(HeadTrackingMode mode)228 std::string toString(HeadTrackingMode mode) {
229 switch (mode) {
230 case HeadTrackingMode::STATIC:
231 return "STATIC";
232 case HeadTrackingMode::WORLD_RELATIVE:
233 return "WORLD_RELATIVE";
234 case HeadTrackingMode::SCREEN_RELATIVE:
235 return "SCREEN_RELATIVE";
236 }
237 return "EnumNotImplemented";
238 };
239
toString(PosePredictorType posePredictorType)240 std::string toString(PosePredictorType posePredictorType) {
241 switch (posePredictorType) {
242 case PosePredictorType::AUTO: return "AUTO";
243 case PosePredictorType::LAST: return "LAST";
244 case PosePredictorType::TWIST: return "TWIST";
245 case PosePredictorType::LEAST_SQUARES: return "LEAST_SQUARES";
246 }
247 return "UNKNOWN" + std::to_string((int)posePredictorType);
248 }
249
isValidPosePredictorType(PosePredictorType posePredictorType)250 bool isValidPosePredictorType(PosePredictorType posePredictorType) {
251 switch (posePredictorType) {
252 case PosePredictorType::AUTO:
253 case PosePredictorType::LAST:
254 case PosePredictorType::TWIST:
255 case PosePredictorType::LEAST_SQUARES:
256 return true;
257 }
258 return false;
259 }
260
261 } // namespace media
262 } // namespace android
263