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 #include "media/QuaternionUtil.h"
19
20 #include <gtest/gtest.h>
21
22 #include "TestUtil.h"
23
24 namespace android {
25 namespace media {
26 namespace {
27
28 using Eigen::Quaternionf;
29 using Eigen::Vector3f;
30 using Options = HeadTrackingProcessor::Options;
31
TEST(HeadTrackingProcessor,Initial)32 TEST(HeadTrackingProcessor, Initial) {
33 for (auto mode : {HeadTrackingMode::STATIC, HeadTrackingMode::WORLD_RELATIVE,
34 HeadTrackingMode::SCREEN_RELATIVE}) {
35 std::unique_ptr<HeadTrackingProcessor> processor =
36 createHeadTrackingProcessor(Options{}, mode);
37 processor->calculate(0);
38 EXPECT_EQ(processor->getActualMode(), HeadTrackingMode::STATIC);
39 EXPECT_EQ(processor->getHeadToStagePose(), Pose3f());
40 }
41 }
42
TEST(HeadTrackingProcessor,BasicComposition)43 TEST(HeadTrackingProcessor, BasicComposition) {
44 const Pose3f worldToHead{{1, 2, 3}, Quaternionf::UnitRandom()};
45 const Pose3f worldToScreen{{4, 5, 6}, Quaternionf::UnitRandom()};
46 const Pose3f screenToStage{{7, 8, 9}, Quaternionf::UnitRandom()};
47 const float physicalToLogical = M_PI_2;
48
49 std::unique_ptr<HeadTrackingProcessor> processor =
50 createHeadTrackingProcessor(Options{}, HeadTrackingMode::SCREEN_RELATIVE);
51
52 // Establish a baseline for the drift compensators.
53 processor->setWorldToHeadPose(0, Pose3f(), Twist3f());
54 processor->setWorldToScreenPose(0, Pose3f());
55
56 processor->setDisplayOrientation(physicalToLogical);
57 processor->setWorldToHeadPose(0, worldToHead, Twist3f());
58 processor->setWorldToScreenPose(0, worldToScreen);
59 processor->setScreenToStagePose(screenToStage);
60 processor->calculate(0);
61 ASSERT_EQ(processor->getActualMode(), HeadTrackingMode::SCREEN_RELATIVE);
62 EXPECT_EQ(processor->getHeadToStagePose(), worldToHead.inverse() * worldToScreen *
63 Pose3f(rotateY(-physicalToLogical)) *
64 screenToStage);
65
66 processor->setDesiredMode(HeadTrackingMode::WORLD_RELATIVE);
67 processor->calculate(0);
68 ASSERT_EQ(processor->getActualMode(), HeadTrackingMode::WORLD_RELATIVE);
69 EXPECT_EQ(processor->getHeadToStagePose(), worldToHead.inverse() * screenToStage);
70
71 processor->setDesiredMode(HeadTrackingMode::STATIC);
72 processor->calculate(0);
73 ASSERT_EQ(processor->getActualMode(), HeadTrackingMode::STATIC);
74 EXPECT_EQ(processor->getHeadToStagePose(), screenToStage);
75 }
76
TEST(HeadTrackingProcessor,Prediction)77 TEST(HeadTrackingProcessor, Prediction) {
78 const Pose3f worldToHead{{1, 2, 3}, Quaternionf::UnitRandom()};
79 const Twist3f headTwist{{4, 5, 6}, quaternionToRotationVector(Quaternionf::UnitRandom()) / 10};
80 const Pose3f worldToScreen{{4, 5, 6}, Quaternionf::UnitRandom()};
81
82 std::unique_ptr<HeadTrackingProcessor> processor = createHeadTrackingProcessor(
83 Options{.predictionDuration = 2.f}, HeadTrackingMode::WORLD_RELATIVE);
84
85 processor->setPosePredictorType(PosePredictorType::TWIST);
86
87 // Establish a baseline for the drift compensators.
88 processor->setWorldToHeadPose(0, Pose3f(), Twist3f());
89 processor->setWorldToScreenPose(0, Pose3f());
90
91 processor->setWorldToHeadPose(0, worldToHead, headTwist);
92 processor->setWorldToScreenPose(0, worldToScreen);
93 processor->calculate(0);
94 ASSERT_EQ(processor->getActualMode(), HeadTrackingMode::WORLD_RELATIVE);
95 EXPECT_EQ(processor->getHeadToStagePose(), (worldToHead * integrate(headTwist, 2.f)).inverse());
96
97 processor->setDesiredMode(HeadTrackingMode::SCREEN_RELATIVE);
98 processor->calculate(0);
99 ASSERT_EQ(processor->getActualMode(), HeadTrackingMode::SCREEN_RELATIVE);
100 EXPECT_EQ(processor->getHeadToStagePose(),
101 (worldToHead * integrate(headTwist, 2.f)).inverse() * worldToScreen);
102
103 processor->setDesiredMode(HeadTrackingMode::STATIC);
104 processor->calculate(0);
105 ASSERT_EQ(processor->getActualMode(), HeadTrackingMode::STATIC);
106 EXPECT_EQ(processor->getHeadToStagePose(), Pose3f());
107 }
108
TEST(HeadTrackingProcessor,SmoothModeSwitch)109 TEST(HeadTrackingProcessor, SmoothModeSwitch) {
110 const Pose3f targetHeadToWorld = Pose3f({4, 0, 0}, rotateZ(M_PI / 2));
111
112 std::unique_ptr<HeadTrackingProcessor> processor = createHeadTrackingProcessor(
113 Options{.maxTranslationalVelocity = 1}, HeadTrackingMode::STATIC);
114
115 // Establish a baseline for the drift compensators.
116 processor->setWorldToHeadPose(0, Pose3f(), Twist3f());
117 processor->setWorldToScreenPose(0, Pose3f());
118
119 processor->calculate(0);
120
121 processor->setDesiredMode(HeadTrackingMode::WORLD_RELATIVE);
122 processor->setWorldToHeadPose(0, targetHeadToWorld.inverse(), Twist3f());
123
124 // We're expecting a gradual move to the target.
125 processor->calculate(0);
126 EXPECT_EQ(HeadTrackingMode::WORLD_RELATIVE, processor->getActualMode());
127 EXPECT_EQ(processor->getHeadToStagePose(), Pose3f());
128
129 processor->calculate(2);
130 EXPECT_EQ(HeadTrackingMode::WORLD_RELATIVE, processor->getActualMode());
131 EXPECT_EQ(processor->getHeadToStagePose(), Pose3f({2, 0, 0}, rotateZ(M_PI / 4)));
132
133 processor->calculate(4);
134 EXPECT_EQ(HeadTrackingMode::WORLD_RELATIVE, processor->getActualMode());
135 EXPECT_EQ(processor->getHeadToStagePose(), targetHeadToWorld);
136
137 // Now that we've reached the target, we should no longer be rate limiting.
138 processor->setWorldToHeadPose(4, Pose3f(), Twist3f());
139 processor->calculate(5);
140 EXPECT_EQ(HeadTrackingMode::WORLD_RELATIVE, processor->getActualMode());
141 EXPECT_EQ(processor->getHeadToStagePose(), Pose3f());
142 }
143
144 } // namespace
145 } // namespace media
146 } // namespace android
147