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 <gtest/gtest.h>
18 #include <cmath>
19
20 #include "PoseDriftCompensator.h"
21
22 #include "media/QuaternionUtil.h"
23 #include "TestUtil.h"
24
25 namespace android {
26 namespace media {
27 namespace {
28
29 using Eigen::Quaternionf;
30 using Eigen::Vector3f;
31 using Options = PoseDriftCompensator::Options;
32
TEST(PoseDriftCompensator,Initial)33 TEST(PoseDriftCompensator, Initial) {
34 PoseDriftCompensator comp(Options{});
35 EXPECT_EQ(comp.getOutput(), Pose3f());
36 }
37
TEST(PoseDriftCompensator,NoDrift)38 TEST(PoseDriftCompensator, NoDrift) {
39 Pose3f pose1({1, 2, 3}, Quaternionf::UnitRandom());
40 Pose3f pose2({4, 5, 6}, Quaternionf::UnitRandom());
41 PoseDriftCompensator comp(Options{});
42
43 // First pose sets the baseline.
44 comp.setInput(1000, pose1);
45 EXPECT_EQ(comp.getOutput(), Pose3f());
46
47 comp.setInput(2000, pose2);
48 EXPECT_EQ(comp.getOutput(), pose1.inverse() * pose2);
49
50 // Recentering resets the baseline.
51 comp.recenter();
52 EXPECT_EQ(comp.getOutput(), Pose3f());
53
54 comp.setInput(3000, pose1);
55 EXPECT_EQ(comp.getOutput(), Pose3f());
56
57 comp.setInput(4000, pose2);
58 EXPECT_EQ(comp.getOutput(), pose1.inverse() * pose2);
59 }
60
TEST(PoseDriftCompensator,NoDriftZeroTime)61 TEST(PoseDriftCompensator, NoDriftZeroTime) {
62 Pose3f pose1({1, 2, 3}, Quaternionf::UnitRandom());
63 Pose3f pose2({4, 5, 6}, Quaternionf::UnitRandom());
64 PoseDriftCompensator comp(Options{});
65
66 comp.setInput(1000, pose1);
67 EXPECT_EQ(comp.getOutput(), Pose3f());
68
69 comp.setInput(1000, pose2);
70 EXPECT_EQ(comp.getOutput(), pose1.inverse() * pose2);
71
72 comp.recenter();
73 EXPECT_EQ(comp.getOutput(), Pose3f());
74
75 comp.setInput(1000, pose1);
76 EXPECT_EQ(comp.getOutput(), Pose3f());
77
78 comp.setInput(1000, pose2);
79 EXPECT_EQ(comp.getOutput(), pose1.inverse() * pose2);
80 }
81
TEST(PoseDriftCompensator,Asymptotic)82 TEST(PoseDriftCompensator, Asymptotic) {
83 Pose3f pose({1, 2, 3}, Quaternionf::UnitRandom());
84
85 PoseDriftCompensator comp(
86 Options{.translationalDriftTimeConstant = 1, .rotationalDriftTimeConstant = 1});
87
88 // Set the same pose for a long time.
89 for (int64_t t = 0; t < 1000; ++t) {
90 comp.setInput(t, pose);
91 }
92
93 // Output would have faded to approx. identity.
94 EXPECT_EQ(comp.getOutput(), Pose3f());
95 }
96
TEST(PoseDriftCompensator,Fast)97 TEST(PoseDriftCompensator, Fast) {
98 Pose3f pose1({1, 2, 3}, Quaternionf::UnitRandom());
99 Pose3f pose2({4, 5, 6}, Quaternionf::UnitRandom());
100 PoseDriftCompensator comp(
101 Options{.translationalDriftTimeConstant = 1e7, .rotationalDriftTimeConstant = 1e7});
102
103 comp.setInput(0, pose1);
104 EXPECT_EQ(comp.getOutput(), Pose3f());
105
106 comp.setInput(1, pose2);
107 EXPECT_EQ(comp.getOutput(), pose1.inverse() * pose2);
108
109 comp.recenter();
110 EXPECT_EQ(comp.getOutput(), Pose3f());
111
112 comp.setInput(2, pose1);
113 EXPECT_EQ(comp.getOutput(), Pose3f());
114
115 comp.setInput(3, pose2);
116 EXPECT_EQ(comp.getOutput(), pose1.inverse() * pose2);
117 }
118
TEST(PoseDriftCompensator,Drift)119 TEST(PoseDriftCompensator, Drift) {
120 Pose3f pose1({1, 2, 3}, rotateZ(-M_PI * 3 / 4));
121 PoseDriftCompensator comp(
122 Options{.translationalDriftTimeConstant = 500, .rotationalDriftTimeConstant = 1000});
123
124 // Establish a baseline.
125 comp.setInput(1000, Pose3f());
126
127 // Initial pose is used as is.
128 comp.setInput(1000, pose1);
129 EXPECT_EQ(comp.getOutput(), pose1);
130
131 // After 1000 ticks, our rotation should be exp(-1) and translation exp(-2) from identity.
132 comp.setInput(2000, pose1);
133 EXPECT_EQ(comp.getOutput(),
134 Pose3f(Vector3f{1, 2, 3} * std::expf(-2), rotateZ(-M_PI * 3 / 4 * std::expf(-1))));
135
136 // As long as the input stays the same, we'll continue to advance towards identity.
137 comp.setInput(3000, pose1);
138 EXPECT_EQ(comp.getOutput(),
139 Pose3f(Vector3f{1, 2, 3} * std::expf(-4), rotateZ(-M_PI * 3 / 4 * std::expf(-2))));
140
141 comp.recenter();
142 EXPECT_EQ(comp.getOutput(), Pose3f());
143 }
144
145 } // namespace
146 } // namespace media
147 } // namespace android
148